force-compensation.h 7.2 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
2
3
4
/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
Francois Bleibel's avatar
Francois Bleibel committed
5
 *
Thomas Moulard's avatar
Thomas Moulard committed
6
 * CNRS/AIST
Francois Bleibel's avatar
Francois Bleibel committed
7
 *
8
9
 * This file is part of sot-dynamic-pinocchio.
 * sot-dynamic-pinocchio is free software: you can redistribute it and/or
Thomas Moulard's avatar
Thomas Moulard committed
10
11
12
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
13
 * sot-dynamic-pinocchio is distributed in the hope that it will be
Thomas Moulard's avatar
Thomas Moulard committed
14
15
16
17
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
18
 * with sot-dynamic-pinocchio.  If not, see <http://www.gnu.org/licenses/>.
Thomas Moulard's avatar
Thomas Moulard committed
19
 */
Francois Bleibel's avatar
Francois Bleibel committed
20
21
22
23
24
25
26
27
28

#ifndef __SOT_SOTFORCECOMPENSATION_H__
#define __SOT_SOTFORCECOMPENSATION_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
29
30
#include <dynamic-graph/linear-algebra.h>

Francois Bleibel's avatar
Francois Bleibel committed
31
32
33
34
35

/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
36
37

#include <sot/core/matrix-geometry.hh>
Francois Bleibel's avatar
Francois Bleibel committed
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

/* STD */
#include <string>

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined (WIN32) 
#  if defined (force_compensation_EXPORTS)
#    define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
#  else  
#    define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
#  endif 
#else
#  define SOTFORCECOMPENSATION_EXPORT
#endif


57
58
namespace dynamicgraph { namespace sot {
    namespace dg = dynamicgraph;
Francois Bleibel's avatar
Francois Bleibel committed
59

60
61
62
    /* --------------------------------------------------------------------- */
    /* --- CLASS ----------------------------------------------------------- */
    /* --------------------------------------------------------------------- */
Francois Bleibel's avatar
Francois Bleibel committed
63

64
65
66
67
68
69
    class SOTFORCECOMPENSATION_EXPORT ForceCompensation
    {
    private:
      static MatrixRotation I3;
    protected:
      bool usingPrecompensation;
Francois Bleibel's avatar
Francois Bleibel committed
70

71
72
73
74
    public:
      ForceCompensation( void );
      static MatrixForce& computeHandXworld( 
					    const MatrixRotation & worldRhand,
75
					    const dynamicgraph::Vector & transSensorCom,
76
					    MatrixForce& res );
Francois Bleibel's avatar
Francois Bleibel committed
77
78

  
79
80
81
      static MatrixForce& computeHandVsensor( const MatrixRotation & sensorRhand,
					      MatrixForce& res );
      static MatrixForce& computeSensorXhand( const MatrixRotation & sensorRhand,
82
					      const dynamicgraph::Vector & transSensorCom,
83
					      MatrixForce& res );
84
      /*   static dynamicgraph::Matrix& computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint, */
85
      /* 					   const MatrixForce& sensorXhand, */
86
      /* 					   dynamicgraph::Matrix& res ); */
87

88
89
90
      static dynamicgraph::Vector& computeTorsorCompensated( const dynamicgraph::Vector& torqueInput,
						   const dynamicgraph::Vector& torquePrecompensation,
						   const dynamicgraph::Vector& gravity,
91
92
						   const MatrixForce& handXworld,
						   const MatrixForce& handVsensor,
93
94
95
96
97
98
99
100
101
						   const dynamicgraph::Matrix& gainSensor,
						   const dynamicgraph::Vector& momentum,
						   dynamicgraph::Vector& res );

