GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/sot-dynamic-pinocchio.cpp Lines: 0 522 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 1156 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2016,
3
 * Fran├žois Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
#include <dynamic-graph/all-commands.h>
10
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
11
12
#include <boost/filesystem.hpp>
13
#include <boost/format.hpp>
14
#include <boost/version.hpp>
15
#include <pinocchio/algorithm/center-of-mass.hpp>
16
#include <pinocchio/algorithm/centroidal.hpp>
17
#include <pinocchio/algorithm/crba.hpp>
18
#include <pinocchio/algorithm/jacobian.hpp>
19
#include <pinocchio/algorithm/kinematics.hpp>
20
#include <pinocchio/fwd.hpp>
21
#include <pinocchio/multibody/model.hpp>
22
#include <pinocchio/spatial/motion.hpp>
23
#include <sot/core/debug.hh>
24
25
#include "../src/dynamic-command.h"
26
27
using namespace dynamicgraph::sot;
28
using namespace dynamicgraph;
29
30
const std::string dg::sot::DynamicPinocchio::CLASS_NAME = "DynamicPinocchio";
31
32
DynamicPinocchio::DynamicPinocchio(const std::string& name)
33
    : Entity(name),
34
      m_model(NULL),
35
      m_data()
36
37
      ,
38
      jointPositionSIN(
39
          NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::position"),
40
      freeFlyerPositionSIN(
41
          NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffposition"),
42
      jointVelocitySIN(
43
          NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::velocity"),
44
      freeFlyerVelocitySIN(
45
          NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::ffvelocity"),
46
      jointAccelerationSIN(NULL, "sotDynamicPinocchio(" + name +
47
                                     ")::input(vector)::acceleration"),
48
      freeFlyerAccelerationSIN(NULL, "sotDynamicPinocchio(" + name +
49
                                         ")::input(vector)::ffacceleration")
50
51
      ,
52
      pinocchioPosSINTERN(
53
          boost::bind(&DynamicPinocchio::getPinocchioPos, this, _1, _2),
54
          jointPositionSIN << freeFlyerPositionSIN,
55
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioPos"),
56
      pinocchioVelSINTERN(
57
          boost::bind(&DynamicPinocchio::getPinocchioVel, this, _1, _2),
58
          jointVelocitySIN << freeFlyerVelocitySIN,
59
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioVel"),
60
      pinocchioAccSINTERN(
61
          boost::bind(&DynamicPinocchio::getPinocchioAcc, this, _1, _2),
62
          jointAccelerationSIN << freeFlyerAccelerationSIN,
63
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::pinocchioAcc")
64
65
      ,
66
      newtonEulerSINTERN(
67
          boost::bind(&DynamicPinocchio::computeNewtonEuler, this, _1, _2),
68
          pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
69
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::newtoneuler"),
70
      jacobiansSINTERN(
71
          boost::bind(&DynamicPinocchio::computeJacobians, this, _1, _2),
72
          pinocchioPosSINTERN,
73
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeJacobians"),
74
      forwardKinematicsSINTERN(
75
          boost::bind(&DynamicPinocchio::computeForwardKinematics, this, _1,
76
                      _2),
77
          pinocchioPosSINTERN << pinocchioVelSINTERN << pinocchioAccSINTERN,
78
          "sotDynamicPinocchio(" + name +
79
              ")::intern(dummy)::computeForwardKinematics"),
80
      ccrbaSINTERN(
81
          boost::bind(&DynamicPinocchio::computeCcrba, this, _1, _2),
82
          pinocchioPosSINTERN << pinocchioVelSINTERN,
83
          "sotDynamicPinocchio(" + name + ")::intern(dummy)::computeCcrba"),
84
      zmpSOUT(boost::bind(&DynamicPinocchio::computeZmp, this, _1, _2),
85
              newtonEulerSINTERN,
86
              "sotDynamicPinocchio(" + name + ")::output(vector)::zmp"),
87
      JcomSOUT(boost::bind(&DynamicPinocchio::computeJcom, this, _1, _2),
88
               pinocchioPosSINTERN,
89
               "sotDynamicPinocchio(" + name + ")::output(matrix)::Jcom"),
90
      comSOUT(boost::bind(&DynamicPinocchio::computeCom, this, _1, _2),
91
              forwardKinematicsSINTERN,
92
              "sotDynamicPinocchio(" + name + ")::output(vector)::com"),
93
      inertiaSOUT(boost::bind(&DynamicPinocchio::computeInertia, this, _1, _2),
94
                  pinocchioPosSINTERN,
95
                  "sotDynamicPinocchio(" + name + ")::output(matrix)::inertia"),
96
      footHeightSOUT(
97
          boost::bind(&DynamicPinocchio::computeFootHeight, this, _1, _2),
98
          pinocchioPosSINTERN,
99
          "sotDynamicPinocchio(" + name + ")::output(double)::footHeight"),
100
      upperJlSOUT(
101
          boost::bind(&DynamicPinocchio::getUpperPositionLimits, this, _1, _2),
102
          sotNOSIGNAL,
103
          "sotDynamicPinocchio(" + name + ")::output(vector)::upperJl")
104
105
      ,
106
      lowerJlSOUT(
107
          boost::bind(&DynamicPinocchio::getLowerPositionLimits, this, _1, _2),
108
          sotNOSIGNAL,
109
          "sotDynamicPinocchio(" + name + ")::output(vector)::lowerJl")
110
111
      ,
112
      upperVlSOUT(
113
          boost::bind(&DynamicPinocchio::getUpperVelocityLimits, this, _1, _2),
114
          sotNOSIGNAL,
115
          "sotDynamicPinocchio(" + name + ")::output(vector)::upperVl")
116
117
      ,
118
      upperTlSOUT(
119
          boost::bind(&DynamicPinocchio::getMaxEffortLimits, this, _1, _2),
120
          sotNOSIGNAL,
121
          "sotDynamicPinocchio(" + name + ")::output(vector)::upperTl")
122
123
      ,
124
      inertiaRotorSOUT("sotDynamicPinocchio(" + name +
125
                       ")::output(matrix)::inertiaRotor"),
126
      gearRatioSOUT("sotDynamicPinocchio(" + name +
127
                    ")::output(matrix)::gearRatio"),
128
      inertiaRealSOUT(
129
          boost::bind(&DynamicPinocchio::computeInertiaReal, this, _1, _2),
130
          inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
131
          "sotDynamicPinocchio(" + name + ")::output(matrix)::inertiaReal"),
132
      MomentaSOUT(boost::bind(&DynamicPinocchio::computeMomenta, this, _1, _2),
133
                  ccrbaSINTERN,
134
                  "sotDynamicPinocchio(" + name + ")::output(vector)::momenta"),
135
      AngularMomentumSOUT(
136
          boost::bind(&DynamicPinocchio::computeAngularMomentum, this, _1, _2),
137
          ccrbaSINTERN,
138
          "sotDynamicPinocchio(" + name + ")::output(vector)::angularmomentum"),
139
      dynamicDriftSOUT(
140
          boost::bind(&DynamicPinocchio::computeTorqueDrift, this, _1, _2),
141
          newtonEulerSINTERN,
142
          "sotDynamicPinocchio(" + name + ")::output(vector)::dynamicDrift") {
143
  sotDEBUGIN(5);
144
145
  // TODO-------------------------------------------
146
147
  // if( build ) buildModel();
148
  // firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
149
  // DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
150
  // endTODO--------------------------------------------
151
152
  signalRegistration(jointPositionSIN);
153
  signalRegistration(freeFlyerPositionSIN);
154
  signalRegistration(jointVelocitySIN);
155
  signalRegistration(freeFlyerVelocitySIN);
156
  signalRegistration(jointAccelerationSIN);
157
  signalRegistration(freeFlyerAccelerationSIN);
158
  signalRegistration(zmpSOUT);
159
  signalRegistration(comSOUT);
160
  signalRegistration(JcomSOUT);
161
  signalRegistration(footHeightSOUT);
162
  signalRegistration(upperJlSOUT);
163
  signalRegistration(lowerJlSOUT);
164
  signalRegistration(upperVlSOUT);
165
  signalRegistration(upperTlSOUT);
166
  signalRegistration(inertiaSOUT);
167
  signalRegistration(inertiaRealSOUT);
168
  signalRegistration(inertiaRotorSOUT);
169
  signalRegistration(gearRatioSOUT);
170
  signalRegistration(MomentaSOUT);
171
  signalRegistration(AngularMomentumSOUT);
172
  signalRegistration(dynamicDriftSOUT);
173
174
  //
175
  // Commands
176
  //
177
  std::string docstring;
178
  // setFiles
179
180
  docstring =
181
      "\n"
182
      "    Display the current robot configuration.\n"
183
      "\n"
184
      "      Input:\n"
185
      "        - none \n"
186
      "\n";
187
  addCommand("displayModel", new command::DisplayModel(*this, docstring));
188
  docstring =
189
      "    \n"
190
      "    Get the dimension of the robot configuration.\n"
191
      "    \n"
192
      "      Return:\n"
193
      "        an unsigned int: the dimension.\n"
194
      "    \n";
195
  addCommand("getDimension", new command::GetDimension(*this, docstring));
196
197
  {
198
    using namespace ::dg::command;
199
    // CreateOpPoint
200
    // TODO add operational joints
201
    docstring =
202
        "    \n"
203
        "    Create an operational point attached to a robot joint local "
204
        "frame.\n"
205
        "    \n"
206
        "      Input: \n"
207
        "        - a string: name of the operational point,\n"
208
        "        - a string: name the joint, or among (gaze, left-ankle, right "
209
        "ankle\n"
210
        "          , left-wrist, right-wrist, waist, chest).\n"
211
        "\n";
212
    addCommand(
213
        "createOpPoint",
214
        makeCommandVoid2(*this, &DynamicPinocchio::cmd_createOpPointSignals,
215
                         docstring));
216
217
    docstring = docCommandVoid2(
218
        "Create a jacobian (world frame) signal only for one joint.",
219
        "string (signal name)", "string (joint name)");
220
    addCommand("createJacobian",
221
               makeCommandVoid2(
222
                   *this, &DynamicPinocchio::cmd_createJacobianWorldSignal,
223
                   docstring));
224
225
    docstring = docCommandVoid2(
226
        "Create a jacobian (endeff frame) signal only for one joint.",
227
        "string (signal name)", "string (joint name)");
228
    addCommand(
229
        "createJacobianEndEff",
230
        makeCommandVoid2(*this,
231
                         &DynamicPinocchio::cmd_createJacobianEndEffectorSignal,
232
                         docstring));
233
234
    docstring = docCommandVoid2(
235
        "Create a jacobian (endeff frame) signal only for one joint. "
236
        "The returned jacobian is placed at the joint position, but oriented "
237
        "with the world axis.",
238
        "string (signal name)", "string (joint name)");
239
    addCommand(
240
        "createJacobianEndEffWorld",
241
        makeCommandVoid2(
242
            *this, &DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal,
243
            docstring));
244
245
    docstring = docCommandVoid2(
246
        "Create a position (matrix homo) signal only for one joint.",
247
        "string (signal name)", "string (joint name)");
248
    addCommand(
249
        "createPosition",
250
        makeCommandVoid2(*this, &DynamicPinocchio::cmd_createPositionSignal,
251
                         docstring));
252
253
    docstring =
254
        docCommandVoid2("Create a velocity (vector) signal only for one joint.",
255
                        "string (signal name)", "string (joint name)");
256
    addCommand(
257
        "createVelocity",
258
        makeCommandVoid2(*this, &DynamicPinocchio::cmd_createVelocitySignal,
259
                         docstring));
260
261
    docstring = docCommandVoid2(
262
        "Create an acceleration (vector) signal only for one joint.",
263
        "string (signal name)", "string (joint name)");
264
    addCommand(
265
        "createAcceleration",
266
        makeCommandVoid2(*this, &DynamicPinocchio::cmd_createAccelerationSignal,
267
                         docstring));
268
    docstring =
269
        "\n"
270
        "  Return robot joint names.\n\n";
271
    addCommand("getJointNames", new command::GetJointNames(*this, docstring));
272
  }
273
274
  sphericalJoints.clear();
275
276
  sotDEBUG(10) << "Dynamic class_name address" << &CLASS_NAME << std::endl;
277
  sotDEBUGOUT(5);
278
}
279
280
DynamicPinocchio::~DynamicPinocchio(void) {
281
  sotDEBUGIN(15);
282
  // TODO currently, m_model and m_data are pointers owned by the Python
283
  // interpreter so we should not delete them. I (Joseph Mirabel) think it would
284
  // be wiser to make them belong to this class but I do not know the impact it
285
  // has. if (0!=m_data ) { delete m_data ; m_data =NULL; } if (0!=m_model) {
286
  // delete m_model; m_model=NULL; }
287
288
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin();
289
       iter != genericSignalRefs.end(); ++iter) {
290
    SignalBase<int>* sigPtr = *iter;
291
    delete sigPtr;
292
  }
293
  sotDEBUGOUT(15);
294
}
295
296
void DynamicPinocchio::setModel(pinocchio::Model* modelPtr) {
297
  this->m_model = modelPtr;
298
299
  if (this->m_model->nq > m_model->nv) {
300
    if (pinocchio::nv(this->m_model->joints[1]) == 6)
301
      sphericalJoints.push_back(3);  // FreeFlyer Orientation
302
303
    for (int i = 1; i < this->m_model->njoints; i++)     // 0: universe
304
      if (pinocchio::nq(this->m_model->joints[i]) == 4)  // Spherical Joint Only
305
        sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i]));
306
  }
307
308
  createData();
309
}
310
311
void DynamicPinocchio::setData(pinocchio::Data*) {}
312
313
void DynamicPinocchio::createData() {
314
  m_data.reset(new pinocchio::Data(*m_model));
315
}
316
317
/*--------------------------------GETTERS-------------------------------------------*/
318
319
dg::Vector& DynamicPinocchio::getLowerPositionLimits(dg::Vector& res,
320
                                                     const int&) const {
321
  sotDEBUGIN(15);
322
  assert(m_model);
323
324
  res.resize(m_model->nv);
325
  if (!sphericalJoints.empty()) {
326
    int fillingIndex = 0;  // SoTValue
327
    int origIndex = 0;     // PinocchioValue
328
    for (std::vector<int>::const_iterator it = sphericalJoints.begin();
329
         it < sphericalJoints.end(); it++) {
330
      if (*it - fillingIndex > 0) {
331
        res.segment(fillingIndex, *it - fillingIndex) =
332
            m_model->lowerPositionLimit.segment(origIndex, *it - fillingIndex);
333
334
        // Don't Change this order
335
        origIndex += *it - fillingIndex;
336
        fillingIndex += *it - fillingIndex;
337
      }
338
      // Found a Spherical Joint.
339
      // Assuming that spherical joint limits are unset
340
      // Version C++11
341
      // res(fillingIndex) = std::numeric_limits<double>::lowest();
342
      // res(fillingIndex + 1) = std::numeric_limits<double>::lowest();
343
      // res(fillingIndex + 2) = std::numeric_limits<double>::lowest();
344
      // For now use C++98
345
      res(fillingIndex) = -std::numeric_limits<double>::max();
346
      res(fillingIndex + 1) = -std::numeric_limits<double>::max();
347
      res(fillingIndex + 2) = -std::numeric_limits<double>::max();
348
349
      fillingIndex += 3;
350
      origIndex += 4;
351
    }
352
353
    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
354
    if (m_model->nv > fillingIndex)
355
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
356
          m_model->lowerPositionLimit.segment(origIndex,
357
                                              m_model->nv - fillingIndex);
358
  } else {
359
    res = m_model->lowerPositionLimit;
360
  }
361
  sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
362
  sotDEBUGOUT(15);
363
  return res;
364
}
365
366
dg::Vector& DynamicPinocchio::getUpperPositionLimits(dg::Vector& res,
367
                                                     const int&) const {
368
  sotDEBUGIN(15);
369
  assert(m_model);
370
371
  res.resize(m_model->nv);
372
  if (!sphericalJoints.empty()) {
373
    int fillingIndex = 0;  // SoTValue
374
    int origIndex = 0;     // PinocchioValue
375
    for (std::vector<int>::const_iterator it = sphericalJoints.begin();
376
         it < sphericalJoints.end(); it++) {
377
      if (*it - fillingIndex > 0) {
378
        res.segment(fillingIndex, *it - fillingIndex) =
379
            m_model->upperPositionLimit.segment(origIndex, *it - fillingIndex);
380
381
        // Don't Change this order
382
        origIndex += *it - fillingIndex;
383
        fillingIndex += *it - fillingIndex;
384
      }
385
      // Found a Spherical Joint.
386
      // Assuming that spherical joint limits are unset
387
      res(fillingIndex) = std::numeric_limits<double>::max();
388
      res(fillingIndex + 1) = std::numeric_limits<double>::max();
389
      res(fillingIndex + 2) = std::numeric_limits<double>::max();
390
      fillingIndex += 3;
391
      origIndex += 4;
392
    }
393
    assert(m_model->nv - fillingIndex == m_model->nq - origIndex);
394
    if (m_model->nv > fillingIndex)
395
      res.segment(fillingIndex, m_model->nv - fillingIndex) =
396
          m_model->upperPositionLimit.segment(origIndex,
397
                                              m_model->nv - fillingIndex);
398
  } else {
399
    res = m_model->upperPositionLimit;
400
  }
401
  sotDEBUG(15) << "upperLimit (" << res << ")=" << std::endl;
402
  sotDEBUGOUT(15);
403
  return res;
404
}
405
406
dg::Vector& DynamicPinocchio::getUpperVelocityLimits(dg::Vector& res,
407
                                                     const int&) const {
408
  sotDEBUGIN(15);
409
  assert(m_model);
410
411
  res.resize(m_model->nv);
412
  res = m_model->velocityLimit;
413
414
  sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" << std::endl;
415
  sotDEBUGOUT(15);
416
  return res;
417
}
418
419
dg::Vector& DynamicPinocchio::getMaxEffortLimits(dg::Vector& res,
420
                                                 const int&) const {
421
  sotDEBUGIN(15);
422
  assert(m_model);
423
424
  res.resize(m_model->nv);
425
  res = m_model->effortLimit;
426
427
  sotDEBUGOUT(15);
428
  return res;
429
}
430
431
/* ---------------- INTERNAL ------------------------------------------------ */
432
dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q, const int& time) {
433
  sotDEBUGIN(15);
434
  dg::Vector qJoints = jointPositionSIN.access(time);
435
  if (!sphericalJoints.empty()) {
436
    if (freeFlyerPositionSIN) {
437
      dg::Vector qFF = freeFlyerPositionSIN.access(time);
438
      qJoints.head<6>() = qFF;  // Overwrite qJoints ff with ffposition value
439
      assert(sphericalJoints[0] == 3);  // FreeFlyer should ideally be present.
440
    }
441
    q.resize(qJoints.size() + sphericalJoints.size());
442
    int fillingIndex = 0;
443
    int origIndex = 0;
444
    for (std::vector<int>::const_iterator it = sphericalJoints.begin();
445
         it < sphericalJoints.end(); it++) {
446
      if (*it - origIndex > 0) {
447
        q.segment(fillingIndex, *it - origIndex) =
448
            qJoints.segment(origIndex, *it - origIndex);
449
        fillingIndex += *it - origIndex;
450
        origIndex += *it - origIndex;
451
      }
452
      assert(*it == origIndex);
453
      Eigen::Quaternion<double> temp =
454
          Eigen::AngleAxisd(qJoints(origIndex + 2), Eigen::Vector3d::UnitZ()) *
455
          Eigen::AngleAxisd(qJoints(origIndex + 1), Eigen::Vector3d::UnitY()) *
456
          Eigen::AngleAxisd(qJoints(origIndex), Eigen::Vector3d::UnitX());
457
      q(fillingIndex) = temp.x();
458
      q(fillingIndex + 1) = temp.y();
459
      q(fillingIndex + 2) = temp.z();
460
      q(fillingIndex + 3) = temp.w();
461
      fillingIndex += 4;
462
      origIndex += 3;
463
    }
464
    if (qJoints.size() > origIndex)
465
      q.segment(fillingIndex, qJoints.size() - origIndex) =
466
          qJoints.tail(qJoints.size() - origIndex);
467
  } else {
468
    q.resize(qJoints.size());
469
    q = qJoints;
470
  }
471
472
  sotDEBUG(15) << "Position out" << q << std::endl;
473
  sotDEBUGOUT(15);
474
  return q;
475
}
476
477
dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time) {
478
  const Eigen::VectorXd vJoints = jointVelocitySIN.access(time);
479
  if (freeFlyerVelocitySIN) {
480
    const Eigen::VectorXd vFF = freeFlyerVelocitySIN.access(time);
481
    if (v.size() != vJoints.size() + vFF.size())
482
      v.resize(vJoints.size() + vFF.size());
483
    v << vFF, vJoints;
484
    return v;
485
  } else {
486
    v = vJoints;
487
    return v;
488
  }
489
}
490
491
dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a, const int& time) {
492
  const Eigen::VectorXd aJoints = jointAccelerationSIN.access(time);
493
  if (freeFlyerAccelerationSIN) {
494
    const Eigen::VectorXd aFF = freeFlyerAccelerationSIN.access(time);
495
    if (a.size() != aJoints.size() + aFF.size())
496
      a.resize(aJoints.size() + aFF.size());
497
    a << aFF, aJoints;
498
    return a;
499
  } else {
500
    a = aJoints;
501
    return a;
502
  }
503
}
504
505
/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
506
dg::SignalTimeDependent<dg::Matrix, int>&
507
DynamicPinocchio::createJacobianSignal(const std::string& signame,
508
                                       const std::string& jointName) {
509
  sotDEBUGIN(15);
510
  assert(m_model);
511
  dg::SignalTimeDependent<dg::Matrix, int>* sig;
512
  if (m_model->existFrame(jointName)) {
513
    long int frameId = m_model->getFrameId(jointName);
514
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
515
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, true,
516
                    frameId, _1, _2),
517
        jacobiansSINTERN,
518
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
519
  } else if (m_model->existJointName(jointName)) {
520
    long int jointId = m_model->getJointId(jointName);
521
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
522
        boost::bind(&DynamicPinocchio::computeGenericJacobian, this, false,
523
                    jointId, _1, _2),
524
        jacobiansSINTERN,
525
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
526
  } else
527
    SOT_THROW ExceptionDynamic(
528
        ExceptionDynamic::GENERIC,
529
        "Robot has no joint corresponding to " + jointName);
530
531
  genericSignalRefs.push_back(sig);
532
  signalRegistration(*sig);
533
  sotDEBUGOUT(15);
534
  return *sig;
535
}
536
537
dg::SignalTimeDependent<dg::Matrix, int>&
538
DynamicPinocchio::createEndeffJacobianSignal(const std::string& signame,
539
                                             const std::string& jointName,
540
                                             const bool isLocal) {
541
  sotDEBUGIN(15);
542
  assert(m_model);
543
  dg::SignalTimeDependent<dg::Matrix, int>* sig;
544
545
  if (m_model->existFrame(jointName)) {
546
    long int frameId = m_model->getFrameId(jointName);
547
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
548
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this, true,
549
                    isLocal, frameId, _1, _2),
550
        jacobiansSINTERN << forwardKinematicsSINTERN,
551
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
552
  } else if (m_model->existJointName(jointName)) {
553
    long int jointId = m_model->getJointId(jointName);
554
    sig = new dg::SignalTimeDependent<dg::Matrix, int>(
555
        boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian, this,
556
                    false, isLocal, jointId, _1, _2),
557
        jacobiansSINTERN << forwardKinematicsSINTERN,
558
        "sotDynamicPinocchio(" + name + ")::output(matrix)::" + signame);
559
  } else
