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; }
168 #endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__