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; }
148 sotDummyType& calibrationTriger(sotDummyType& dummy,
int time);
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