| 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 |
|
|
} |
| 1115 |
|
|
|