GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/robots/robot-wrapper.cpp Lines: 0 182 0.0 %
Date: 2024-02-02 08:47:34 Branches: 0 229 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
#include "tsid/robots/robot-wrapper.hpp"
19
20
#include <pinocchio/multibody/model.hpp>
21
#include <pinocchio/parsers/urdf.hpp>
22
#include <pinocchio/algorithm/center-of-mass.hpp>
23
#include <pinocchio/algorithm/compute-all-terms.hpp>
24
#include <pinocchio/algorithm/jacobian.hpp>
25
#include <pinocchio/algorithm/frames.hpp>
26
#include <pinocchio/algorithm/centroidal.hpp>
27
28
using namespace pinocchio;
29
using namespace tsid::math;
30
31
namespace tsid {
32
namespace robots {
33
34
RobotWrapper::RobotWrapper(const std::string& filename,
35
                           const std::vector<std::string>&, bool verbose)
36
    : m_verbose(verbose) {
37
  pinocchio::urdf::buildModel(filename, m_model, m_verbose);
38
  m_model_filename = filename;
39
  m_na = m_model.nv;
40
  m_nq_actuated = m_model.nq;
41
  m_is_fixed_base = true;
42
  init();
43
}
44
45
RobotWrapper::RobotWrapper(const std::string& filename,
46
                           const std::vector<std::string>&,
47
                           const pinocchio::JointModelVariant& rootJoint,
48
                           bool verbose)
49
    : m_verbose(verbose) {
50
  pinocchio::urdf::buildModel(filename, rootJoint, m_model, m_verbose);
51
  m_model_filename = filename;
52
  m_na = m_model.nv - 6;
53
  m_nq_actuated = m_model.nq - 7;
54
  m_is_fixed_base = false;
55
  init();
56
}
57
58
RobotWrapper::RobotWrapper(const pinocchio::Model& m, bool verbose)
59
    : m_verbose(verbose) {
60
  m_model = m;
61
  m_model_filename = "";
62
  m_na = m_model.nv - 6;
63
  m_nq_actuated = m_model.nq - 7;
64
  m_is_fixed_base = false;
65
  init();
66
}
67
68
RobotWrapper::RobotWrapper(const pinocchio::Model& m, RootJointType rootJoint,
69
                           bool verbose)
70
    : m_verbose(verbose) {
71
  m_model = m;
72
  m_model_filename = "";
73
  m_na = m_model.nv;
74
  m_nq_actuated = m_model.nq;
75
  m_is_fixed_base = true;
76
  switch (rootJoint) {
77
    case FIXED_BASE_SYSTEM:
78
      break;
79
    case FLOATING_BASE_SYSTEM:
80
      m_na -= 6;
81
      m_nq_actuated = m_model.nq - 7;
82
      m_is_fixed_base = false;
83
    default:
84
      break;
85
  }
86
  init();
87
}
88
89
void RobotWrapper::init() {
90
  m_rotor_inertias.setZero(m_na);
91
  m_gear_ratios.setZero(m_na);
92
  m_Md.setZero(m_na);
93
  m_M.setZero(m_model.nv, m_model.nv);
94
}
95
96
int RobotWrapper::nq() const { return m_model.nq; }
97
int RobotWrapper::nv() const { return m_model.nv; }
98
int RobotWrapper::na() const { return m_na; }
99
int RobotWrapper::nq_actuated() const { return m_nq_actuated; }
100
bool RobotWrapper::is_fixed_base() const { return m_is_fixed_base; }
101
102
const Model& RobotWrapper::model() const { return m_model; }
103
Model& RobotWrapper::model() { return m_model; }
104
105
void RobotWrapper::computeAllTerms(Data& data, const Vector& q,
106
                                   const Vector& v) const {
107
  pinocchio::computeAllTerms(m_model, data, q, v);
108
  data.M.triangularView<Eigen::StrictlyLower>() =
109
      data.M.transpose().triangularView<Eigen::StrictlyLower>();
110
  // computeAllTerms does not compute the com acceleration, so we need to call
111
  // centerOfMass Check this line, calling with zero acceleration at the last
112
  // phase compute the CoM acceleration.
113
  //      pinocchio::centerOfMass(m_model, data, q,v,false);
114
  pinocchio::updateFramePlacements(m_model, data);
115
  pinocchio::centerOfMass(m_model, data, q, v, Eigen::VectorXd::Zero(nv()));
116
  pinocchio::ccrba(m_model, data, q, v);
117
}
118
119
const Vector& RobotWrapper::rotor_inertias() const { return m_rotor_inertias; }
120
const Vector& RobotWrapper::gear_ratios() const { return m_gear_ratios; }
121
122
bool RobotWrapper::rotor_inertias(ConstRefVector rotor_inertias) {
123
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
124
      rotor_inertias.size() == m_rotor_inertias.size(),
125
      "The size of the rotor_inertias vector is incorrect!");
126
  m_rotor_inertias = rotor_inertias;
127
  updateMd();
128
  return true;
129
}
130
131
bool RobotWrapper::gear_ratios(ConstRefVector gear_ratios) {
132
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
133
      gear_ratios.size() == m_gear_ratios.size(),
134
      "The size of the gear_ratios vector is incorrect!");
135
  m_gear_ratios = gear_ratios;
136
  updateMd();
137
  return true;
138
}
139
140
void RobotWrapper::updateMd() {
141
  m_Md =
142
      m_gear_ratios.cwiseProduct(m_gear_ratios.cwiseProduct(m_rotor_inertias));
143
}
144
145
void RobotWrapper::com(const Data& data, RefVector com_pos, RefVector com_vel,
146
                       RefVector com_acc) const {
147
  com_pos = data.com[0];
148
  com_vel = data.vcom[0];
149
  com_acc = data.acom[0];
150
}
151
152
const Vector3& RobotWrapper::com(const Data& data) const { return data.com[0]; }
153
154
const Vector3& RobotWrapper::com_vel(const Data& data) const {
155
  return data.vcom[0];
156
}
157
158
const Vector3& RobotWrapper::com_acc(const Data& data) const {
159
  return data.acom[0];
160
}
161
162
const Matrix3x& RobotWrapper::Jcom(const Data& data) const { return data.Jcom; }
163
164
const Matrix& RobotWrapper::mass(const Data& data) {
165
  m_M = data.M;
166
  m_M.diagonal().tail(m_na) += m_Md;
167
  return m_M;
168
}
169
170
const Vector& RobotWrapper::nonLinearEffects(const Data& data) const {
171
  return data.nle;
172
}
173
174
const SE3& RobotWrapper::position(const Data& data,
175
                                  const Model::JointIndex index) const {
176
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
177
      index < data.oMi.size(),
178
      "The index needs to be less than the size of the oMi vector");
