dynamic-pinocchio.h 9.77 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
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#define __SOT_DYNAMIC_PINOCCHIO_H__
Francois Bleibel's avatar
Francois Bleibel committed
23
24
25
26
27

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

28

florent's avatar
florent committed
29
/* STD */
30
#include <Python.h>
florent's avatar
florent committed
31
32
#include <string>
#include <map>
Francois Bleibel's avatar
Francois Bleibel committed
33
/* Matrix */
34
#include <dynamic-graph/linear-algebra.h>
florent's avatar
florent committed
35

36
/*Python API*/
Francois Bleibel's avatar
Francois Bleibel committed
37
38

/* SOT */
39
#include <sot/core/flags.hh>
Francois Bleibel's avatar
Francois Bleibel committed
40
41
42
43
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
44
#include <sot/core/exception-dynamic.hh>
45
#include <sot/core/matrix-geometry.hh>
Francois Bleibel's avatar
Francois Bleibel committed
46

47
/* PINOCCHIO */
48
#include <pinocchio/macros.hpp>
49
50
51
52
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
53
#include <pinocchio/algorithm/frames.hpp>
54

Francois Bleibel's avatar
Francois Bleibel committed
55
56
57
58
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

florent's avatar
florent committed
59
#if defined (WIN32)
Francois Bleibel's avatar
Francois Bleibel committed
60
61
#  if defined (dynamic_EXPORTS)
#    define SOTDYNAMIC_EXPORT __declspec(dllexport)
florent's avatar
florent committed
62
#  else
Francois Bleibel's avatar
Francois Bleibel committed
63
#    define SOTDYNAMIC_EXPORT __declspec(dllimport)
florent's avatar
florent committed
64
#  endif
Francois Bleibel's avatar
Francois Bleibel committed
65
66
67
68
69
#else
#  define SOTDYNAMIC_EXPORT
#endif


70
71
namespace dynamicgraph { 
  namespace sot {
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
    namespace dg = dynamicgraph;
    
    namespace command {
      class SetFile;
      class CreateOpPoint;
    }
    /* --------------------------------------------------------------------- */
    /* --- CLASS ----------------------------------------------------------- */
    /* --------------------------------------------------------------------- */
    
    
    
