dynamic-pinocchio.h 9.79 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


Guilhem Saurel's avatar
Guilhem Saurel committed
70
namespace dynamicgraph {
71
  namespace sot {
72
    namespace dg = dynamicgraph;
Guilhem Saurel's avatar
Guilhem Saurel committed
73

74
75
76
77
78
79
80
    namespace command {
      class SetFile;
      class CreateOpPoint;
    }
    /* --------------------------------------------------------------------- */
    /* --- CLASS ----------------------------------------------------------- */
    /* --------------------------------------------------------------------- */
Guilhem Saurel's avatar
Guilhem Saurel committed
81
82
83



84
85
86
87
88
89
    /*! @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
  //  friend class sot::command::InitializeRobot;
Guilhem Saurel's avatar
Guilhem Saurel committed
95
96

    public:
97
  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 );
Guilhem Saurel's avatar
Guilhem Saurel committed
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 );
Guilhem Saurel's avatar
Guilhem Saurel committed
117

118
119
120
  dg::SignalTimeDependent< dg::Vector,int >&
    createVelocitySignal( const std::string&,const std::string& );
  void destroyVelocitySignal( const std::string& signame );
Guilhem Saurel's avatar
Guilhem Saurel committed
121

122
123
  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
    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;
Guilhem Saurel's avatar
Guilhem Saurel committed
140

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 );
Guilhem Saurel's avatar
Guilhem Saurel committed
154

155
156
157
158
  dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
  dg::SignalTimeDependent<dg::Matrix,int> JcomSOUT;
  dg::SignalTimeDependent<dg::Vector,int> comSOUT;
  dg::SignalTimeDependent<dg::Matrix,int> inertiaSOUT;
Guilhem Saurel's avatar
Guilhem Saurel committed
159

160
  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
  dg::SignalTimeDependent<dg::Vector,int>& velocitiesSOUT( const std::string& name );
  dg::SignalTimeDependent<dg::Vector,int>& accelerationsSOUT( const std::string& name );
Guilhem Saurel's avatar
Guilhem Saurel committed
164

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

Guilhem Saurel's avatar
Guilhem Saurel committed
187
188

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

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

  void setData(se3::Data*);
Guilhem Saurel's avatar
Guilhem Saurel committed
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
  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 );
Guilhem Saurel's avatar
Guilhem Saurel committed
231
  MatrixHomogeneous& computeGenericPosition(const bool isFrame,
232
233
234
235
236
237
238
239
240
241
242
243
244
					    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

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

 public: /* --- PARAMS --- */
  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 );
Alexis Nicolin's avatar
Alexis Nicolin committed
253
  void cmd_createVelocitySignal           ( const std::string& sig,const std::string& j );
254
  void cmd_createAccelerationSignal       ( const std::string& sig,const std::string& j );
255

florent's avatar
florent committed
256
257
 private:
  /// \brief map of joints in construction.
Guilhem Saurel's avatar
Guilhem Saurel committed
258
  /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
259
260
261
  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);
Guilhem Saurel's avatar
Guilhem Saurel committed
262

263
  //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
264
  std::vector<int> sphericalJoints;
265

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

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



273
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__