      static dynamicgraph::Vector& crossProduct_V_F( const dynamicgraph::Vector& velocity,
					   const dynamicgraph::Vector& force,
					   dynamicgraph::Vector& res );
      static dynamicgraph::Vector& computeMomentum( const dynamicgraph::Vector& velocity,
					  const dynamicgraph::Vector& acceleration,
102
					  const MatrixForce& sensorXhand,
103
104
					  const dynamicgraph::Matrix& inertiaJoint,
					  dynamicgraph::Vector& res );
105

106
107
108
      static dynamicgraph::Vector& computeDeadZone( const dynamicgraph::Vector& torqueInput,
					  const dynamicgraph::Vector& deadZoneLimit,
					  dynamicgraph::Vector& res );
Francois Bleibel's avatar
Francois Bleibel committed
109
  
110
    public: // CALIBRATION
Francois Bleibel's avatar
Francois Bleibel committed
111

112
      std::list<dynamicgraph::Vector> torsorList;
113
      std::list<MatrixRotation> rotationList;
Francois Bleibel's avatar
Francois Bleibel committed
114

115
      void clearCalibration( void );
116
      void addCalibrationValue( const dynamicgraph::Vector& torsor,
117
				const MatrixRotation & worldRhand );
Francois Bleibel's avatar
Francois Bleibel committed
118
  
119
      dynamicgraph::Vector calibrateTransSensorCom( const dynamicgraph::Vector& gravity,
120
					  const MatrixRotation& handRsensor );
121
      dynamicgraph::Vector calibrateGravity( const MatrixRotation& handRsensor,
122
123
				   bool precompensationCalibration = false,
				   const MatrixRotation& hand0Rsensor = I3 );
Francois Bleibel's avatar
Francois Bleibel committed
124
125
126

    
  
127
    };
Francois Bleibel's avatar
Francois Bleibel committed
128

129
130
131
    /* --------------------------------------------------------------------- */
    /* --- PLUGIN ---------------------------------------------------------- */
    /* --------------------------------------------------------------------- */
Francois Bleibel's avatar
Francois Bleibel committed
132

133
134
135
136
137
    class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
      :public dg::Entity, public ForceCompensation
    {
    public:
      static const std::string CLASS_NAME;
138
      virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
139
      bool calibrationStarted;
Francois Bleibel's avatar
Francois Bleibel committed
140
141


142
    public: /* --- CONSTRUCTION --- */
Francois Bleibel's avatar
Francois Bleibel committed
143

144
145
      ForceCompensationPlugin( const std::string& name );
      virtual ~ForceCompensationPlugin( void );
Francois Bleibel's avatar
Francois Bleibel committed
146

147
    public: /* --- SIGNAL --- */
Francois Bleibel's avatar
Francois Bleibel committed
148

149
      /* --- INPUTS --- */
150
      dg::SignalPtr<dynamicgraph::Vector,int> torsorSIN; 
151
      dg::SignalPtr<MatrixRotation,int> worldRhandSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
152

153
154
      /* --- CONSTANTS --- */
      dg::SignalPtr<MatrixRotation,int> handRsensorSIN; 
155
156
157
158
159
160
161
      dg::SignalPtr<dynamicgraph::Vector,int> translationSensorComSIN; 
      dg::SignalPtr<dynamicgraph::Vector,int> gravitySIN; 
      dg::SignalPtr<dynamicgraph::Vector,int> precompensationSIN; 
      dg::SignalPtr<dynamicgraph::Matrix,int> gainSensorSIN; 
      dg::SignalPtr<dynamicgraph::Vector,int> deadZoneLimitSIN; 
      dg::SignalPtr<dynamicgraph::Vector,int> transSensorJointSIN; 
      dg::SignalPtr<dynamicgraph::Matrix,int> inertiaJointSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
162

163
164
      dg::SignalPtr<dynamicgraph::Vector,int> velocitySIN; 
      dg::SignalPtr<dynamicgraph::Vector,int> accelerationSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
165

166
167
168
      /* --- INTERMEDIATE OUTPUTS --- */
      dg::SignalTimeDependent<MatrixForce,int> handXworldSOUT; 
      dg::SignalTimeDependent<MatrixForce,int> handVsensorSOUT; 
169
      dg::SignalPtr<dynamicgraph::Vector,int> torsorDeadZoneSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
170

171
      dg::SignalTimeDependent<MatrixForce,int> sensorXhandSOUT;
172
173
174
      //dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
      dg::SignalTimeDependent<dynamicgraph::Vector,int> momentumSOUT; 
      dg::SignalPtr<dynamicgraph::Vector,int> momentumSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
175

176
      /* --- OUTPUTS --- */
177
178
      dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorCompensatedSOUT; 
      dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorDeadZoneSOUT;
Francois Bleibel's avatar
Francois Bleibel committed
179

180
181
      typedef int sotDummyType;
      dg::SignalTimeDependent<sotDummyType,int> calibrationTrigerSOUT; 
Francois Bleibel's avatar
Francois Bleibel committed
182

183
    public: /* --- COMMANDLINE --- */
Francois Bleibel's avatar
Francois Bleibel committed
184

185
      sotDummyType& calibrationTriger( sotDummyType& dummy,int time );
Francois Bleibel's avatar
Francois Bleibel committed
186
187


188
189
190
      virtual void commandLine( const std::string& cmdLine,
				std::istringstream& cmdArgs,
				std::ostream& os );
Francois Bleibel's avatar
Francois Bleibel committed
191
192
193



194
    };
Francois Bleibel's avatar
Francois Bleibel committed
195
196


197
198
  } // namaspace sot
} // namespace dynamicgraph
Francois Bleibel's avatar
Francois Bleibel committed
199
200

#endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__