dynamic-pinocchio.h 9.76 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
30
31
/* STD */
#include <string>
#include <map>
Francois Bleibel's avatar
Francois Bleibel committed
32
/* Matrix */
33
#include <dynamic-graph/linear-algebra.h>
florent's avatar
florent committed
34

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

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

Francois Bleibel's avatar
Francois Bleibel committed
52
53
54
55
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

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


Guilhem Saurel's avatar
Guilhem Saurel committed
67
namespace dynamicgraph {
68
  namespace sot {
69
    namespace dg = dynamicgraph;
Guilhem Saurel's avatar
Guilhem Saurel committed
70

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



81
82
83
84
85
86
    /*! @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.
    */
87
class SOTDYNAMIC_EXPORT DynamicPinocchio
88
89
 :public dg::Entity {
  friend class sot::command::SetFile;
90
  friend class sot::command::CreateOpPoint;
91
  //  friend class sot::command::InitializeRobot;
Guilhem Saurel's avatar
Guilhem Saurel committed
92
93

    public:
94
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
95
  DYNAMIC_GRAPH_ENTITY_DECL();
Francois Bleibel's avatar
Francois Bleibel committed
96

97
  /*  --- MODEL ATRIBUTES --- */
98
  se3::Model*  m_model;
99
  se3::Data*   m_data;
florent's avatar
florent committed
100

101
  /*  --- MODEL ATRIBUTES --- */
Francois Bleibel's avatar
Francois Bleibel committed
102

103
104
105
106
107
108
    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
109
  void destroyJacobianSignal( const std::string& signame );
Guilhem Saurel's avatar
Guilhem Saurel committed
110

Francois Bleibel's avatar
Francois Bleibel committed
111
  dg::SignalTimeDependent< MatrixHomogeneous,int >&
112
    createPositionSignal( const std::string&,const std::string& );
Francois Bleibel's avatar
Francois Bleibel committed
113
  void destroyPositionSignal( const std::string& signame );
Guilhem Saurel's avatar
Guilhem Saurel committed
114

115
116
117
  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
118

119
120
  dg::SignalTimeDependent< dg::Vector,int >&
    createAccelerationSignal( const std::string&, const std::string& );
Francois Bleibel's avatar
Francois Bleibel committed
121
122
  void destroyAccelerationSignal( const std::string& signame );

123
124
  /*! @} */
  std::list< dg::SignalBase<int>*  > genericSignalRefs;
Francois Bleibel's avatar
Francois Bleibel committed
125
126
127



128
129
130
131
132
133
134
135
136
    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
137

138
139
140
141
  dg::SignalTimeDependent<dg::Vector,int> pinocchioPosSINTERN;
  dg::SignalTimeDependent<dg::Vector,int> pinocchioVelSINTERN;
  dg::SignalTimeDependent<dg::Vector,int> pinocchioAccSINTERN;

142
  dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
143
144
145
  dg::SignalTimeDependent<Dummy,int> jacobiansSINTERN;
  dg::SignalTimeDependent<Dummy,int> forwardKinematicsSINTERN;
  dg::SignalTimeDependent<Dummy,int> ccrbaSINTERN;
146

147
148
149
150
  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
151

152
153
154
155
  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
156

157
  dg::SignalTimeDependent<dg::Matrix,int>& jacobiansSOUT( const std::string& name );
Francois Bleibel's avatar
Francois Bleibel committed
158
  dg::SignalTimeDependent<MatrixHomogeneous,int>& positionsSOUT( const std::string& name );
159
160
  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
161

Francois Bleibel's avatar
Francois Bleibel committed
162
  dg::SignalTimeDependent<double,int> footHeightSOUT;
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
  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 --- */
178
179
  DynamicPinocchio( const std::string& name);
  virtual ~DynamicPinocchio( void );
180

Francois Bleibel's avatar
Francois Bleibel committed
181

182
  /* --- MODEL CREATION --- */
Francois Bleibel's avatar
Francois Bleibel committed
183

Guilhem Saurel's avatar
Guilhem Saurel committed
184
185

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

188
189
190
  void setModel(se3::Model*);

  void setData(se3::Data*);
Guilhem Saurel's avatar
Guilhem Saurel committed
191

192
  /* --- GETTERS --- */
193

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

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

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

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

218
219
220
221

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

    protected:
222
223
224
225
226
227
  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
228
  MatrixHomogeneous& computeGenericPosition(const bool isFrame,
229
230
231
232
233
234
235
236
237
238
239
240
241
					    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 );
242
243
244
245
246
247
248
249

  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
250
  void cmd_createVelocitySignal           ( const std::string& sig,const std::string& j );
251
  void cmd_createAccelerationSignal       ( const std::string& sig,const std::string& j );
252

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

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

Francois Bleibel's avatar
Francois Bleibel committed
263
264
};

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



270
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__