560
    SOT_THROW ExceptionDynamic(
561
        ExceptionDynamic::GENERIC,
562
        "Robot has no joint corresponding to " + jointName);
563
  genericSignalRefs.push_back(sig);
564
  signalRegistration(*sig);
565
  sotDEBUGOUT(15);
566
  return *sig;
567
}
568
569
dg::SignalTimeDependent<MatrixHomogeneous, int>&
570
DynamicPinocchio::createPositionSignal(const std::string& signame,
571
                                       const std::string& jointName) {
572
  sotDEBUGIN(15);
573
  assert(m_model);
574
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig;
575
  if (m_model->existFrame(jointName)) {
576
    long int frameId = m_model->getFrameId(jointName);
577
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
578
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, true,
579
                    frameId, _1, _2),
580
        forwardKinematicsSINTERN,
581
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
582
  } else if (m_model->existJointName(jointName)) {
583
    long int jointId = m_model->getJointId(jointName);
584
    sig = new dg::SignalTimeDependent<MatrixHomogeneous, int>(
585
        boost::bind(&DynamicPinocchio::computeGenericPosition, this, false,
586
                    jointId, _1, _2),
587
        forwardKinematicsSINTERN,
588
        "sotDynamicPinocchio(" + name + ")::output(matrixHomo)::" + signame);
589
  } else
