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