GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/robots/robot-wrapper.hpp Lines: 2 2 100.0 %
Date: 2024-05-10 01:36:27 Branches: 0 0 - %

Line Branch Exec Source
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
namespace tsid {
33
namespace robots {
34
///
35
/// \brief Wrapper for a robot based on pinocchio
36
///
37
class RobotWrapper {
38
 public:
39
15
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40
41
  typedef math::Scalar Scalar;
42
  typedef pinocchio::Model Model;
43
  typedef pinocchio::Data Data;
44
  typedef pinocchio::Motion Motion;
45
  typedef pinocchio::Frame Frame;
46
  typedef pinocchio::SE3 SE3;
47
  typedef math::Vector Vector;
48
  typedef math::Vector3 Vector3;
49
  typedef math::Vector6 Vector6;
50
  typedef math::Matrix Matrix;
51
  typedef math::Matrix3x Matrix3x;
52
  typedef math::RefVector RefVector;
53
  typedef math::ConstRefVector ConstRefVector;
54
55
  /* Possible root joints */
56
  typedef enum e_RootJointType {
57
    FIXED_BASE_SYSTEM = 0,
58
    FLOATING_BASE_SYSTEM = 1,
59
  } RootJointType;
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
  TSID_DEPRECATED RobotWrapper(const Model& m, bool verbose = false);
71
72
  RobotWrapper(const Model& m, RootJointType rootJoint, bool verbose = false);
73
74
56
  virtual ~RobotWrapper() {}
75
76
  virtual int nq() const;
77
  virtual int nq_actuated() const;
78
  virtual int nv() const;
79
  virtual int na() const;
80
  virtual bool is_fixed_base() const;
81
82
  ///
83
  /// \brief Accessor to model.
84
  ///
85
  /// \returns a const reference on the model.
86
  ///
87
  const Model& model() const;
88
  Model& model();
89
90
  void computeAllTerms(Data& data, const Vector& q, const Vector& v) const;
91
92
  const Vector& rotor_inertias() const;
93
  const Vector& gear_ratios() const;
94
95
  bool rotor_inertias(ConstRefVector rotor_inertias);
96
  bool gear_ratios(ConstRefVector gear_ratios);
97
98
  void com(const Data& data, RefVector com_pos, RefVector com_vel,
99
           RefVector com_acc) const;
100
101
  const Vector3& com(const Data& data) const;
102
103
  const Vector3& com_vel(const Data& data) const;
104
105
  const Vector3& com_acc(const Data& data) const;
106
107
  const Matrix3x& Jcom(const Data& data) const;
108
109
  const Matrix& mass(const Data& data);
110
111
  const Vector& nonLinearEffects(const Data& data) const;
112
113
  const SE3& position(const Data& data, const Model::JointIndex index) const;
114
115
  const Motion& velocity(const Data& data, const Model::JointIndex index) const;
116
117
  const Motion& acceleration(const Data& data,
118
                             const Model::JointIndex index) const;
119
120
  void jacobianWorld(const Data& data, const Model::JointIndex index,
121
                     Data::Matrix6x& J) const;
122
123
  void jacobianLocal(const Data& data, const Model::JointIndex index,
124
                     Data::Matrix6x& J) const;
125
126
  SE3 framePosition(const Data& data, const Model::FrameIndex index) const;
127
128
  void framePosition(const Data& data, const Model::FrameIndex index,
129
                     SE3& framePosition) const;
130
131
  Motion frameVelocity(const Data& data, const Model::FrameIndex index) const;
132
133
  Motion frameVelocityWorldOriented(const Data& data,
134
                                    const Model::FrameIndex index) const;
135
136
  void frameVelocity(const Data& data, const Model::FrameIndex index,
137
                     Motion& frameVelocity) const;
138
139
  Motion frameAcceleration(const Data& data,
140
                           const Model::FrameIndex index) const;
141
142
  Motion frameAccelerationWorldOriented(const Data& data,
143
                                        const Model::FrameIndex index) const;
144
145
  void frameAcceleration(const Data& data, const Model::FrameIndex index,
146
                         Motion& frameAcceleration) const;
147
148
  Motion frameClassicAcceleration(const Data& data,
149
                                  const Model::FrameIndex index) const;
150
151
  Motion frameClassicAccelerationWorldOriented(
152
      const Data& data, const Model::FrameIndex index) const;
153
154
  void frameClassicAcceleration(const Data& data, const Model::FrameIndex index,
155
                                Motion& frameAcceleration) const;
156
157
  void frameJacobianWorld(Data& data, const Model::FrameIndex index,
158
                          Data::Matrix6x& J) const;
159
160
  void frameJacobianLocal(Data& data, const Model::FrameIndex index,
161
                          Data::Matrix6x& J) const;
162
163
  const Data::Matrix6x& momentumJacobian(const Data& data) const;
164
165
  Vector3 angularMomentumTimeVariation(const Data& data) const;
166
167
  void setGravity(const Motion& gravity);
168
169
 protected:
170
  void init();
171
  void updateMd();
172
173
  /// \brief Robot model.
174
  Model m_model;
175
  std::string m_model_filename;
176
  bool m_verbose;
177
178
  int m_nq_actuated;  /// dimension of the configuration space of the actuated
179
                      /// DoF (nq for fixed-based, nq-7 for floating-base
180
                      /// robots)
181
  int m_na;  /// number of actuators (nv for fixed-based, nv-6 for floating-base
182
             /// robots)
183
  bool m_is_fixed_base;
184
  Vector m_rotor_inertias;
185
  Vector m_gear_ratios;
186
  Vector m_Md;  /// diagonal part of inertia matrix due to rotor inertias
187
  Matrix m_M;   /// inertia matrix including rotor inertias
188
};
189
190
}  // namespace robots
191
192
}  // namespace tsid
193
194
#endif  // ifndef __invdyn_robot_wrapper_hpp__