10 #ifndef __SOT_SOTFORCECOMPENSATION_H__ 11 #define __SOT_SOTFORCECOMPENSATION_H__ 18 #include <dynamic-graph/linear-algebra.h> 21 #include <dynamic-graph/entity.h> 22 #include <dynamic-graph/signal-ptr.h> 23 #include <dynamic-graph/signal-time-dependent.h> 25 #include <sot/core/matrix-geometry.hh> 35 #if defined(force_compensation_EXPORTS) 36 #define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport) 38 #define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport) 41 #define SOTFORCECOMPENSATION_EXPORT 54 static MatrixRotation I3;
61 static MatrixForce& computeHandXworld(
62 const MatrixRotation& worldRhand,
63 const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
65 static MatrixForce& computeHandVsensor(
const MatrixRotation& sensorRhand,
67 static MatrixForce& computeSensorXhand(
68 const MatrixRotation& sensorRhand,
69 const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
75 static dynamicgraph::Vector& computeTorsorCompensated(
76 const dynamicgraph::Vector& torqueInput,
77 const dynamicgraph::Vector& torquePrecompensation,
78 const dynamicgraph::Vector& gravity,
const MatrixForce& handXworld,
79 const MatrixForce& handVsensor,
const dynamicgraph::Matrix& gainSensor,
80 const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
82 static dynamicgraph::Vector& crossProduct_V_F(
83 const dynamicgraph::Vector& velocity,
const dynamicgraph::Vector& force,
84 dynamicgraph::Vector& res);
85 static dynamicgraph::Vector& computeMomentum(
86 const dynamicgraph::Vector& velocity,
87 const dynamicgraph::Vector& acceleration,
const MatrixForce& sensorXhand,
88 const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
90 static dynamicgraph::Vector& computeDeadZone(
91 const dynamicgraph::Vector& torqueInput,
92 const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
98 void clearCalibration(
void);
99 void addCalibrationValue(
const dynamicgraph::Vector& torsor,
100 const MatrixRotation& worldRhand);
102 dynamicgraph::Vector calibrateTransSensorCom(
103 const dynamicgraph::Vector& gravity,
const MatrixRotation& handRsensor);
104 dynamicgraph::Vector calibrateGravity(
105 const MatrixRotation& handRsensor,
106 bool precompensationCalibration =
false,
107 const MatrixRotation& hand0Rsensor = I3);
119 virtual const std::string&
getClassName(
void)
const {
return CLASS_NAME; }
162 sotDummyType& calibrationTriger(sotDummyType& dummy,
int time);
168 #endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__ bool calibrationStarted
Definition: force-compensation.h:120
dg::SignalPtr< dynamicgraph::Vector, int > transSensorJointSIN
Definition: force-compensation.h:138
int sotDummyType
Definition: force-compensation.h:158
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorCompensatedSOUT
Definition: force-compensation.h:155
dg::SignalTimeDependent< MatrixForce, int > handVsensorSOUT
Definition: force-compensation.h:146
bool usingPrecompensation
Definition: force-compensation.h:57
dg::SignalPtr< dynamicgraph::Vector, int > velocitySIN
Definition: force-compensation.h:141
virtual const std::string & getClassName(void) const
Definition: force-compensation.h:119
dg::SignalPtr< dynamicgraph::Matrix, int > inertiaJointSIN
Definition: force-compensation.h:139
Definition: force-compensation.h:114
dg::SignalPtr< dynamicgraph::Vector, int > momentumSIN
Definition: force-compensation.h:152
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorDeadZoneSOUT
Definition: force-compensation.h:156
std::list< MatrixRotation > rotationList
Definition: force-compensation.h:96
std::list< dynamicgraph::Vector > torsorList
Definition: force-compensation.h:95
dg::SignalPtr< dynamicgraph::Vector, int > accelerationSIN
Definition: force-compensation.h:142
dg::SignalPtr< MatrixRotation, int > worldRhandSIN
Definition: force-compensation.h:129
dg::SignalTimeDependent< dynamicgraph::Vector, int > momentumSOUT
Definition: force-compensation.h:151
dg::SignalTimeDependent< MatrixForce, int > sensorXhandSOUT
Definition: force-compensation.h:149
dg::SignalPtr< MatrixRotation, int > handRsensorSIN
Definition: force-compensation.h:132
dg::SignalPtr< dynamicgraph::Vector, int > precompensationSIN
Definition: force-compensation.h:135
dg::SignalTimeDependent< MatrixForce, int > handXworldSOUT
Definition: force-compensation.h:145
Definition: force-compensation.h:52
dg::SignalPtr< dynamicgraph::Vector, int > deadZoneLimitSIN
Definition: force-compensation.h:137
dg::SignalPtr< dynamicgraph::Vector, int > torsorSIN
Definition: force-compensation.h:128
static const std::string CLASS_NAME
Definition: force-compensation.h:118
#define SOTFORCECOMPENSATION_EXPORT
Definition: force-compensation.h:41
dg::SignalPtr< dynamicgraph::Vector, int > gravitySIN
Definition: force-compensation.h:134
Definition: angle-estimator.h:43
dg::SignalPtr< dynamicgraph::Vector, int > translationSensorComSIN
Definition: force-compensation.h:133
dg::SignalTimeDependent< sotDummyType, int > calibrationTrigerSOUT
Definition: force-compensation.h:159
dg::SignalPtr< dynamicgraph::Vector, int > torsorDeadZoneSIN
Definition: force-compensation.h:147
dg::SignalPtr< dynamicgraph::Matrix, int > gainSensorSIN
Definition: force-compensation.h:136