    /*! @ingroup signals
      \brief This class provides an inverse dynamic model of the robot.
      More precisely it wraps the newton euler algorithm implemented
      by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
      The robot is described by a VRML file.
    */
90
class SOTDYNAMIC_EXPORT DynamicPinocchio
91
92
 :public dg::Entity {
  friend class sot::command::SetFile;
93
  friend class sot::command::CreateOpPoint;
94
95
96
97
  //  friend class sot::command::InitializeRobot;
  
    public: 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98
  DYNAMIC_GRAPH_ENTITY_DECL();
Francois Bleibel's avatar
Francois Bleibel committed
99

100
  /*  --- MODEL ATRIBUTES --- */
101
  se3::Model*  m_model;
102
  se3::Data*   m_data;
florent's avatar
florent committed
103

104
  /*  --- MODEL ATRIBUTES --- */
Francois Bleibel's avatar
Francois Bleibel committed
105

106
107
108
109
110
111
    public:
  /* --- SIGNAL ACTIVATION --- */
  dg::SignalTimeDependent< dg::Matrix,int >&
    createEndeffJacobianSignal( const std::string& signame, const std::string& );
  dg::SignalTimeDependent< dg::Matrix,int >&
    createJacobianSignal( const std::string& signame, const std::string& );
Francois Bleibel's avatar
Francois Bleibel committed
112
  void destroyJacobianSignal( const std::string& signame );
113
  
Francois Bleibel's avatar
Francois Bleibel committed
114
  dg::SignalTimeDependent< MatrixHomogeneous,int >&
115
    createPositionSignal( const std::string&,const std::string& );
Francois Bleibel's avatar
Francois Bleibel committed
116
  void destroyPositionSignal( const std::string& signame );
117
118
119
120
121
122
123
  
  dg::SignalTimeDependent< dg::Vector,int >&
    createVelocitySignal( const std::string&,const std::string& );
  void destroyVelocitySignal( const std::string& signame );
  
  dg::SignalTimeDependent< dg::Vector,int >&
    createAccelerationSignal( const std::string&, const std::string& );
Francois Bleibel's avatar
Francois Bleibel committed
124
125
  void destroyAccelerationSignal( const std::string& signame );

126
127
  /*! @} */
  std::list< dg::SignalBase<int>*  > genericSignalRefs;
Francois Bleibel's avatar
Francois Bleibel committed
128
129
130



131
132
133
134
135
136
137
138
139
140
    public:
  /* --- SIGNAL --- */
  typedef int Dummy;
  dg::SignalPtr<dg::Vector,int> jointPositionSIN;
  dg::SignalPtr<dg::Vector,int> freeFlyerPositionSIN;
  dg::SignalPtr<dg::Vector,int> jointVelocitySIN;
  dg::SignalPtr<dg::Vector,int> freeFlyerVelocitySIN;
  dg::SignalPtr<dg::Vector,int> jointAccelerationSIN;
  dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
  
141
142
143
144
  dg::SignalTimeDependent<dg::Vector,int> pinocchioPosSINTERN;
  dg::SignalTimeDependent<dg::Vector,int> pinocchioVelSINTERN;
  dg::SignalTimeDependent<dg::Vector,int> pinocchioAccSINTERN;

145
  dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
146
147
148
  dg::SignalTimeDependent<Dummy,int> jacobiansSINTERN;
  dg::SignalTimeDependent<Dummy,int> forwardKinematicsSINTERN;
  dg::SignalTimeDependent<Dummy,int> ccrbaSINTERN;
149

150
151
152
153
  int& computeNewtonEuler(int& dummy,const int& time );
  int& computeForwardKinematics(int& dummy,const int& time );
  int& computeCcrba( int& dummy,const int& time );
  int& computeJacobians( int& dummy,const int& time );
154
155
156
157
158
159
160
  
  dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
  dg::SignalTimeDependent<dg::Matrix,int> JcomSOUT;
  dg::SignalTimeDependent<dg::Vector,int> comSOUT;
  dg::SignalTimeDependent<dg::Matrix,int> inertiaSOUT;
  
  dg::SignalTimeDependent<dg::Matrix,int>& jacobiansSOUT( const std::string& name );
Francois Bleibel's avatar
Francois Bleibel committed
161
  dg::SignalTimeDependent<MatrixHomogeneous,int>& positionsSOUT( const std::string& name );
162
163
164
  dg::SignalTimeDependent<dg::Vector,int>& velocitiesSOUT( const std::string& name );
  dg::SignalTimeDependent<dg::Vector,int>& accelerationsSOUT( const std::string& name );
  
Francois Bleibel's avatar
Francois Bleibel committed
165
  dg::SignalTimeDependent<double,int> footHeightSOUT;
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
  dg::SignalTimeDependent<dg::Vector,int> upperJlSOUT;
  dg::SignalTimeDependent<dg::Vector,int> lowerJlSOUT;
  dg::SignalTimeDependent<dg::Vector,int> upperVlSOUT;
  dg::SignalTimeDependent<dg::Vector,int> upperTlSOUT;

  dg::Signal<dg::Vector,int> inertiaRotorSOUT;
  dg::Signal<dg::Vector,int> gearRatioSOUT;
  dg::SignalTimeDependent<dg::Matrix,int> inertiaRealSOUT;
  dg::SignalTimeDependent<dg::Vector,int> MomentaSOUT;
  dg::SignalTimeDependent<dg::Vector,int> AngularMomentumSOUT;
  dg::SignalTimeDependent<dg::Vector,int> dynamicDriftSOUT;


    public:
  /* --- CONSTRUCTOR --- */
181
182
  DynamicPinocchio( const std::string& name);
  virtual ~DynamicPinocchio( void );
183

Francois Bleibel's avatar
Francois Bleibel committed
184

185
  /* --- MODEL CREATION --- */
Francois Bleibel's avatar
Francois Bleibel committed
186

187
  
188
189
  void displayModel() const 
  { assert(m_model); std::cout<<(*m_model)<<std::endl; };
190

191
192
193
  void setModel(se3::Model*);

  void setData(se3::Data*);
194
195
  
  /* --- GETTERS --- */
196

197
  /// \brief Get joint position lower limits
198
  ///
199
200
  /// \param[out] result vector
  /// \return result vector
201
  dg::Vector& getLowerPositionLimits(dg::Vector& res,const int&) const ;
202

203
  /// \brief Get joint position upper limits
204
  ///
205
206
  /// \param[out] result vector
  /// \return result vector
207
  dg::Vector& getUpperPositionLimits(dg::Vector& res,const int&) const;
208

209
  /// \brief Get joint velocity upper limits
210
  ///
211
212
  /// \param[out] result vector
  /// \return result vector
213
  dg::Vector& getUpperVelocityLimits(dg::Vector& res,const int&) const;
214

215
  /// \brief Get joint effort upper limits
florent's avatar
florent committed
216
  ///
217
218
  /// \param[out] result vector
  /// \return result vector
219
  dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&) const;
220

221
222
223
224