590
    SOT_THROW ExceptionDynamic(
591
        ExceptionDynamic::GENERIC,
592
        "Robot has no joint corresponding to " + jointName);
593
594
  genericSignalRefs.push_back(sig);
595
  signalRegistration(*sig);
596
  sotDEBUGOUT(15);
597
  return *sig;
598
}
599
600
SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::createVelocitySignal(
601
    const std::string& signame, const std::string& jointName) {
602
  sotDEBUGIN(15);
603
  assert(m_model);
604
  long int jointId = m_model->getJointId(jointName);
605
606
  SignalTimeDependent<dg::Vector, int>* sig =
607
      new SignalTimeDependent<dg::Vector, int>(
608
          boost::bind(&DynamicPinocchio::computeGenericVelocity, this, jointId,
609
                      _1, _2),
610
          forwardKinematicsSINTERN,
611
          "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);
612
  genericSignalRefs.push_back(sig);
613
  signalRegistration(*sig);
614
615
  sotDEBUGOUT(15);
616
  return *sig;
617
}
618
619
dg::SignalTimeDependent<dg::Vector, int>&
620
DynamicPinocchio::createAccelerationSignal(const std::string& signame,
621
                                           const std::string& jointName) {
622
  sotDEBUGIN(15);
623
  assert(m_model);
624
  long int jointId = m_model->getJointId(jointName);
625
  dg::SignalTimeDependent<dg::Vector, int>* sig =
626
      new dg::SignalTimeDependent<dg::Vector, int>(
627
          boost::bind(&DynamicPinocchio::computeGenericAcceleration, this,
628
                      jointId, _1, _2),
629
          forwardKinematicsSINTERN,
630
          "sotDynamicPinocchio(" + name + ")::output(dg::Vector)::" + signame);
631
632
  genericSignalRefs.push_back(sig);
633
  signalRegistration(*sig);
634
635
  sotDEBUGOUT(15);
636
  return *sig;
637
}
638
639
void DynamicPinocchio::destroyJacobianSignal(const std::string& signame) {
640
  sotDEBUGIN(15);
641
642
  bool deletable = false;
643
  dg::SignalTimeDependent<dg::Matrix, int>* sig = &jacobiansSOUT(signame);
644
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin();
645
       iter != genericSignalRefs.end(); ++iter) {
646
    if ((*iter) == sig) {
647
      genericSignalRefs.erase(iter);
648
      deletable = true;
649
      break;
650
    }
651
  }
652
653
  if (!deletable) {
654
    SOT_THROW ExceptionDynamic(
655
        ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
656
        " (while trying to remove generic jac. signal <%s>).", signame.c_str());
657
  }
658
  signalDeregistration(signame);
659
  delete sig;
660
}
661
662
void DynamicPinocchio::destroyPositionSignal(const std::string& signame) {
663
  sotDEBUGIN(15);
664
  bool deletable = false;
665
  dg::SignalTimeDependent<MatrixHomogeneous, int>* sig =
666
      &positionsSOUT(signame);
667
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin();
668
       iter != genericSignalRefs.end(); ++iter) {
669
    if ((*iter) == sig) {
670
      genericSignalRefs.erase(iter);
671
      deletable = true;
672
      break;
673
    }
674
  }
