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