sot-talos-balance  1.7.0
robot-wrapper.hh
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 
23 
24 #include <pinocchio/multibody/model.hpp>
25 #include <pinocchio/multibody/data.hpp>
26 #include <pinocchio/spatial/fwd.hpp>
27 
28 #include <string>
29 #include <vector>
30 namespace dynamicgraph
31 {
32  namespace sot
33  {
34  namespace talos_balance
35  {
36  namespace robots
37  {
42  {
43  public:
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
47  typedef pinocchio::Model Model;
48  typedef pinocchio::Data Data;
49  typedef pinocchio::Motion Motion;
50  typedef pinocchio::Frame Frame;
51  typedef pinocchio::SE3 SE3;
59 
60 
61  RobotWrapper(const std::string & filename,
62  const std::vector<std::string> & package_dirs,
63  bool verbose=false);
64 
65  RobotWrapper(const std::string & filename,
66  const std::vector<std::string> & package_dirs,
67  const pinocchio::JointModelVariant & rootJoint,
68  bool verbose=false);
69 
70  virtual int nq() const;
71  virtual int nv() const;
72 
78  const Model & model() const;
79  Model & model();
80 
81  void computeAllTerms(Data & data, const Vector & q, const Vector & v) const;
82 
83  const Vector & rotor_inertias() const;
84  const Vector & gear_ratios() const;
85 
86  bool rotor_inertias(ConstRefVector rotor_inertias);
87  bool gear_ratios(ConstRefVector gear_ratios);
88 
89  void com(const Data & data,
90  RefVector com_pos,
91  RefVector com_vel,
92  RefVector com_acc) const;
93 
94  const Vector3 & com(const Data & data) const;
95 
96  const Vector3 & com_vel(const Data & data) const;
97 
98  const Vector3 & com_acc(const Data & data) const;
99 
100  const Matrix3x & Jcom(const Data & data) const;
101 
102  const Matrix & mass(const Data & data);
103 
104  const Vector & nonLinearEffects(const Data & data) const;
105 
106  const SE3 & position(const Data & data,
107  const Model::JointIndex index) const;
108 
109  const Motion & velocity(const Data & data,
110  const Model::JointIndex index) const;
111 
112  const Motion & acceleration(const Data & data,
113  const Model::JointIndex index) const;
114 
115  void jacobianWorld(const Data & data,
116  const Model::JointIndex index,
117  Data::Matrix6x & J) const;
118 
119  void jacobianLocal(const Data & data,
120  const Model::JointIndex index,
121  Data::Matrix6x & J) const;
122 
123  SE3 framePosition(const Data & data,
124  const Model::FrameIndex index) const;
125 
126  void framePosition(const Data & data,
127  const Model::FrameIndex index,
128  SE3 & framePosition) const;
129 
130  Motion frameVelocity(const Data & data,
131  const Model::FrameIndex index) const;
132 
133  void frameVelocity(const Data & data,
134  const Model::FrameIndex index,
135  Motion & frameVelocity) const;
136 
137  Motion frameAcceleration(const Data & data,
138  const Model::FrameIndex index) const;
139 
140  void frameAcceleration(const Data & data,
141  const Model::FrameIndex index,
142  Motion & frameAcceleration) const;
143 
144  Motion frameClassicAcceleration(const Data & data,
145  const Model::FrameIndex index) const;
146 
147  void frameClassicAcceleration(const Data & data,
148  const Model::FrameIndex index,
149  Motion & frameAcceleration) const;
150 
151  void frameJacobianWorld(const Data & data,
152  const Model::FrameIndex index,
153  Data::Matrix6x & J) const;
154 
155  void frameJacobianLocal(const Data & data,
156  const Model::FrameIndex index,
157  Data::Matrix6x & J) const;
158 
159 
160  protected:
161 
162  void updateMd();
163 
164 
166  Model m_model;
167  std::string m_model_filename;
168  bool m_verbose;
169 
172  Vector m_Md;
173  Matrix m_M;
174  };
175 
176  } // namespace robots
177 
178  } // namespace talos_balance
179  }
180 }
181 #endif // ifndef __invdyn_robot_wrapper_hpp__
void frameJacobianWorld(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: math/fwd.hh:45
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3x
Definition: math/fwd.hh:47
void jacobianWorld(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
const Matrix3x & Jcom(const Data &data) const
Eigen::Ref< Vector > RefVector
Definition: math/fwd.hh:52
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
void jacobianLocal(const Data &data, const Model::JointIndex index, Data::Matrix6x &J) const
void com(const Data &data, RefVector com_pos, RefVector com_vel, RefVector com_acc) const
Wrapper for a robot based on pinocchio.
SE3 framePosition(const Data &data, const Model::FrameIndex index) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Scalar Scalar
const Vector3 & com_acc(const Data &data) const
const Vector & nonLinearEffects(const Data &data) const
Motion frameClassicAcceleration(const Data &data, const Model::FrameIndex index) const
const Eigen::Ref< const Vector > ConstRefVector
Definition: math/fwd.hh:53
RobotWrapper(const std::string &filename, const std::vector< std::string > &package_dirs, bool verbose=false)
Motion frameVelocity(const Data &data, const Model::FrameIndex index) const
const Motion & velocity(const Data &data, const Model::JointIndex index) const
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
const Model & model() const
Accessor to model.
const SE3 & position(const Data &data, const Model::JointIndex index) const
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: math/fwd.hh:41
Matrix m_M
diagonal part of inertia matrix due to rotor inertias
Motion frameAcceleration(const Data &data, const Model::FrameIndex index) const
const Motion & acceleration(const Data &data, const Model::JointIndex index) const
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: math/fwd.hh:46
void frameJacobianLocal(const Data &data, const Model::FrameIndex index, Data::Matrix6x &J) const
const Vector3 & com_vel(const Data &data) const