675
676
  if (!deletable) {
677
    SOT_THROW ExceptionDynamic(
678
        ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
679
        " (while trying to remove generic pos. signal <%s>).", signame.c_str());
680
  }
681
682
  signalDeregistration(signame);
683
684
  delete sig;
685
}
686
687
void DynamicPinocchio::destroyVelocitySignal(const std::string& signame) {
688
  sotDEBUGIN(15);
689
  bool deletable = false;
690
  SignalTimeDependent<dg::Vector, int>* sig = &velocitiesSOUT(signame);
691
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin();
692
       iter != genericSignalRefs.end(); ++iter) {
693
    if ((*iter) == sig) {
694
      genericSignalRefs.erase(iter);
695
      deletable = true;
696
      break;
697
    }
698
  }
699
700
  if (!deletable) {
701
    SOT_THROW ExceptionDynamic(
702
        ExceptionDynamic::CANT_DESTROY_SIGNAL, "Cannot destroy signal",
703
        " (while trying to remove generic pos. signal <%s>).", signame.c_str());
704
  }
705
706
  signalDeregistration(signame);
707
708
  delete sig;
709
}
710
711
void DynamicPinocchio::destroyAccelerationSignal(const std::string& signame) {
712
  sotDEBUGIN(15);
713
  bool deletable = false;
714
  dg::SignalTimeDependent<dg::Vector, int>* sig = &accelerationsSOUT(signame);
715
  for (std::list<SignalBase<int>*>::iterator iter = genericSignalRefs.begin();
716
       iter != genericSignalRefs.end(); ++iter) {
717
    if ((*iter) == sig) {
718
      genericSignalRefs.erase(iter);
719
      deletable = true;
720
      break;
721
    }
722
  }
723
724
  if (!deletable) {
725
    SOT_THROW ExceptionDynamic(ExceptionDynamic::CANT_DESTROY_SIGNAL,
726
                               getName() + ":cannot destroy signal",
727
                               " (while trying to remove generic acc "
728
                               "signal <%s>).",
729
                               signame.c_str());
730
  }
731
732
  signalDeregistration(signame);
733
734
  delete sig;
735
}
736
737
/* --------------------- COMPUTE
738
 * ------------------------------------------------- */
