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(
const MatrixRotation& worldRhand,
const dynamicgraph::Vector& transSensorCom,
64 static MatrixForce& computeHandVsensor(
const MatrixRotation& sensorRhand, MatrixForce& res);
65 static MatrixForce& computeSensorXhand(
const MatrixRotation& sensorRhand,
const dynamicgraph::Vector& transSensorCom,
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);
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);
83 static dynamicgraph::Vector& computeDeadZone(
const dynamicgraph::Vector& torqueInput,
84 const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
90 void clearCalibration(
void);
91 void addCalibrationValue(
const dynamicgraph::Vector& torsor,
const MatrixRotation& worldRhand);
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);
105 virtual const std::string&
getClassName(
void)
const {
return CLASS_NAME; }
154 #endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__