179
  return data.oMi[index];
180
}
181
182
const Motion& RobotWrapper::velocity(const Data& data,
183
                                     const Model::JointIndex index) const {
184
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
185
      index < data.v.size(),
186
      "The index needs to be less than the size of the v vector");
187
  return data.v[index];
188
}
189
190
const Motion& RobotWrapper::acceleration(const Data& data,
191
                                         const Model::JointIndex index) const {
192
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
193
      index < data.a.size(),
194
      "The index needs to be less than the size of the a vector");
195
  return data.a[index];
196
}
197
198
void RobotWrapper::jacobianWorld(const Data& data,
199
                                 const Model::JointIndex index,
200
                                 Data::Matrix6x& J) const {
201
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
202
      index < data.oMi.size(),
203
      "The index needs to be less than the size of the oMi vector");
204
  return pinocchio::getJointJacobian(m_model, data, index, pinocchio::WORLD, J);
205
}
206
207
void RobotWrapper::jacobianLocal(const Data& data,
208
                                 const Model::JointIndex index,
209
                                 Data::Matrix6x& J) const {
210
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
211
      index < data.oMi.size(),
212
      "The index needs to be less than the size of the oMi vector");
213
  return pinocchio::getJointJacobian(m_model, data, index, pinocchio::LOCAL, J);
214
}
215
216
SE3 RobotWrapper::framePosition(const Data& data,
217
                                const Model::FrameIndex index) const {
218
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
219
                                 "Frame index greater than size of frame "
220
                                 "vector in model - frame may not exist");
221
  const Frame& f = m_model.frames[index];
222
  return data.oMi[f.parent].act(f.placement);
223
}
224
225
void RobotWrapper::framePosition(const Data& data,
226
                                 const Model::FrameIndex index,
227
                                 SE3& framePosition) const {
228
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
229
                                 "Frame index greater than size of frame "
230
                                 "vector in model - frame may not exist");
231
  const Frame& f = m_model.frames[index];
232
  framePosition = data.oMi[f.parent].act(f.placement);
233
}
234
235
Motion RobotWrapper::frameVelocity(const Data& data,
236
                                   const Model::FrameIndex index) const {
237
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
238
                                 "Frame index greater than size of frame "
239
                                 "vector in model - frame may not exist");
240
  const Frame& f = m_model.frames[index];
241
  return f.placement.actInv(data.v[f.parent]);
242
}
243
244
void RobotWrapper::frameVelocity(const Data& data,
245
                                 const Model::FrameIndex index,
246
                                 Motion& frameVelocity) const {
247
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
248
                                 "Frame index greater than size of frame "
249
                                 "vector in model - frame may not exist");
250
  const Frame& f = m_model.frames[index];
251
  frameVelocity = f.placement.actInv(data.v[f.parent]);