739
740
dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res, const int& time) {
741
  // TODO: To be verified
742
  sotDEBUGIN(25);
743
  assert(m_data);
744
  if (res.size() != 3) res.resize(3);
745
  newtonEulerSINTERN(time);
746
747
  const pinocchio::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
748
  const pinocchio::Force::Vector3& tau = ftau.angular();
749
  const pinocchio::Force::Vector3& f = ftau.linear();
750
  res(0) = -tau[1] / f[2];
751
  res(1) = tau[0] / f[2];
752
  res(2) = 0;
753
754
  sotDEBUGOUT(25);
755
756
  return res;
757
}
758
759
// In world coordinates
760
761
// Updates the jacobian matrix in m_data
762
int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
763
  sotDEBUGIN(25);
764
  forwardKinematicsSINTERN(time);
765
  pinocchio::computeJointJacobians(*m_model, *m_data);
766
  sotDEBUG(25) << "Jacobians updated" << std::endl;
767
  sotDEBUGOUT(25);
768
  return dummy;
769
}
770
int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
771
  sotDEBUGIN(25);
772
  assert(m_model);
773
  assert(m_data);
774
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
775
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
776
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
777
  pinocchio::forwardKinematics(*m_model, *m_data, q, v, a);
778
  sotDEBUG(25) << "Kinematics updated" << std::endl;
