dynamic-pinocchio.h 9.51 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
 */
Francois Bleibel's avatar
Francois Bleibel committed
9

10
11
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#define __SOT_DYNAMIC_PINOCCHIO_H__
Francois Bleibel's avatar
Francois Bleibel committed
12
13
14
15
16

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

florent's avatar
florent committed
17
18
19
/* STD */
#include <string>
#include <map>
20
#include <memory>
florent's avatar
florent committed
21

Francois Bleibel's avatar
Francois Bleibel committed
22
/* SOT */
23
#include <pinocchio/fwd.hpp>
24
#include <sot/core/flags.hh>
Francois Bleibel's avatar
Francois Bleibel committed
25
26
27
28
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
29
#include <sot/core/exception-dynamic.hh>
30
#include <sot/core/matrix-geometry.hh>
31
32
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
33
#include <sot/dynamic-pinocchio/deprecated.hh>
Francois Bleibel's avatar
Francois Bleibel committed
34

35
/* PINOCCHIO */
36
#include <pinocchio/macros.hpp>
37
38
39
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
40
#include <pinocchio/algorithm/frames.hpp>
41

Francois Bleibel's avatar
Francois Bleibel committed
42
43
44
45
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
46
47
48
#if defined(WIN32)
#if defined(dynamic_EXPORTS)
#define SOTDYNAMIC_EXPORT __declspec(dllexport)
Francois Bleibel's avatar
Francois Bleibel committed
49
#else
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
50
51
52
53
#define SOTDYNAMIC_EXPORT __declspec(dllimport)
#endif
#else
#define SOTDYNAMIC_EXPORT
Francois Bleibel's avatar
Francois Bleibel committed
54
55
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
56
namespace dynamicgraph {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
namespace sot {
namespace dg = dynamicgraph;

namespace command {
class SetFile;
class CreateOpPoint;
}  // namespace command
   /* --------------------------------------------------------------------- */
   /* --- 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.
*/
class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
75
  friend class sot::command::SetFile;
76
  friend class sot::command::CreateOpPoint;
77
  //  friend class sot::command::InitializeRobot;
Guilhem Saurel's avatar
Guilhem Saurel committed
78

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
79
 public:
80
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81
  DYNAMIC_GRAPH_ENTITY_DECL();
Francois Bleibel's avatar
Francois Bleibel committed
82

83
  /*  --- MODEL ATRIBUTES --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
84
  pinocchio::Model* m_model;
85
  std::unique_ptr<pinocchio::Data> m_data;
florent's avatar
florent committed
86

87
  /*  --- MODEL ATRIBUTES --- */
Francois Bleibel's avatar
Francois Bleibel committed
88

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
89
 public:
90
  /* --- SIGNAL ACTIVATION --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
91
92
93
94
  dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(const std::string& signame, const std::string&,
                                                                       const bool isLocal = true);
  dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(const std::string& signame, const std::string&);
  void destroyJacobianSignal(const std::string& signame);
Guilhem Saurel's avatar
Guilhem Saurel committed
95

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
96
97
  dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(const std::string&, const std::string&);
  void destroyPositionSignal(const std::string& signame);
Guilhem Saurel's avatar
Guilhem Saurel committed
98

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
99
100
  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
101

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
102
103
  dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(const std::string&, const std::string&);
  void destroyAccelerationSignal(const std::string& signame);
Francois Bleibel's avatar
Francois Bleibel committed
104

105
  /*! @} */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
106
  std::list<dg::SignalBase<int>*> genericSignalRefs;
Francois Bleibel's avatar
Francois Bleibel committed
107

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
108
 public:
109
110
  /* --- SIGNAL --- */
  typedef int Dummy;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
  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;

  dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
  dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
  dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;

  dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
  dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
  dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
  dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;

  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);

  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);
  dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(const std::string& name);
  dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(const std::string& name);
  dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(const std::string& name);

  dg::SignalTimeDependent<double, int> footHeightSOUT;
  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:
156
  /* --- CONSTRUCTOR --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
157
158
  DynamicPinocchio(const std::string& name);
  virtual ~DynamicPinocchio(void);
Francois Bleibel's avatar
Francois Bleibel committed
159

160
  /* --- MODEL CREATION --- */
Francois Bleibel's avatar
Francois Bleibel committed
161

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
162
163
164
165
  void displayModel() const {
    assert(m_model);
    std::cout << (*m_model) << std::endl;
  };
166

167
  void setModel(pinocchio::Model*);
168

169
170
171
172
173
  void createData();

  /// \deprecated this function does nothing. This class has its own
  /// pinocchio::Data object, which can be access with \ref getData.
  void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
Guilhem Saurel's avatar
Guilhem Saurel committed
174

Joseph Mirabel's avatar
Joseph Mirabel committed
175
176
  pinocchio::Model* getModel() { return m_model; };

177
  pinocchio::Data* getData() { return m_data.get(); };
Joseph Mirabel's avatar
Joseph Mirabel committed
178

179
  /* --- GETTERS --- */
180

181
  /// \brief Get joint position lower limits
182
  ///
183
184
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
185
  dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
186

187
  /// \brief Get joint position upper limits
188
  ///
189
190
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
191
  dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
192

193
  /// \brief Get joint velocity upper limits
194
  ///
195
196
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
197
  dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
198

199
  /// \brief Get joint effort upper limits
florent's avatar
florent committed
200
  ///
201
202
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
203
  dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
204
205
206

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

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
 protected:
  dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res, const int& time);
  dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, 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);

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

 public: /* --- PARAMS --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
228
229
230
231
232
233
234
  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_createJacobianEndEffectorWorldSignal(const std::string& sig, const std::string& j);
  void cmd_createPositionSignal(const std::string& sig, const std::string& j);
  void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
  void cmd_createAccelerationSignal(const std::string& sig, const std::string& j);
235

florent's avatar
florent committed
236
237
 private:
  /// \brief map of joints in construction.
Guilhem Saurel's avatar
Guilhem Saurel committed
238
  /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
239
  dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
240
  dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
241
  dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
Guilhem Saurel's avatar
Guilhem Saurel committed
242

243
  //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
244
  std::vector<int> sphericalJoints;
Francois Bleibel's avatar
Francois Bleibel committed
245
246
};

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
247
248
249
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */
} /* namespace dynamicgraph */
Francois Bleibel's avatar
Francois Bleibel committed
250

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
251
#endif  // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__