GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/dynamic-pinocchio/dynamic-pinocchio.h Lines: 0 8 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 2 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * Fran├žois Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
11
#define __SOT_DYNAMIC_PINOCCHIO_H__
12
13
/* --------------------------------------------------------------------- */
14
/* --- INCLUDE --------------------------------------------------------- */
15
/* --------------------------------------------------------------------- */
16
17
/* STD */
18
#include <map>
19
#include <memory>
20
#include <string>
21
22
/* pinocchio */
23
24
#include <pinocchio/fwd.hpp>
25
26
/* SOT */
27
#include <dynamic-graph/entity.h>
28
#include <dynamic-graph/pool.h>
29
#include <dynamic-graph/signal-ptr.h>
30
#include <dynamic-graph/signal-time-dependent.h>
31
32
#include <sot/core/exception-dynamic.hh>
33
#include <sot/core/flags.hh>
34
#include <sot/core/matrix-geometry.hh>
35
/* Matrix */
36
#include <dynamic-graph/linear-algebra.h>
37
38
#include <sot/dynamic-pinocchio/deprecated.hh>
39
40
/* PINOCCHIO */
41
#include <pinocchio/algorithm/frames.hpp>
42
#include <pinocchio/algorithm/jacobian.hpp>
43
#include <pinocchio/algorithm/rnea.hpp>
44
#include <pinocchio/macros.hpp>
45
#include <pinocchio/multibody/model.hpp>
46
47
/* --------------------------------------------------------------------- */
48
/* --- API ------------------------------------------------------------- */
49
/* --------------------------------------------------------------------- */
50
51
#if defined(WIN32)
52
#if defined(dynamic_EXPORTS)
53
#define SOTDYNAMIC_EXPORT __declspec(dllexport)
54
#else
55
#define SOTDYNAMIC_EXPORT __declspec(dllimport)
56
#endif
57
#else
58
#define SOTDYNAMIC_EXPORT
59
#endif
60
61
namespace dynamicgraph {
62
namespace sot {
63
namespace dg = dynamicgraph;
64
65
namespace command {
66
class SetFile;
67
class CreateOpPoint;
68
}  // namespace command
69
   /* --------------------------------------------------------------------- */
70
   /* --- CLASS ----------------------------------------------------------- */
71
   /* --------------------------------------------------------------------- */
72
73
/*! @ingroup signals
74
  \brief This class provides an inverse dynamic model of the robot.
75
  More precisely it wraps the newton euler algorithm implemented
76
  by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
77
  The robot is described by a VRML file.
78
*/
79
class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
80
  friend class sot::command::SetFile;
81
  friend class sot::command::CreateOpPoint;
82
  //  friend class sot::command::InitializeRobot;
83
84
 public:
85
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86
  DYNAMIC_GRAPH_ENTITY_DECL();
87
88
  /*  --- MODEL ATRIBUTES --- */
89
  pinocchio::Model* m_model;
90
  std::unique_ptr<pinocchio::Data> m_data;
91
92
  /*  --- MODEL ATRIBUTES --- */
93
94
 public:
95
  /* --- SIGNAL ACTIVATION --- */
96
  dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(
97
      const std::string& signame, const std::string&,
98
      const bool isLocal = true);
99
  dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(
100
      const std::string& signame, const std::string&);
101
  void destroyJacobianSignal(const std::string& signame);
102
103
  dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
104
      const std::string&, const std::string&);
105
  void destroyPositionSignal(const std::string& signame);
106
107
  dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
108
      const std::string&, const std::string&);
109
  void destroyVelocitySignal(const std::string& signame);
110
111
  dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
112
      const std::string&, const std::string&);
113
  void destroyAccelerationSignal(const std::string& signame);
114
115
  /*! @} */
116
  std::list<dg::SignalBase<int>*> genericSignalRefs;
117
118
 public:
119
  /* --- SIGNAL --- */
120
  typedef int Dummy;
121
  dg::SignalPtr<dg::Vector, int> jointPositionSIN;
122
  dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
123
  dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
124
  dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
125
  dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
126
  dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;
127
128
  dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
129
  dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
130
  dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;
131
132
  dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
133
  dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
134
  dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
135
  dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;