779
  sotDEBUGOUT(25);
780
  return dummy;
781
}
782
783
int& DynamicPinocchio::computeCcrba(int& dummy, const int& time) {
784
  sotDEBUGIN(25);
785
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
786
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
787
  pinocchio::ccrba(*m_model, *m_data, q, v);
788
  sotDEBUG(25) << "Inertia and Momentum updated" << std::endl;
789
  sotDEBUGOUT(25);
790
  return dummy;
791
}
792
793
dg::Matrix& DynamicPinocchio::computeGenericJacobian(const bool isFrame,
794
                                                     const int jointId,
795
                                                     dg::Matrix& res,
796
                                                     const int& time) {
797
  sotDEBUGIN(25);
798
  assert(m_model);
799
  assert(m_data);
800
  if (res.rows() != 6 || res.cols() != m_model->nv)
801
    res = Matrix::Zero(6, m_model->nv);
802
  jacobiansSINTERN(time);
803
804
  pinocchio::JointIndex id =
805
      isFrame ? m_model->frames[(pinocchio::JointIndex)jointId].parent
806
              : (pinocchio::JointIndex)jointId;
807
808
  // Computes Jacobian in world coordinates.
809
  pinocchio::getJointJacobian(*m_model, *m_data, id, pinocchio::WORLD, res);
810
  sotDEBUGOUT(25);
811
  return res;
812
}
813
814
dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame,
815
                                                           const bool isLocal,
816
                                                           const int id,
817
                                                           dg::Matrix& res,