252
}
253
254
Motion RobotWrapper::frameVelocityWorldOriented(
255
    const Data& data, const Model::FrameIndex index) const {
256
  Motion v_local, v_world;
257
  SE3 oMi, oMi_rotation_only;
258
  framePosition(data, index, oMi);
259
  frameVelocity(data, index, v_local);
260
  oMi_rotation_only.rotation(oMi.rotation());
261
  v_world = oMi_rotation_only.act(v_local);
262
  return v_world;
263
}
264
265
Motion RobotWrapper::frameAcceleration(const Data& data,
266
                                       const Model::FrameIndex index) const {
267
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
268
                                 "Frame index greater than size of frame "
269
                                 "vector in model - frame may not exist");
270
  const Frame& f = m_model.frames[index];
271
  return f.placement.actInv(data.a[f.parent]);
272
}
273
274
void RobotWrapper::frameAcceleration(const Data& data,
275
                                     const Model::FrameIndex index,
276
                                     Motion& frameAcceleration) const {
277
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
278
                                 "Frame index greater than size of frame "
279
                                 "vector in model - frame may not exist");
280
  const Frame& f = m_model.frames[index];
281
  frameAcceleration = f.placement.actInv(data.a[f.parent]);
282
}
283
284
Motion RobotWrapper::frameAccelerationWorldOriented(
285
    const Data& data, const Model::FrameIndex index) const {
286
  Motion a_local, a_world;
287
  SE3 oMi, oMi_rotation_only;
288
  framePosition(data, index, oMi);
289
  frameAcceleration(data, index, a_local);
290
  oMi_rotation_only.rotation(oMi.rotation());
291
  a_world = oMi_rotation_only.act(a_local);
292
  return a_world;
293
}
294
295
Motion RobotWrapper::frameClassicAcceleration(
296
    const Data& data, const Model::FrameIndex index) const {
297
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
298
                                 "Frame index greater than size of frame "
299
                                 "vector in model - frame may not exist");
300
  const Frame& f = m_model.frames[index];
301
  Motion a = f.placement.actInv(data.a[f.parent]);
302
  Motion v = f.placement.actInv(data.v[f.parent]);
303
  a.linear() += v.angular().cross(v.linear());
304
  return a;
305
}
306
307
void RobotWrapper::frameClassicAcceleration(const Data& data,
308
                                            const Model::FrameIndex index,
309
                                            Motion& frameAcceleration) const {
310
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
311
                                 "Frame index greater than size of frame "
312
                                 "vector in model - frame may not exist");
313
  const Frame& f = m_model.frames[index];
314
  frameAcceleration = f.placement.actInv(data.a[f.parent]);
315
  Motion v = f.placement.actInv(data.v[f.parent]);
316
  frameAcceleration.linear() += v.angular().cross(v.linear());
317
}
318
319
Motion RobotWrapper::frameClassicAccelerationWorldOriented(
320
    const Data& data, const Model::FrameIndex index) const {
321
  Motion a_local, a_world;
322
  SE3 oMi, oMi_rotation_only;
323
  framePosition(data, index, oMi);
324
  frameClassicAcceleration(data, index, a_local);
325
  oMi_rotation_only.rotation(oMi.rotation());
326
  a_world = oMi_rotation_only.act(a_local);
327
  return a_world;
328
}
329
330
void RobotWrapper::frameJacobianWorld(Data& data, const Model::FrameIndex index,
331
                                      Data::Matrix6x& J) const {
332
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
333
                                 "Frame index greater than size of frame "
334
                                 "vector in model - frame may not exist");
335
  return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::WORLD, J);
336
}
337
338
void RobotWrapper::frameJacobianLocal(Data& data, const Model::FrameIndex index,
339
                                      Data::Matrix6x& J) const {
340
  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_model.frames.size(),
341
                                 "Frame index greater than size of frame "
342
                                 "vector in model - frame may not exist");
343
  return pinocchio::getFrameJacobian(m_model, data, index, pinocchio::LOCAL, J);
344
}
345
346
const Data::Matrix6x& RobotWrapper::momentumJacobian(const Data& data) const {
347
  return data.Ag;
348
}
349
350
Vector3 RobotWrapper::angularMomentumTimeVariation(const Data& data) const {
351
  return pinocchio::computeCentroidalMomentumTimeVariation(
352
             m_model, const_cast<Data&>(data))
353
      .angular();
354
}
355
356
void RobotWrapper::setGravity(const Motion& gravity) {
357
  m_model.gravity = gravity;
358
}
359
360
//    const Vector3 & com(Data & data,const Vector & q,
361
//                        const bool computeSubtreeComs = true,
362
//                        const bool updateKinematics = true)
363
//    {
364
//      return pinocchio::centerOfMass(m_model, data, q, computeSubtreeComs,
365
//      updateKinematics);
366
//    }
367
//    const Vector3 & com(Data & data, const Vector & q, const Vector & v,
368
//                 const bool computeSubtreeComs = true,
369
//                 const bool updateKinematics = true)
370
//    {
371
//      return pinocchio::centerOfMass(m_model, data, q, v, computeSubtreeComs,
372
//      updateKinematics);
373
//    }
374
375
}  // namespace robots
376
}  // namespace tsid