136
137
  int& computeNewtonEuler(int& dummy, const int& time);
138
  int& computeForwardKinematics(int& dummy, const int& time);
139
  int& computeCcrba(int& dummy, const int& time);
140
  int& computeJacobians(int& dummy, const int& time);
141
142
  dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
143
  dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
144
  dg::SignalTimeDependent<dg::Vector, int> comSOUT;
145
  dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;
146
147
  dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(
148
      const std::string& name);
149
  dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(
150
      const std::string& name);
151
  dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(
152
      const std::string& name);
153
  dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(
154
      const std::string& name);
155
156
  dg::SignalTimeDependent<double, int> footHeightSOUT;
157
  dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
158
  dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
159
  dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
160
  dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
161
162
  dg::Signal<dg::Vector, int> inertiaRotorSOUT;
163
  dg::Signal<dg::Vector, int> gearRatioSOUT;
164
  dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
165
  dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
166
  dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
167
  dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
168
169
 public:
170
  /* --- CONSTRUCTOR --- */
171
  DynamicPinocchio(const std::string& name);
172
  virtual ~DynamicPinocchio(void);
173
174
  /* --- MODEL CREATION --- */
175
176
  void displayModel() const {
177
    assert(m_model);
178
    std::cout << (*m_model) << std::endl;
179
  };
180
181
  void setModel(pinocchio::Model*);
182
183
  void createData();
184
185
  /// \deprecated this function does nothing. This class has its own
186
  /// pinocchio::Data object, which can be access with \ref getData.
187
  void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
188
189
  pinocchio::Model* getModel() { return m_model; };
190
191
  pinocchio::Data* getData() { return m_data.get(); };
192
193
  /* --- GETTERS --- */
194
195
  /// \brief Get joint position lower limits
196
  ///
197
  /// \param[out] result vector
198
  /// \return result vector
199
  dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
200
201
  /// \brief Get joint position upper limits
202
  ///
203
  /// \param[out] result vector
204
  /// \return result vector
205
  dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
206
207
  /// \brief Get joint velocity upper limits
208
  ///
209
  /// \param[out] result vector
210
  /// \return result vector
211
  dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
212
213
  /// \brief Get joint effort upper limits
214
  ///
215
  /// \param[out] result vector
216
  /// \return result vector
217
  dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
218
219
  //  dg::Vector& getAnklePositionInFootFrame() const;
220
221
 protected:
222
  dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId,
223
                                     dg::Matrix& res, const int& time);
224
  dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
225
                                           const bool isLocal,
226
                                           const int jointId, dg::Matrix& res,
227
                                           const int& time);
228
  MatrixHomogeneous& computeGenericPosition(const bool isFrame,
229
                                            const int jointId,
230
                                            MatrixHomogeneous& res,
231
                                            const int& time);
232
  dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res,
233
                                     const int& time);
234
  dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res,
235
                                         const int& time);
236
237
  dg::Vector& computeZmp(dg::Vector& res, const int& time);
238
  dg::Vector& computeMomenta(dg::Vector& res, const int& time);
239
  dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
240
  dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
241
  dg::Vector& computeCom(dg::Vector& res, const int& time);
242
  dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
243
  dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
244
  double& computeFootHeight(double& res, const int& time);
245
246
  dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
247
248
 public: /* --- PARAMS --- */
249
  void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
250
  void cmd_createJacobianWorldSignal(const std::string& sig,
251
                                     const std::string& j);
252
  void cmd_createJacobianEndEffectorSignal(const std::string& sig,
253
                                           const std::string& j);
254
  void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig,
255
                                                const std::string& j);
256
  void cmd_createPositionSignal(const std::string& sig, const std::string& j);
257
  void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
258
  void cmd_createAccelerationSignal(const std::string& sig,
259
                                    const std::string& j);
260
261
 private:
262
  /// \brief map of joints in construction.
263
  /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr
264
  /// to pinocchio Joint)
265
  dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
266
  dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
267
  dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
268
269
  //\brief Index list for the first dof of (spherical joints)/ (spherical part
270
  // of free-flyer joint).
271
  std::vector<int> sphericalJoints;
272
};
273
274
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
275
} /* namespace sot */
276
} /* namespace dynamicgraph */
277
278
#endif  // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__