robot-wrapper.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef __invdyn_robot_wrapper_hpp__
19 #define __invdyn_robot_wrapper_hpp__
20 
21 #include "tsid/deprecated.hh"
22 #include "tsid/math/fwd.hpp"
23 #include "tsid/robots/fwd.hpp"
24 
25 #include <pinocchio/multibody/model.hpp>
26 #include <pinocchio/multibody/data.hpp>
27 #include <pinocchio/spatial/fwd.hpp>
28 
29 #include <string>
30 #include <vector>
31 
32 
33 namespace tsid
34 {
35  namespace robots
36  {
41  {
42  public:
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
46  typedef pinocchio::Model Model;
48  typedef pinocchio::Motion Motion;
49  typedef pinocchio::Frame Frame;
58 
59  /* Possible root joints */
60  typedef enum e_RootJointType
61  {
64  } RootJointType;
65 
66  RobotWrapper(const std::string & filename,
67  const std::vector<std::string> & package_dirs,
68  bool verbose=false);
69 
70  RobotWrapper(const std::string & filename,
71  const std::vector<std::string> & package_dirs,
72  const pinocchio::JointModelVariant & rootJoint,
73  bool verbose=false);
74 
75  TSID_DEPRECATED RobotWrapper(const Model & m,
76  bool verbose=false);
77 
78  RobotWrapper(const Model & m,
79  RootJointType rootJoint,
80  bool verbose=false);
81 
82  virtual int nq() const;
83  virtual int nq_actuated() const;
84  virtual int nv() const;
85  virtual int na() const;
86  virtual bool is_fixed_base() const;
87 
93  const Model & model() const;
94  Model & model();
95 
96  void computeAllTerms(Data & data, const Vector & q, const Vector & v) const;
97 
98  const Vector & rotor_inertias() const;
99  const Vector & gear_ratios() const;
100 
101  bool rotor_inertias(ConstRefVector rotor_inertias);
102  bool gear_ratios(ConstRefVector gear_ratios);
103 
104  void com(const Data & data,
105  RefVector com_pos,
106  RefVector com_vel,
107  RefVector com_acc) const;
108 
109  const Vector3 & com(const Data & data) const;
110 
111  const Vector3 & com_vel(const Data & data) const;
112 
113  const Vector3 & com_acc(const Data & data) const;
114 
115  const Matrix3x & Jcom(const Data & data) const;
116 
117  const Matrix & mass(const Data & data);
118 
119  const Vector & nonLinearEffects(const Data & data) const;
120 
121  const SE3 & position(const Data & data,
122  const Model::JointIndex index) const;
123 
124  const Motion & velocity(const Data & data,
125  const Model::JointIndex index) const;
126 
127  const Motion & acceleration(const Data & data,
128  const Model::JointIndex index) const;
129 
130  void jacobianWorld(const Data & data,
131  const Model::JointIndex index,
132  Data::Matrix6x & J) const;
133 
134  void jacobianLocal(const Data & data,
135  const Model::JointIndex index,
136  Data::Matrix6x & J) const;
137 
138  SE3 framePosition(const Data & data,
139  const Model::FrameIndex index) const;
140 
141  void framePosition(const Data & data,
142  const Model::FrameIndex index,
143  SE3 & framePosition) const;
144 
145  Motion frameVelocity(const Data & data,
146  const Model::FrameIndex index) const;
147 
148  Motion frameVelocityWorldOriented(const Data & data,
149  const Model::FrameIndex index) const;
150 
151  void frameVelocity(const Data & data,
152  const Model::FrameIndex index,
153  Motion & frameVelocity) const;
154 
155  Motion frameAcceleration(const Data & data,
156  const Model::FrameIndex index) const;
157 
158  Motion frameAccelerationWorldOriented(const Data & data,
159  const Model::FrameIndex index) const;
160 
161  void frameAcceleration(const Data & data,
162  const Model::FrameIndex index,
163  Motion & frameAcceleration) const;
164 
165  Motion frameClassicAcceleration(const Data & data,
166  const Model::FrameIndex index) const;
167 
168  Motion frameClassicAccelerationWorldOriented(const Data & data,
169  const Model::FrameIndex index) const;
170 
171  void frameClassicAcceleration(const Data & data,
172  const Model::FrameIndex index,
173  Motion & frameAcceleration) const;
174 
175  void frameJacobianWorld(Data & data,
176  const Model::FrameIndex index,
177  Data::Matrix6x & J) const;
178 
179  void frameJacobianLocal(Data & data,
180  const Model::FrameIndex index,
181  Data::Matrix6x & J) const;
182 
183  const Data::Matrix6x & momentumJacobian(const Data & data) const;
184 
185  Vector3 angularMomentumTimeVariation(const Data & data) const;
186 
187  void setGravity(const Motion & gravity) ;
188 
189 
190  protected:
191 
192  void init();
193  void updateMd();
194 
195 
197  Model m_model;
198  std::string m_model_filename;
199  bool m_verbose;
200 
202  int m_na;
206  Vector m_Md;
207  Matrix m_M;
208  };
209 
210  } // namespace robots
211 
212 } // namespace tsid
213 
214 #endif // ifndef __invdyn_robot_wrapper_hpp__
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
const Matrix3x & Jcom(const Data &data) const
Definition: robot-wrapper.cpp:183
enum tsid::robots::RobotWrapper::e_RootJointType RootJointType
virtual int na() const
Definition: robot-wrapper.cpp:108
Vector3 angularMomentumTimeVariation(const Data &data) const
Definition: robot-wrapper.cpp:367
const Motion & velocity(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:207
pinocchio::Motion Motion
Definition: robot-wrapper.hpp:48
Matrix m_M
diagonal part of inertia matrix due to rotor inertias
Definition: robot-wrapper.hpp:207
virtual int nq() const
Definition: robot-wrapper.cpp:106
const Data::Matrix6x & momentumJacobian(const Data &data) const
Definition: robot-wrapper.cpp:362
void init()
Definition: robot-wrapper.cpp:98
double Scalar
Definition: fwd.hpp:36
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
int m_nq_actuated
Definition: robot-wrapper.hpp:201
virtual int nq_actuated() const
Definition: robot-wrapper.cpp:109
Vector m_Md
Definition: robot-wrapper.hpp:206
const Model & model() const
Accessor to model.
Definition: robot-wrapper.cpp:112
void frameJacobianWorld(Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:346
const Motion & acceleration(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:214
Motion frameClassicAcceleration(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:312
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
Definition: fwd.hpp:44
pinocchio::Model Model
Definition: robot-wrapper.hpp:46
Motion frameVelocityWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:271
RobotWrapper(const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
Definition: robot-wrapper.cpp:36
math::Matrix Matrix
Definition: robot-wrapper.hpp:54
Model m_model
Robot model.
Definition: robot-wrapper.hpp:197
void updateMd()
Definition: robot-wrapper.cpp:153
Motion frameAccelerationWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:300
math::Matrix3x Matrix3x
Definition: robot-wrapper.hpp:55
math::RefVector RefVector
Definition: robot-wrapper.hpp:56
Vector m_gear_ratios
Definition: robot-wrapper.hpp:205
math::Vector6 Vector6
Definition: robot-wrapper.hpp:53
virtual bool is_fixed_base() const
Definition: robot-wrapper.cpp:110
const Vector3 & com_acc(const Data &data) const
Definition: robot-wrapper.cpp:178
int m_na
dimension of the configuration space of the actuated DoF (nq for fixed-based, nq-7 for floating-base ...
Definition: robot-wrapper.hpp:202
Motion frameAcceleration(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:283
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
void jacobianLocal(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:229
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:237
std::string m_model_filename
Definition: robot-wrapper.hpp:198
bool m_verbose
Definition: robot-wrapper.hpp:199
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
const Matrix & mass(const Data &data)
Definition: robot-wrapper.cpp:188
void com(const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
Definition: robot-wrapper.cpp:158
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar Scalar
Definition: robot-wrapper.hpp:45
const Vector3 & com_vel(const Data &data) const
Definition: robot-wrapper.cpp:173
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const Vector & nonLinearEffects(const Data &data) const
Definition: robot-wrapper.cpp:195
void frameJacobianLocal(Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:354
math::Vector Vector
Definition: robot-wrapper.hpp:51
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
const Vector & gear_ratios() const
Definition: robot-wrapper.cpp:132
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
pinocchio::SE3 SE3
Definition: robot-wrapper.hpp:50
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
const SE3 & position(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:200
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
Definition: robot-wrapper.cpp:115
Definition: robot-wrapper.hpp:63
void setGravity(const Motion &gravity)
Definition: robot-wrapper.cpp:371
Vector m_rotor_inertias
Definition: robot-wrapper.hpp:204
Motion frameVelocity(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:254
Definition: constraint-bound.hpp:26
Definition: robot-wrapper.hpp:62
void jacobianWorld(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:221
const Vector & rotor_inertias() const
Definition: robot-wrapper.cpp:128
pinocchio::Data Data
Definition: robot-wrapper.hpp:47
virtual int nv() const
Definition: robot-wrapper.cpp:107
e_RootJointType
Definition: robot-wrapper.hpp:60
math::ConstRefVector ConstRefVector
Definition: robot-wrapper.hpp:57
pinocchio::Frame Frame
Definition: robot-wrapper.hpp:49
math::Vector3 Vector3
Definition: robot-wrapper.hpp:52
Motion frameClassicAccelerationWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:334
bool m_is_fixed_base
number of actuators (nv for fixed-based, nv-6 for floating-base robots)
Definition: robot-wrapper.hpp:203
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29