Loading...
Searching...
No Matches
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
44namespace dynamicgraph {
45namespace sot {
46namespace dg = dynamicgraph;
47
48/* --------------------------------------------------------------------- */
49/* --- CLASS ----------------------------------------------------------- */
50/* --------------------------------------------------------------------- */
51
53 private:
54 static MatrixRotation I3;
55
56 protected:
58
59 public:
61 static MatrixForce& computeHandXworld(
62 const MatrixRotation& worldRhand,
63 const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
64
65 static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand,
66 MatrixForce& res);
67 static MatrixForce& computeSensorXhand(
68 const MatrixRotation& sensorRhand,
69 const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
70 /* static dynamicgraph::Matrix& computeInertiaSensor( const
71 * dynamicgraph::Matrix& inertiaJoint, */
72 /* const MatrixForce& sensorXhand, */
73 /* dynamicgraph::Matrix& res ); */
74
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);
81
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);
89
90 static dynamicgraph::Vector& computeDeadZone(
91 const dynamicgraph::Vector& torqueInput,
92 const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
93
94 public: // CALIBRATION
95 std::list<dynamicgraph::Vector> torsorList;
96 std::list<MatrixRotation> rotationList;
97
98 void clearCalibration(void);
99 void addCalibrationValue(const dynamicgraph::Vector& torsor,
100 const MatrixRotation& worldRhand);
101
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);
108};
109
110/* --------------------------------------------------------------------- */
111/* --- PLUGIN ---------------------------------------------------------- */
112/* --------------------------------------------------------------------- */
113
115 : public dg::Entity,
116 public ForceCompensation {
117 public:
118 static const std::string CLASS_NAME;
119 virtual const std::string& getClassName(void) const { return CLASS_NAME; }
121
122 public: /* --- CONSTRUCTION --- */
123 ForceCompensationPlugin(const std::string& name);
125
126 public: /* --- SIGNAL --- */
127 /* --- INPUTS --- */
128 dg::SignalPtr<dynamicgraph::Vector, int> torsorSIN;
129 dg::SignalPtr<MatrixRotation, int> worldRhandSIN;
130
131 /* --- CONSTANTS --- */
132 dg::SignalPtr<MatrixRotation, int> handRsensorSIN;
133 dg::SignalPtr<dynamicgraph::Vector, int> translationSensorComSIN;
134 dg::SignalPtr<dynamicgraph::Vector, int> gravitySIN;
135 dg::SignalPtr<dynamicgraph::Vector, int> precompensationSIN;
136 dg::SignalPtr<dynamicgraph::Matrix, int> gainSensorSIN;
137 dg::SignalPtr<dynamicgraph::Vector, int> deadZoneLimitSIN;
138 dg::SignalPtr<dynamicgraph::Vector, int> transSensorJointSIN;
139 dg::SignalPtr<dynamicgraph::Matrix, int> inertiaJointSIN;
140
141 dg::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
142 dg::SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
143
144 /* --- INTERMEDIATE OUTPUTS --- */
145 dg::SignalTimeDependent<MatrixForce, int> handXworldSOUT;
146 dg::SignalTimeDependent<MatrixForce, int> handVsensorSOUT;
147 dg::SignalPtr<dynamicgraph::Vector, int> torsorDeadZoneSIN;
148
149 dg::SignalTimeDependent<MatrixForce, int> sensorXhandSOUT;
150 // dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
151 dg::SignalTimeDependent<dynamicgraph::Vector, int> momentumSOUT;
152 dg::SignalPtr<dynamicgraph::Vector, int> momentumSIN;
153
154 /* --- OUTPUTS --- */
155 dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorCompensatedSOUT;
156 dg::SignalTimeDependent<dynamicgraph::Vector, int> torsorDeadZoneSOUT;
157
158 typedef int sotDummyType;
159 dg::SignalTimeDependent<sotDummyType, int> calibrationTrigerSOUT;
160
161 public: /* --- COMMANDLINE --- */
163};
164
165} // namespace sot
166} // namespace dynamicgraph
167
168#endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__
Definition: force-compensation.h:116
dg::SignalPtr< dynamicgraph::Matrix, int > inertiaJointSIN
Definition: force-compensation.h:139
dg::SignalPtr< dynamicgraph::Vector, int > deadZoneLimitSIN
Definition: force-compensation.h:137
virtual const std::string & getClassName(void) const
Definition: force-compensation.h:119
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorDeadZoneSOUT
Definition: force-compensation.h:156
dg::SignalPtr< dynamicgraph::Vector, int > accelerationSIN
Definition: force-compensation.h:142
ForceCompensationPlugin(const std::string &name)
dg::SignalPtr< dynamicgraph::Vector, int > transSensorJointSIN
Definition: force-compensation.h:138
dg::SignalPtr< dynamicgraph::Matrix, int > gainSensorSIN
Definition: force-compensation.h:136
static const std::string CLASS_NAME
Definition: force-compensation.h:118
dg::SignalPtr< dynamicgraph::Vector, int > translationSensorComSIN
Definition: force-compensation.h:133
dg::SignalPtr< MatrixRotation, int > handRsensorSIN
Definition: force-compensation.h:132
sotDummyType & calibrationTriger(sotDummyType &dummy, int time)
dg::SignalTimeDependent< MatrixForce, int > handVsensorSOUT
Definition: force-compensation.h:146
dg::SignalPtr< dynamicgraph::Vector, int > gravitySIN
Definition: force-compensation.h:134
dg::SignalTimeDependent< MatrixForce, int > sensorXhandSOUT
Definition: force-compensation.h:149
int sotDummyType
Definition: force-compensation.h:158
bool calibrationStarted
Definition: force-compensation.h:120
dg::SignalPtr< dynamicgraph::Vector, int > velocitySIN
Definition: force-compensation.h:141
dg::SignalTimeDependent< sotDummyType, int > calibrationTrigerSOUT
Definition: force-compensation.h:159
dg::SignalTimeDependent< MatrixForce, int > handXworldSOUT
Definition: force-compensation.h:145
dg::SignalPtr< dynamicgraph::Vector, int > torsorDeadZoneSIN
Definition: force-compensation.h:147
dg::SignalTimeDependent< dynamicgraph::Vector, int > momentumSOUT
Definition: force-compensation.h:151
dg::SignalPtr< dynamicgraph::Vector, int > torsorSIN
Definition: force-compensation.h:128
dg::SignalPtr< dynamicgraph::Vector, int > precompensationSIN
Definition: force-compensation.h:135
dg::SignalPtr< dynamicgraph::Vector, int > momentumSIN
Definition: force-compensation.h:152
dg::SignalPtr< MatrixRotation, int > worldRhandSIN
Definition: force-compensation.h:129
dg::SignalTimeDependent< dynamicgraph::Vector, int > torsorCompensatedSOUT
Definition: force-compensation.h:155
Definition: force-compensation.h:52
static MatrixForce & computeHandVsensor(const MatrixRotation &sensorRhand, MatrixForce &res)
dynamicgraph::Vector calibrateTransSensorCom(const dynamicgraph::Vector &gravity, const MatrixRotation &handRsensor)
void addCalibrationValue(const dynamicgraph::Vector &torsor, const MatrixRotation &worldRhand)
static MatrixForce & computeHandXworld(const MatrixRotation &worldRhand, const dynamicgraph::Vector &transSensorCom, MatrixForce &res)
bool usingPrecompensation
Definition: force-compensation.h:57
static dynamicgraph::Vector & crossProduct_V_F(const dynamicgraph::Vector &velocity, const dynamicgraph::Vector &force, dynamicgraph::Vector &res)
static dynamicgraph::Vector & computeDeadZone(const dynamicgraph::Vector &torqueInput, const dynamicgraph::Vector &deadZoneLimit, dynamicgraph::Vector &res)
std::list< MatrixRotation > rotationList
Definition: force-compensation.h:96
static dynamicgraph::Vector & computeTorsorCompensated(const dynamicgraph::Vector &torqueInput, const dynamicgraph::Vector &torquePrecompensation, const dynamicgraph::Vector &gravity, const MatrixForce &handXworld, const MatrixForce &handVsensor, const dynamicgraph::Matrix &gainSensor, const dynamicgraph::Vector &momentum, dynamicgraph::Vector &res)
static MatrixForce & computeSensorXhand(const MatrixRotation &sensorRhand, const dynamicgraph::Vector &transSensorCom, MatrixForce &res)
static dynamicgraph::Vector & computeMomentum(const dynamicgraph::Vector &velocity, const dynamicgraph::Vector &acceleration, const MatrixForce &sensorXhand, const dynamicgraph::Matrix &inertiaJoint, dynamicgraph::Vector &res)
std::list< dynamicgraph::Vector > torsorList
Definition: force-compensation.h:95
dynamicgraph::Vector calibrateGravity(const MatrixRotation &handRsensor, bool precompensationCalibration=false, const MatrixRotation &hand0Rsensor=I3)
#define SOTFORCECOMPENSATION_EXPORT
Definition: force-compensation.h:41
Definition: angle-estimator.h:43