  //  dg::Vector& getAnklePositionInFootFrame() const;

    protected:
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
  dg::Matrix& computeGenericJacobian(const bool isFrame,
				     const int jointId,
				     dg::Matrix& res,const int& time );
  dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
					   const int jointId,
					   dg::Matrix& res,const int& time );
  MatrixHomogeneous& computeGenericPosition(const bool isFrame, 
					    const int jointId,
					    MatrixHomogeneous& res,const int& time );
  dg::Vector& computeGenericVelocity(const int jointId,dg::Vector& res,const int& time );
  dg::Vector& computeGenericAcceleration(const int jointId,dg::Vector& res,const int& time );

  dg::Vector& computeZmp( dg::Vector& res,const int& time );
  dg::Vector& computeMomenta( dg::Vector &res,const  int& time);
  dg::Vector& computeAngularMomentum( dg::Vector &res,const  int& time);
  dg::Matrix& computeJcom( dg::Matrix& res,const int& time );
  dg::Vector& computeCom( dg::Vector& res,const int& time );
  dg::Matrix& computeInertia( dg::Matrix& res,const int& time );
  dg::Matrix& computeInertiaReal( dg::Matrix& res,const int& time );
  double& computeFootHeight( double& res,const int& time );
245
246
247
248
249
250
251
252
253
254
255
256

  dg::Vector& computeTorqueDrift( dg::Vector& res,const int& time );

 public: /* --- PARAMS --- */
  virtual void commandLine( const std::string& cmdLine,
			    std::istringstream& cmdArgs,
			    std::ostream& os );
  void cmd_createOpPointSignals           ( const std::string& sig,const std::string& j );
  void cmd_createJacobianWorldSignal      ( const std::string& sig,const std::string& j );
  void cmd_createJacobianEndEffectorSignal( const std::string& sig,const std::string& j );
  void cmd_createPositionSignal           ( const std::string& sig,const std::string& j );

florent's avatar
florent committed
257
258
 private:
  /// \brief map of joints in construction.
259
  /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint) 
260
261
262
  dg::Vector& getPinocchioPos(dg::Vector& q,const int& time);
  dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
  dg::Vector& getPinocchioAcc(dg::Vector& a, const int&time);
263
264
  
  //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
265
  std::vector<int> sphericalJoints;
266

Francois Bleibel's avatar
Francois Bleibel committed
267
268
};

269
  // std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
270
} /* namespace sot */} /* namespace dynamicgraph */
Francois Bleibel's avatar
Francois Bleibel committed
271
272
273



274
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__