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 
103 
104  void com(const Data & data,
105  RefVector com_pos,
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 
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 
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 
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 
198  std::string m_model_filename;
199  bool m_verbose;
200 
202  int m_na;
208  };
209 
210  } // namespace robots
211 
212 } // namespace tsid
213 
214 #endif // ifndef __invdyn_robot_wrapper_hpp__
tsid::robots::RobotWrapper::framePosition
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:237
tsid::robots::RobotWrapper::is_fixed_base
virtual bool is_fixed_base() const
Definition: robot-wrapper.cpp:110
tsid::robots::RobotWrapper::m_rotor_inertias
Vector m_rotor_inertias
Definition: robot-wrapper.hpp:204
tsid::robots::RobotWrapper::frameVelocity
Motion frameVelocity(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:254
tsid::robots::RobotWrapper::frameClassicAccelerationWorldOriented
Motion frameClassicAccelerationWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:334
tsid::robots::RobotWrapper::mass
const Matrix & mass(const Data &data)
Definition: robot-wrapper.cpp:188
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
tsid::robots::RobotWrapper::frameJacobianWorld
void frameJacobianWorld(Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:346
tsid::robots::RobotWrapper::nonLinearEffects
const Vector & nonLinearEffects(const Data &data) const
Definition: robot-wrapper.cpp:195
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::robots::RobotWrapper::velocity
const Motion & velocity(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:207
tsid::robots::RobotWrapper::angularMomentumTimeVariation
Vector3 angularMomentumTimeVariation(const Data &data) const
Definition: robot-wrapper.cpp:367
tsid::math::Vector6
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
tsid::robots::RobotWrapper::m_gear_ratios
Vector m_gear_ratios
Definition: robot-wrapper.hpp:205
tsid::robots::RobotWrapper::Vector
math::Vector Vector
Definition: robot-wrapper.hpp:51
tsid::robots::RobotWrapper::setGravity
void setGravity(const Motion &gravity)
Definition: robot-wrapper.cpp:371
tsid::robots::RobotWrapper::m_is_fixed_base
bool m_is_fixed_base
number of actuators (nv for fixed-based, nv-6 for floating-base robots)
Definition: robot-wrapper.hpp:203
tsid::robots::RobotWrapper::m_model
Model m_model
Robot model.
Definition: robot-wrapper.hpp:197
tsid::robots::RobotWrapper::FLOATING_BASE_SYSTEM
@ FLOATING_BASE_SYSTEM
Definition: robot-wrapper.hpp:63
tsid::robots::RobotWrapper::gear_ratios
const Vector & gear_ratios() const
Definition: robot-wrapper.cpp:132
tsid::math::Matrix
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::robots::RobotWrapper::FIXED_BASE_SYSTEM
@ FIXED_BASE_SYSTEM
Definition: robot-wrapper.hpp:62
tsid::robots::RobotWrapper::rotor_inertias
const Vector & rotor_inertias() const
Definition: robot-wrapper.cpp:128
tsid::robots::RobotWrapper::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: robot-wrapper.hpp:57
tsid::robots::RobotWrapper::e_RootJointType
e_RootJointType
Definition: robot-wrapper.hpp:60
tsid::robots::RobotWrapper::frameClassicAcceleration
Motion frameClassicAcceleration(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:312
tsid::robots::RobotWrapper::Frame
pinocchio::Frame Frame
Definition: robot-wrapper.hpp:49
tsid::robots::RobotWrapper::Vector3
math::Vector3 Vector3
Definition: robot-wrapper.hpp:52
tsid::robots::RobotWrapper::nv
virtual int nv() const
Definition: robot-wrapper.cpp:107
tsid::robots::RobotWrapper::nq_actuated
virtual int nq_actuated() const
Definition: robot-wrapper.cpp:109
tsid::robots::RobotWrapper::model
const Model & model() const
Accessor to model.
Definition: robot-wrapper.cpp:112
tsid::math::RefVector
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
tsid::robots::RobotWrapper::com
void com(const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
Definition: robot-wrapper.cpp:158
fwd.hpp
tsid::robots::RobotWrapper::Matrix3x
math::Matrix3x Matrix3x
Definition: robot-wrapper.hpp:55
tsid::robots::RobotWrapper::RefVector
math::RefVector RefVector
Definition: robot-wrapper.hpp:56
tsid::robots::RobotWrapper::frameJacobianLocal
void frameJacobianLocal(Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:354
tsid::robots::RobotWrapper::m_na
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
tsid::robots::RobotWrapper::m_model_filename
std::string m_model_filename
Definition: robot-wrapper.hpp:198
tsid::robots::RobotWrapper::updateMd
void updateMd()
Definition: robot-wrapper.cpp:153
tsid::math::ConstRefVector
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
tsid::robots::RobotWrapper::momentumJacobian
const Data::Matrix6x & momentumJacobian(const Data &data) const
Definition: robot-wrapper.cpp:362
tsid::robots::RobotWrapper::Motion
pinocchio::Motion Motion
Definition: robot-wrapper.hpp:48
tsid::robots::RobotWrapper::m_verbose
bool m_verbose
Definition: robot-wrapper.hpp:199
tsid::robots::RobotWrapper::computeAllTerms
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
Definition: robot-wrapper.cpp:115
tsid::robots::RobotWrapper::jacobianLocal
void jacobianLocal(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:229
tsid::robots::RobotWrapper::m_nq_actuated
int m_nq_actuated
Definition: robot-wrapper.hpp:201
fwd.hpp
tsid::math::Scalar
double Scalar
Definition: fwd.hpp:36
tsid
Definition: constraint-bound.hpp:26
tsid::robots::RobotWrapper::Data
pinocchio::Data Data
Definition: robot-wrapper.hpp:47
tsid::robots::RobotWrapper::nq
virtual int nq() const
Definition: robot-wrapper.cpp:106
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::robots::RobotWrapper::jacobianWorld
void jacobianWorld(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
Definition: robot-wrapper.cpp:221
tsid::robots::RobotWrapper::frameVelocityWorldOriented
Motion frameVelocityWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:271
tsid::robots::RobotWrapper::acceleration
const Motion & acceleration(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:214
tsid::robots::RobotWrapper::init
void init()
Definition: robot-wrapper.cpp:98
tsid::robots::RobotWrapper::m_Md
Vector m_Md
Definition: robot-wrapper.hpp:206
tsid::math::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
Definition: fwd.hpp:44
tsid::robots::RobotWrapper::Jcom
const Matrix3x & Jcom(const Data &data) const
Definition: robot-wrapper.cpp:183
tsid::robots::RobotWrapper::Matrix
math::Matrix Matrix
Definition: robot-wrapper.hpp:54
tsid::robots::RobotWrapper::RobotWrapper
RobotWrapper(const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
Definition: robot-wrapper.cpp:36
tsid::robots::RobotWrapper::Model
pinocchio::Model Model
Definition: robot-wrapper.hpp:46
tsid::robots::RobotWrapper::m_M
Matrix m_M
diagonal part of inertia matrix due to rotor inertias
Definition: robot-wrapper.hpp:207
tsid::robots::RobotWrapper::RootJointType
enum tsid::robots::RobotWrapper::e_RootJointType RootJointType
tsid::robots::RobotWrapper::Vector6
math::Vector6 Vector6
Definition: robot-wrapper.hpp:53
tsid::robots::RobotWrapper::com_acc
const Vector3 & com_acc(const Data &data) const
Definition: robot-wrapper.cpp:178
tsid::robots::RobotWrapper::Scalar
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar Scalar
Definition: robot-wrapper.hpp:45
tsid::robots::RobotWrapper::com_vel
const Vector3 & com_vel(const Data &data) const
Definition: robot-wrapper.cpp:173
tsid::math::Vector3
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
tsid::robots::RobotWrapper::position
const SE3 & position(const Data &data, const Model::JointIndex index) const
Definition: robot-wrapper.cpp:200
tsid::robots::RobotWrapper::na
virtual int na() const
Definition: robot-wrapper.cpp:108
tsid::robots::RobotWrapper::frameAccelerationWorldOriented
Motion frameAccelerationWorldOriented(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:300
tsid::robots::RobotWrapper::frameAcceleration
Motion frameAcceleration(const Data &data, const Model::FrameIndex index) const
Definition: robot-wrapper.cpp:283
tsid::robots::RobotWrapper::SE3
pinocchio::SE3 SE3
Definition: robot-wrapper.hpp:50