818
                                                           const int& time) {
819
  sotDEBUGIN(25);
820
  assert(m_model);
821
  assert(m_data);
822
  if (res.rows() != 6 || res.cols() != m_model->nv)
823
    res = Matrix::Zero(6, m_model->nv);
824
825
  jacobiansSINTERN(time);
826
827
  pinocchio::FrameIndex fid;
828
  pinocchio::JointIndex jid;
829
  bool changeFrame = !isLocal;
830
  pinocchio::SE3 M;
831
832
  // Computes Jacobian in end-eff coordinates.
833
  if (isFrame) {
834
    changeFrame = true;
835
    fid = (pinocchio::FrameIndex)id;
836
    const pinocchio::Frame& frame = m_model->frames[fid];
837
    jid = frame.parent;
838
839
    M = frame.placement.inverse();
840
841
    if (!isLocal) {  // Express the jacobian is world coordinate system.
842
      // Need to compute frame placement for oMf.
843
      pinocchio::updateFramePlacement(*m_model, *m_data, fid);
844
845
      pinocchio::SE3 T;
846
      T.rotation() = m_data->oMf[fid].rotation();
847
      T.translation().setZero();
848
      M = T * M;
849
    }
850
  } else {
851
    jid = (pinocchio::JointIndex)id;
852
    if (!isLocal) {  // Express the jacobian is world coordinate system.
853
      M.rotation() = m_data->oMi[jid].rotation();
854
      M.translation().setZero();
855
    }
856
  }
857
  pinocchio::getJointJacobian(*m_model, *m_data, jid, pinocchio::LOCAL, res);
858
859
  if (changeFrame) pinocchio::motionSet::se3Action(M, res, res);
860
861
  sotDEBUGOUT(25);
862
  return res;
863
}
864
865
MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(
866
    const bool isFrame, const int id, MatrixHomogeneous& res, const int& time) {
867
  sotDEBUGIN(25);
868
  forwardKinematicsSINTERN(time);
869
  if (isFrame) {
870
    const pinocchio::Frame& frame = m_model->frames[id];
871
    res.matrix() =
872
        (m_data->oMi[frame.parent] * frame.placement).toHomogeneousMatrix();
873
  } else {
874
    res.matrix() = m_data->oMi[id].toHomogeneousMatrix();
875
  }
876
  sotDEBUG(25) << "For "
877
               << (isFrame ? m_model->frames[id].name : m_model->names[id])
878
               << " with id: " << id << " position is " << res << std::endl;
879
  sotDEBUGOUT(25);
880
  return res;
881
}
882
883
dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId,
884
                                                     dg::Vector& res,
885
                                                     const int& time) {
886
  sotDEBUGIN(25);
887
  forwardKinematicsSINTERN(time);
888
  res.resize(6);
889
  const pinocchio::Motion& aRV = m_data->v[jointId];
890
  res << aRV.linear(), aRV.angular();
891
  sotDEBUGOUT(25);
892
  return res;
893
}
894
895
dg::Vector& DynamicPinocchio::computeGenericAcceleration(const int jointId,
896
                                                         dg::Vector& res,
897
                                                         const int& time) {
898
  sotDEBUGIN(25);
899
  forwardKinematicsSINTERN(time);
900
  res.resize(6);
901
  const pinocchio::Motion& aRA = m_data->a[jointId];
902
  res << aRA.linear(), aRA.angular();
903
  sotDEBUGOUT(25);
904
  return res;
905
}
906
907
int& DynamicPinocchio::computeNewtonEuler(int& dummy, const int& time) {
908
  sotDEBUGIN(15);
909
  assert(m_model);
910
  assert(m_data);
911
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
912
  const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
913
  const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
914
  pinocchio::rnea(*m_model, *m_data, q, v, a);
915
916
  sotDEBUG(1) << "pos = " << q << std::endl;
917
  sotDEBUG(1) << "vel = " << v << std::endl;
918
  sotDEBUG(1) << "acc = " << a << std::endl;
919
920
  sotDEBUGOUT(15);
921
  return dummy;
922
}
923
924
dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom, const int& time) {
925
  sotDEBUGIN(25);
926
  forwardKinematicsSINTERN(time);
927
  Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, false);
928
  sotDEBUGOUT(25);
929
  return Jcom;
930
}
931
932
dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const int& time) {
933
  sotDEBUGIN(25);
934
  if (JcomSOUT.needUpdate(time)) {
935
    forwardKinematicsSINTERN(time);
936
    pinocchio::centerOfMass(*m_model, *m_data, false);
937
  }
938
  com = m_data->com[0];
939
  sotDEBUGOUT(25);
940
  return com;
941
}
942
943
dg::Matrix& DynamicPinocchio::computeInertia(dg::Matrix& res, const int& time) {
944
  sotDEBUGIN(25);
945
  const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
946
  res = pinocchio::crba(*m_model, *m_data, q);
947
  res.triangularView<Eigen::StrictlyLower>() =
948
      res.transpose().triangularView<Eigen::StrictlyLower>();
949
  sotDEBUGOUT(25);
950
  return res;
951
}
952
953
dg::Matrix& DynamicPinocchio::computeInertiaReal(dg::Matrix& res,
954
                                                 const int& time) {
955
  sotDEBUGIN(25);
956
957
  const dg::Matrix& A = inertiaSOUT(time);
958
  const dg::Vector& gearRatio = gearRatioSOUT(time);
959
  const dg::Vector& inertiaRotor = inertiaRotorSOUT(time);
960
961
  res = A;
962
  for (int i = 0; i < gearRatio.size(); ++i)
963
    res(i, i) += (gearRatio(i) * gearRatio(i) * inertiaRotor(i));
964
965
  sotDEBUGOUT(25);
966
  return res;
967
}
968
969
double& DynamicPinocchio::computeFootHeight(double& res, const int&) {
970
  // Ankle position in local foot frame
971
  // TODO: Confirm that it is in the foot frame
972
  sotDEBUGIN(25);
973
  if (!m_model->existJointName("r_sole_joint")) {
974
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
975
                               "Robot has no joint corresponding to rigthFoot");
976
  }
977
  long int jointId = m_model->getJointId("r_sole_joint");
978
  Eigen::Vector3d anklePosInLocalRefFrame = m_data->liMi[jointId].translation();
979
  // TODO: positive or negative? Current output:negative
980
  res = anklePosInLocalRefFrame(2);
