force-compensation.h
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_SOTFORCECOMPENSATION_H__
11 #define __SOT_SOTFORCECOMPENSATION_H__
12 
13 /* --------------------------------------------------------------------- */
14 /* --- INCLUDE --------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 /* Matrix */
18 #include <dynamic-graph/linear-algebra.h>
19 
20 /* SOT */
21 #include <dynamic-graph/entity.h>
22 #include <dynamic-graph/signal-ptr.h>
23 #include <dynamic-graph/signal-time-dependent.h>
24 
25 #include <sot/core/matrix-geometry.hh>
26 
27 /* STD */
28 #include <string>
29 
30 /* --------------------------------------------------------------------- */
31 /* --- API ------------------------------------------------------------- */
32 /* --------------------------------------------------------------------- */
33 
34 #if defined(WIN32)
35 #if defined(force_compensation_EXPORTS)
36 #define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
37 #else
38 #define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
39 #endif
40 #else
41 #define SOTFORCECOMPENSATION_EXPORT
42 #endif
43 
44 namespace dynamicgraph {
45 namespace sot {
46 namespace dg = dynamicgraph;
47 
48 /* --------------------------------------------------------------------- */
49 /* --- CLASS ----------------------------------------------------------- */
50 /* --------------------------------------------------------------------- */
51 
53  private:
54  static MatrixRotation I3;
55 
56  protected:
58 
59  public:
60  ForceCompensation(void);
61  static MatrixForce& computeHandXworld(const MatrixRotation& worldRhand, const dynamicgraph::Vector& transSensorCom,
62  MatrixForce& res);
63 
64  static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand, MatrixForce& res);
65  static MatrixForce& computeSensorXhand(const MatrixRotation& sensorRhand, const dynamicgraph::Vector& transSensorCom,
66  MatrixForce& res);
67  /* static dynamicgraph::Matrix& computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint, */
68  /* const MatrixForce& sensorXhand, */
69  /* dynamicgraph::Matrix& res ); */
70 
71  static dynamicgraph::Vector& computeTorsorCompensated(
72  const dynamicgraph::Vector& torqueInput, const dynamicgraph::Vector& torquePrecompensation,
73  const dynamicgraph::Vector& gravity, const MatrixForce& handXworld, const MatrixForce& handVsensor,
74  const dynamicgraph::Matrix& gainSensor, const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
75 
76  static dynamicgraph::Vector& crossProduct_V_F(const dynamicgraph::Vector& velocity,
77  const dynamicgraph::Vector& force, dynamicgraph::Vector& res);
78  static dynamicgraph::Vector& computeMomentum(const dynamicgraph::Vector& velocity,
79  const dynamicgraph::Vector& acceleration,
80  const MatrixForce& sensorXhand,
81  const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
82 
83  static dynamicgraph::Vector& computeDeadZone(const dynamicgraph::Vector& torqueInput,
84  const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
85 
86  public: // CALIBRATION
87  std::list<dynamicgraph::Vector> torsorList;
88  std::list<MatrixRotation> rotationList;
89 
90  void clearCalibration(void);
91  void addCalibrationValue(const dynamicgraph::Vector& torsor, const MatrixRotation& worldRhand);
92 
93  dynamicgraph::Vector calibrateTransSensorCom(const dynamicgraph::Vector& gravity, const MatrixRotation& handRsensor);
94  dynamicgraph::Vector calibrateGravity(const MatrixRotation& handRsensor, bool precompensationCalibration = false,
95  const MatrixRotation& hand0Rsensor = I3);
96 };
97 
98 /* --------------------------------------------------------------------- */
99 /* --- PLUGIN ---------------------------------------------------------- */
100 /* --------------------------------------------------------------------- */
101 
103  public:
104  static const std::string CLASS_NAME;
105  virtual const std::string& getClassName(void) const { return CLASS_NAME; }
107 
108  public: /* --- CONSTRUCTION --- */
109  ForceCompensationPlugin(const std::string& name);
110  virtual ~ForceCompensationPlugin(void);
111 
112  public: /* --- SIGNAL --- */
113  /* --- INPUTS --- */
114  dg::SignalPtr<dynamicgraph::Vector, int> torsorSIN;
115  dg::SignalPtr<MatrixRotation, int> worldRhandSIN;
116 
117  /* --- CONSTANTS --- */
118  dg::SignalPtr<MatrixRotation, int> handRsensorSIN;
119  dg::SignalPtr<dynamicgraph::Vector, int> translationSensorComSIN;
120  dg::SignalPtr<dynamicgraph::Vector, int> gravitySIN;
121  dg::SignalPtr<dynamicgraph::Vector, int> precompensationSIN;
122  dg::SignalPtr<dynamicgraph::Matrix, int> gainSensorSIN;
123  dg::SignalPtr<dynamicgraph::Vector, int> deadZoneLimitSIN;
124  dg::SignalPtr<dynamicgraph::Vector, int> transSensorJointSIN;
125  dg::SignalPtr<dynamicgraph::Matrix, int> inertiaJointSIN;
126 
127  dg::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
128  dg::SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
129 
130  /* --- INTERMEDIATE OUTPUTS --- */
131  dg::SignalTimeDependent<MatrixForce, int> handXworldSOUT;
132  dg::SignalTimeDependent<MatrixForce, int> handVsensorSOUT;
133  dg::SignalPtr<dynamicgraph::Vector, int> torsorDeadZoneSIN;
134 
135  dg::SignalTimeDependent<MatrixForce, int> sensorXhandSOUT;
136  // dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
137  dg::SignalTimeDependent<dynamicgraph::Vector, int> momentumSOUT;
138  dg::SignalPtr<dynamicgraph::Vector, int> momentumSIN;
139 
140  /* --- OUTPUTS --- */
141  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorCompensatedSOUT;
142  dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorDeadZoneSOUT;
143 
144  typedef int sotDummyType;
145  dg::SignalTimeDependent<sotDummyType, int> calibrationTrigerSOUT;
146 
147  public: /* --- COMMANDLINE --- */
148  sotDummyType& calibrationTriger(sotDummyType& dummy, int time);
149 };
150 
151 } // namespace sot
152 } // namespace dynamicgraph
153 
154 #endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__
bool calibrationStarted
Definition: force-compensation.h:106
dg::SignalPtr< dynamicgraph::Vector, int > transSensorJointSIN
Definition: force-compensation.h:124
int sotDummyType
Definition: force-compensation.h:144
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorCompensatedSOUT
Definition: force-compensation.h:141
dg::SignalTimeDependent< MatrixForce, int > handVsensorSOUT
Definition: force-compensation.h:132
bool usingPrecompensation
Definition: force-compensation.h:57
dg::SignalPtr< dynamicgraph::Vector, int > velocitySIN
Definition: force-compensation.h:127
virtual const std::string & getClassName(void) const
Definition: force-compensation.h:105
dg::SignalPtr< dynamicgraph::Matrix, int > inertiaJointSIN
Definition: force-compensation.h:125
Definition: force-compensation.h:102
dg::SignalPtr< dynamicgraph::Vector, int > momentumSIN
Definition: force-compensation.h:138
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorDeadZoneSOUT
Definition: force-compensation.h:142
std::list< MatrixRotation > rotationList
Definition: force-compensation.h:88
std::list< dynamicgraph::Vector > torsorList
Definition: force-compensation.h:87
dg::SignalPtr< dynamicgraph::Vector, int > accelerationSIN
Definition: force-compensation.h:128
dg::SignalPtr< MatrixRotation, int > worldRhandSIN
Definition: force-compensation.h:115
dg::SignalTimeDependent< dynamicgraph::Vector, int > momentumSOUT
Definition: force-compensation.h:137
dg::SignalTimeDependent< MatrixForce, int > sensorXhandSOUT
Definition: force-compensation.h:135
dg::SignalPtr< MatrixRotation, int > handRsensorSIN
Definition: force-compensation.h:118
dg::SignalPtr< dynamicgraph::Vector, int > precompensationSIN
Definition: force-compensation.h:121
dg::SignalTimeDependent< MatrixForce, int > handXworldSOUT
Definition: force-compensation.h:131
Definition: force-compensation.h:52
dg::SignalPtr< dynamicgraph::Vector, int > deadZoneLimitSIN
Definition: force-compensation.h:123
dg::SignalPtr< dynamicgraph::Vector, int > torsorSIN
Definition: force-compensation.h:114
static const std::string CLASS_NAME
Definition: force-compensation.h:104
#define SOTFORCECOMPENSATION_EXPORT
Definition: force-compensation.h:41
dg::SignalPtr< dynamicgraph::Vector, int > gravitySIN
Definition: force-compensation.h:120
Definition: angle-estimator.h:42
dg::SignalPtr< dynamicgraph::Vector, int > translationSensorComSIN
Definition: force-compensation.h:119
dg::SignalTimeDependent< sotDummyType, int > calibrationTrigerSOUT
Definition: force-compensation.h:145
dg::SignalPtr< dynamicgraph::Vector, int > torsorDeadZoneSIN
Definition: force-compensation.h:133
dg::SignalPtr< dynamicgraph::Matrix, int > gainSensorSIN
Definition: force-compensation.h:122