force-compensation.h 6.72 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
 *
Thomas Moulard's avatar
Thomas Moulard committed
8
9
10
11
12
13
14
15
16
17
18
19
 * This file is part of sot-dynamic.
 * sot-dynamic is free software: you can redistribute it and/or
 * 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.
 * sot-dynamic is distributed in the hope that it will be
 * 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
 * with sot-dynamic.  If not, see <http://www.gnu.org/licenses/>.
 */
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 */
Thomas Moulard's avatar
Thomas Moulard committed
29
#include <jrl/mal/boost.hh>
Francois Bleibel's avatar
Francois Bleibel committed
30
31
32
33
34
35
namespace ml = maal::boost;

/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
36
#include <sot/core/matrix-rotation.hh>
37
#include <sot/core/matrix-force.hh>
38
#include <sot/core/matrix-homogeneous.hh>
Francois Bleibel's avatar
Francois Bleibel committed
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

/* 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


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

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

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

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

  
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
      static MatrixForce& computeHandVsensor( const MatrixRotation & sensorRhand,
					      MatrixForce& res );
      static MatrixForce& computeSensorXhand( const MatrixRotation & sensorRhand,
					      const ml::Vector & transSensorCom,
					      MatrixForce& res );
      /*   static ml::Matrix& computeInertiaSensor( const ml::Matrix& inertiaJoint, */
      /* 					   const MatrixForce& sensorXhand, */
      /* 					   ml::Matrix& res ); */

      static ml::Vector& computeTorsorCompensated( const ml::Vector& torqueInput,
						   const ml::Vector& torquePrecompensation,
						   const ml::Vector& gravity,
						   const MatrixForce& handXworld,
						   const MatrixForce& handVsensor,
						   const ml::Matrix& gainSensor,
						   const ml::Vector& momentum,
						   ml::Vector& res );

      static ml::Vector& crossProduct_V_F( const ml::Vector& velocity,
					   const ml::Vector& force,
					   ml::Vector& res );
      static ml::Vector& computeMomentum( const ml::Vector& velocity,
					  const ml::Vector& acceleration,
					  const MatrixForce& sensorXhand,
					  const ml::Matrix& inertiaJoint,
					  ml::Vector& res );

      static ml::Vector& computeDeadZone( const ml::Vector& torqueInput,
					  const ml::Vector& deadZoneLimit,
					  ml::Vector& res );
Francois Bleibel's avatar
Francois Bleibel committed
110
  
111
    public: // CALIBRATION
Francois Bleibel's avatar
Francois Bleibel committed
112

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

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

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

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

134
135
136
137
138
139
    class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
      :public dg::Entity, public ForceCompensation
    {
    public:
      static const std::string CLASS_NAME;
      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
150
151
      /* --- INPUTS --- */
      dg::SignalPtr<ml::Vector,int> torsorSIN; 
      dg::SignalPtr<MatrixRotation,int> worldRhandSIN; 
Francois Bleibel's avatar
Francois Bleibel committed
152

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

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

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

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

176
177
178
      /* --- OUTPUTS --- */
      dg::SignalTimeDependent<ml::Vector,int> torsorCompensatedSOUT; 
      dg::SignalTimeDependent<ml::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__