981
  sotDEBUGOUT(25);
982
  return res;
983
}
984
985
dg::Vector& DynamicPinocchio::computeTorqueDrift(dg::Vector& tauDrift,
986
                                                 const int& time) {
987
  sotDEBUGIN(25);
988
  newtonEulerSINTERN(time);
989
  tauDrift = m_data->tau;
990
  sotDEBUGOUT(25);
991
  return tauDrift;
992
}
993
994
dg::Vector& DynamicPinocchio::computeMomenta(dg::Vector& Momenta,
995
                                             const int& time) {
996
  sotDEBUGIN(25);
997
  ccrbaSINTERN(time);
998
  if (Momenta.size() != 6) Momenta.resize(6);
999
1000
  Momenta = m_data->hg.toVector_impl();
1001
1002
  sotDEBUGOUT(25) << "Momenta :" << Momenta;
1003
  return Momenta;
1004
}
1005
1006
dg::Vector& DynamicPinocchio::computeAngularMomentum(dg::Vector& Momenta,
1007
                                                     const int& time) {
1008
  sotDEBUGIN(25);
1009
  ccrbaSINTERN(time);
1010
1011
  if (Momenta.size() != 3) Momenta.resize(3);
1012
  Momenta = m_data->hg.angular_impl();
1013
1014
  sotDEBUGOUT(25) << "AngularMomenta :" << Momenta;
1015
  return Momenta;
1016
}
1017
1018
/* ------------------------ SIGNAL
1019
 * CASTING--------------------------------------- */
1020
1021
dg::SignalTimeDependent<dg::Matrix, int>& DynamicPinocchio::jacobiansSOUT(
1022
    const std::string& name) {
1023
  SignalBase<int>& sigabs = Entity::getSignal(name);
1024
  try {
1025
    dg::SignalTimeDependent<dg::Matrix, int>& res =
1026
        dynamic_cast<dg::SignalTimeDependent<dg::Matrix, int>&>(sigabs);
1027
    return res;
1028
  } catch (std::bad_cast e) {
1029
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
1030
                              " (while getting signal <%s> of type matrix.",
1031
                              name.c_str());
1032
  }
1033
}
1034
dg::SignalTimeDependent<MatrixHomogeneous, int>&
1035
DynamicPinocchio::positionsSOUT(const std::string& name) {
1036
  SignalBase<int>& sigabs = Entity::getSignal(name);
1037
  try {
1038
    dg::SignalTimeDependent<MatrixHomogeneous, int>& res =
1039
        dynamic_cast<dg::SignalTimeDependent<MatrixHomogeneous, int>&>(sigabs);
1040
    return res;
1041
  } catch (std::bad_cast e) {
1042
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
1043
                              " (while getting signal <%s> of type matrixHomo.",
1044
                              name.c_str());
1045
  }
1046
}
1047
1048
dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::velocitiesSOUT(
1049
    const std::string& name) {
1050
  SignalBase<int>& sigabs = Entity::getSignal(name);
1051
  try {
1052
    dg::SignalTimeDependent<dg::Vector, int>& res =
1053
        dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
1054
    return res;
1055
  } catch (std::bad_cast e) {
1056
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
1057
                              " (while getting signal <%s> of type Vector.",
1058
                              name.c_str());
1059
  }
1060
}
1061
1062
dg::SignalTimeDependent<dg::Vector, int>& DynamicPinocchio::accelerationsSOUT(
1063
    const std::string& name) {
1064
  SignalBase<int>& sigabs = Entity::getSignal(name);
1065
  try {
1066
    dg::SignalTimeDependent<dg::Vector, int>& res =
1067
        dynamic_cast<dg::SignalTimeDependent<dg::Vector, int>&>(sigabs);
1068
    return res;
1069
  } catch (std::bad_cast e) {
1070
    SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
1071
                              " (while getting signal <%s> of type Vector.",
1072
                              name.c_str());
1073
  }
1074
}
1075
1076
/*-------------------------------------------------------------------------*/
1077
1078
/*-------------------------------------------------------------------------*/
1079
1080
/* --- PARAMS --------------------------------------------------------------- */
1081
1082
// jointName is either a fixed-joint (pinocchio operational frame) or a
1083
// movable joint (pinocchio joint-variant).
1084
void DynamicPinocchio::cmd_createOpPointSignals(const std::string& opPointName,
1085
                                                const std::string& jointName) {
1086
  createEndeffJacobianSignal(std::string("J") + opPointName, jointName, true);
1087
  createPositionSignal(opPointName, jointName);
1088
}
1089
void DynamicPinocchio::cmd_createJacobianWorldSignal(
1090
    const std::string& signalName, const std::string& jointName) {
1091
  createJacobianSignal(signalName, jointName);
1092
}
1093
void DynamicPinocchio::cmd_createJacobianEndEffectorSignal(
1094
    const std::string& signalName, const std::string& jointName) {
1095
  createEndeffJacobianSignal(signalName, jointName, true);
1096
}
1097
1098
void DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal(
1099
    const std::string& signalName, const std::string& jointName) {
1100
  createEndeffJacobianSignal(signalName, jointName, false);
1101
}
1102
1103
void DynamicPinocchio::cmd_createPositionSignal(const std::string& signalName,
1104
                                                const std::string& jointName) {
1105
  createPositionSignal(signalName, jointName);
1106
}
1107
void DynamicPinocchio::cmd_createVelocitySignal(const std::string& signalName,
1108
                                                const std::string& jointName) {
1109
  createVelocitySignal(signalName, jointName);
1110
}
1111
void DynamicPinocchio::cmd_createAccelerationSignal(
1112
    const std::string& signalName, const std::string& jointName) {
1113
  createAccelerationSignal(signalName, jointName);
1114
}