1 /** \file itasc/Armature.cpp
2 * \ingroup itasc
3 */
4 /*
5 * Armature.cpp
6 *
7 * Created on: Feb 3, 2009
8 * Author: benoitbolsee
9 */
10
11 #include "Armature.hpp"
12 #include <algorithm>
13 #include <string.h>
14 #include <stdlib.h>
15
16 namespace iTaSC {
17
18 #if 0
19 // a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot
20 static const unsigned int constraintCacheSize = 5;
21 #endif
22 std::string Armature::m_root = "root";
23
Armature()24 Armature::Armature():
25 ControlledObject(),
26 m_tree(),
27 m_njoint(0),
28 m_nconstraint(0),
29 m_noutput(0),
30 m_neffector(0),
31 m_finalized(false),
32 m_cache(NULL),
33 m_buf(NULL),
34 m_qCCh(-1),
35 m_qCTs(0),
36 m_yCCh(-1),
37 #if 0
38 m_yCTs(0),
39 #endif
40 m_qKdl(),
41 m_oldqKdl(),
42 m_newqKdl(),
43 m_qdotKdl(),
44 m_jac(NULL),
45 m_armlength(0.0),
46 m_jacsolver(NULL),
47 m_fksolver(NULL)
48 {
49 }
50
~Armature()51 Armature::~Armature()
52 {
53 if (m_jac)
54 delete m_jac;
55 if (m_jacsolver)
56 delete m_jacsolver;
57 if (m_fksolver)
58 delete m_fksolver;
59 for (JointConstraintList::iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
60 if (*it != NULL)
61 delete (*it);
62 }
63 if (m_buf)
64 delete [] m_buf;
65 m_constraints.clear();
66 }
67
JointConstraint_struct(SegmentMap::const_iterator _segment,unsigned int _y_nr,ConstraintCallback _function,void * _param,bool _freeParam,bool _substep)68 Armature::JointConstraint_struct::JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep):
69 segment(_segment), value(), values(), function(_function), y_nr(_y_nr), param(_param), freeParam(_freeParam), substep(_substep)
70 {
71 memset(values, 0, sizeof(values));
72 memset(value, 0, sizeof(value));
73 values[0].feedback = 20.0;
74 values[1].feedback = 20.0;
75 values[2].feedback = 20.0;
76 values[0].tolerance = 1.0;
77 values[1].tolerance = 1.0;
78 values[2].tolerance = 1.0;
79 values[0].values = &value[0];
80 values[1].values = &value[1];
81 values[2].values = &value[2];
82 values[0].number = 1;
83 values[1].number = 1;
84 values[2].number = 1;
85 switch (segment->second.segment.getJoint().getType()) {
86 case Joint::RotX:
87 value[0].id = ID_JOINT_RX;
88 values[0].id = ID_JOINT_RX;
89 v_nr = 1;
90 break;
91 case Joint::RotY:
92 value[0].id = ID_JOINT_RY;
93 values[0].id = ID_JOINT_RY;
94 v_nr = 1;
95 break;
96 case Joint::RotZ:
97 value[0].id = ID_JOINT_RZ;
98 values[0].id = ID_JOINT_RZ;
99 v_nr = 1;
100 break;
101 case Joint::TransX:
102 value[0].id = ID_JOINT_TX;
103 values[0].id = ID_JOINT_TX;
104 v_nr = 1;
105 break;
106 case Joint::TransY:
107 value[0].id = ID_JOINT_TY;
108 values[0].id = ID_JOINT_TY;
109 v_nr = 1;
110 break;
111 case Joint::TransZ:
112 value[0].id = ID_JOINT_TZ;
113 values[0].id = ID_JOINT_TZ;
114 v_nr = 1;
115 break;
116 case Joint::Sphere:
117 values[0].id = value[0].id = ID_JOINT_RX;
118 values[1].id = value[1].id = ID_JOINT_RY;
119 values[2].id = value[2].id = ID_JOINT_RZ;
120 v_nr = 3;
121 break;
122 case Joint::Swing:
123 values[0].id = value[0].id = ID_JOINT_RX;
124 values[1].id = value[1].id = ID_JOINT_RZ;
125 v_nr = 2;
126 break;
127 case Joint::None:
128 break;
129 }
130 }
131
~JointConstraint_struct()132 Armature::JointConstraint_struct::~JointConstraint_struct()
133 {
134 if (freeParam && param)
135 free(param);
136 }
137
initCache(Cache * _cache)138 void Armature::initCache(Cache *_cache)
139 {
140 m_cache = _cache;
141 m_qCCh = -1;
142 m_yCCh = -1;
143 m_buf = NULL;
144 if (m_cache) {
145 // add a special channel for the joint
146 m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
147 #if 0
148 // for the constraints, instead of creating many different channels, we will
149 // create a single channel for all the constraints
150 if (m_nconstraint) {
151 m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double));
152 m_buf = new double[m_nconstraint*constraintCacheSize];
153 }
154 // store the initial cache position at timestamp 0
155 pushConstraints(0);
156 #endif
157 pushQ(0);
158 }
159 }
160
pushQ(CacheTS timestamp)161 void Armature::pushQ(CacheTS timestamp)
162 {
163 if (m_qCCh >= 0) {
164 // try to keep the cache if the joints are the same
165 m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
166 m_qCTs = timestamp;
167 }
168 }
169
170 /* return true if a m_cache position was loaded */
popQ(CacheTS timestamp)171 bool Armature::popQ(CacheTS timestamp)
172 {
173 if (m_qCCh >= 0) {
174 double* item;
175 item = (double *)m_cache->getPreviousCacheItem(this, m_qCCh, ×tamp);
176 if (item && m_qCTs != timestamp) {
177 double* q = m_qKdl(0);
178 memcpy(q, item, m_qKdl.rows()*sizeof(double));
179 m_qCTs = timestamp;
180 // changing the joint => recompute the jacobian
181 updateJacobian();
182 }
183 return (item) ? true : false;
184 }
185 return true;
186 }
187 #if 0
188 void Armature::pushConstraints(CacheTS timestamp)
189 {
190 if (m_yCCh >= 0) {
191 double *buf = NULL;
192 if (m_nconstraint) {
193 double *item = m_buf;
194 for (unsigned int i=0; i<m_nconstraint; i++) {
195 JointConstraint_struct* pConstraint = m_constraints[i];
196 *item++ = pConstraint->values.feedback;
197 *item++ = pConstraint->values.tolerance;
198 *item++ = pConstraint->value.yd;
199 *item++ = pConstraint->value.yddot;
200 *item++ = pConstraint->values.alpha;
201 }
202 }
203 m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon);
204 m_yCTs = timestamp;
205 }
206 }
207
208 /* return true if a cache position was loaded */
209 bool Armature::popConstraints(CacheTS timestamp)
210 {
211 if (m_yCCh >= 0) {
212 double *item = (double*)m_cache->getPreviousCacheItem(this, m_yCCh, ×tamp);
213 if (item && m_yCTs != timestamp) {
214 for (unsigned int i=0; i<m_nconstraint; i++) {
215 JointConstraint_struct* pConstraint = m_constraints[i];
216 if (pConstraint->function != Joint1DOFLimitCallback) {
217 pConstraint->values.feedback = *item++;
218 pConstraint->values.tolerance = *item++;
219 pConstraint->value.yd = *item++;
220 pConstraint->value.yddot = *item++;
221 pConstraint->values.alpha = *item++;
222 } else {
223 item += constraintCacheSize;
224 }
225 }
226 m_yCTs = timestamp;
227 }
228 return (item) ? true : false;
229 }
230 return true;
231 }
232 #endif
233
addSegment(const std::string & segment_name,const std::string & hook_name,const Joint & joint,const double & q_rest,const Frame & f_tip,const Inertia & M)234 bool Armature::addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip, const Inertia& M)
235 {
236 if (m_finalized)
237 return false;
238
239 Segment segment(joint, f_tip, M);
240 if (!m_tree.addSegment(segment, segment_name, hook_name))
241 return false;
242 int ndof = joint.getNDof();
243 for (int dof=0; dof<ndof; dof++) {
244 Joint_struct js(joint.getType(), ndof, (&q_rest)[dof]);
245 m_joints.push_back(js);
246 }
247 m_njoint+=ndof;
248 return true;
249 }
250
getSegment(const std::string & name,const unsigned int q_size,const Joint * & p_joint,double & q_rest,double & q,const Frame * & p_tip)251 bool Armature::getSegment(const std::string& name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip)
252 {
253 SegmentMap::const_iterator sit = m_tree.getSegment(name);
254 if (sit == m_tree.getSegments().end())
255 return false;
256 p_joint = &sit->second.segment.getJoint();
257 if (q_size < p_joint->getNDof())
258 return false;
259 p_tip = &sit->second.segment.getFrameToTip();
260 for (unsigned int dof=0; dof<p_joint->getNDof(); dof++) {
261 (&q_rest)[dof] = m_joints[sit->second.q_nr+dof].rest;
262 (&q)[dof] = m_qKdl[sit->second.q_nr+dof];
263 }
264 return true;
265 }
266
getMaxJointChange()267 double Armature::getMaxJointChange()
268 {
269 if (!m_finalized)
270 return 0.0;
271 double maxJoint = 0.0;
272 for (unsigned int i=0; i<m_njoint; i++) {
273 // this is a very rough calculation, it doesn't work well for spherical joint
274 double joint = fabs(m_oldqKdl[i]-m_qKdl[i]);
275 if (maxJoint < joint)
276 maxJoint = joint;
277 }
278 return maxJoint;
279 }
280
getMaxEndEffectorChange()281 double Armature::getMaxEndEffectorChange()
282 {
283 if (!m_finalized)
284 return 0.0;
285 double maxDelta = 0.0;
286 double delta;
287 Twist twist;
288 for (unsigned int i = 0; i<m_neffector; i++) {
289 twist = diff(m_effectors[i].pose, m_effectors[i].oldpose);
290 delta = twist.rot.Norm();
291 if (delta > maxDelta)
292 maxDelta = delta;
293 delta = twist.vel.Norm();
294 if (delta > maxDelta)
295 maxDelta = delta;
296 }
297 return maxDelta;
298 }
299
addConstraint(const std::string & segment_name,ConstraintCallback _function,void * _param,bool _freeParam,bool _substep)300 int Armature::addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep)
301 {
302 SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
303 // not suitable for NDof joints
304 if (segment_it == m_tree.getSegments().end()) {
305 if (_freeParam && _param)
306 free(_param);
307 return -1;
308 }
309 JointConstraintList::iterator constraint_it;
310 JointConstraint_struct* pConstraint;
311 int iConstraint;
312 for (iConstraint=0, constraint_it=m_constraints.begin(); constraint_it != m_constraints.end(); constraint_it++, iConstraint++) {
313 pConstraint = *constraint_it;
314 if (pConstraint->segment == segment_it) {
315 // redefining a constraint
316 if (pConstraint->freeParam && pConstraint->param) {
317 free(pConstraint->param);
318 }
319 pConstraint->function = _function;
320 pConstraint->param = _param;
321 pConstraint->freeParam = _freeParam;
322 pConstraint->substep = _substep;
323 return iConstraint;
324 }
325 }
326 if (m_finalized) {
327 if (_freeParam && _param)
328 free(_param);
329 return -1;
330 }
331 // new constraint, append
332 pConstraint = new JointConstraint_struct(segment_it, m_noutput, _function, _param, _freeParam, _substep);
333 m_constraints.push_back(pConstraint);
334 m_noutput += pConstraint->v_nr;
335 return m_nconstraint++;
336 }
337
addLimitConstraint(const std::string & segment_name,unsigned int dof,double _min,double _max)338 int Armature::addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max)
339 {
340 SegmentMap::const_iterator segment_it = m_tree.getSegment(segment_name);
341 if (segment_it == m_tree.getSegments().end())
342 return -1;
343 const Joint& joint = segment_it->second.segment.getJoint();
344 if (joint.getNDof() != 1 && joint.getType() != Joint::Swing) {
345 // not suitable for Sphere joints
346 return -1;
347 }
348 if ((joint.getNDof() == 1 && dof > 0) || (joint.getNDof() == 2 && dof > 1))
349 return -1;
350 Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
351 p_joint.min = _min;
352 p_joint.max = _max;
353 p_joint.useLimit = true;
354 return 0;
355 }
356
addEndEffector(const std::string & name)357 int Armature::addEndEffector(const std::string& name)
358 {
359 const SegmentMap& segments = m_tree.getSegments();
360 if (segments.find(name) == segments.end())
361 return -1;
362
363 EffectorList::const_iterator it;
364 int ee;
365 for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
366 if (it->name == name)
367 return ee;
368 }
369 if (m_finalized)
370 return -1;
371 Effector_struct effector(name);
372 m_effectors.push_back(effector);
373 return m_neffector++;
374 }
375
finalize()376 bool Armature::finalize()
377 {
378 unsigned int i, j, c;
379 if (m_finalized)
380 return true;
381 if (m_njoint == 0)
382 return false;
383 initialize(m_njoint, m_noutput, m_neffector);
384 for (i=c=0; i<m_nconstraint; i++) {
385 JointConstraint_struct* pConstraint = m_constraints[i];
386 for (j=0; j<pConstraint->v_nr; j++, c++) {
387 m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0;
388 m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
389 }
390 }
391 m_jacsolver= new KDL::TreeJntToJacSolver(m_tree);
392 m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
393 m_jac = new Jacobian(m_njoint);
394 m_qKdl.resize(m_njoint);
395 m_oldqKdl.resize(m_njoint);
396 m_newqKdl.resize(m_njoint);
397 m_qdotKdl.resize(m_njoint);
398 for (i=0; i<m_njoint; i++) {
399 m_newqKdl[i] = m_oldqKdl[i] = m_qKdl[i] = m_joints[i].rest;
400 }
401 updateJacobian();
402 // estimate the maximum size of the robot arms
403 double length;
404 m_armlength = 0.0;
405 for (i=0; i<m_neffector; i++) {
406 length = 0.0;
407 KDL::SegmentMap::value_type const *sit = m_tree.getSegmentPtr(m_effectors[i].name);
408 while (sit->first != "root") {
409 Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
410 length += tip.p.Norm();
411 sit = sit->second.parent;
412 }
413 if (length > m_armlength)
414 m_armlength = length;
415 }
416 if (m_armlength < KDL::epsilon)
417 m_armlength = KDL::epsilon;
418 m_finalized = true;
419 return true;
420 }
421
pushCache(const Timestamp & timestamp)422 void Armature::pushCache(const Timestamp& timestamp)
423 {
424 if (!timestamp.substep && timestamp.cache) {
425 pushQ(timestamp.cacheTimestamp);
426 //pushConstraints(timestamp.cacheTimestamp);
427 }
428 }
429
setJointArray(const KDL::JntArray & joints)430 bool Armature::setJointArray(const KDL::JntArray& joints)
431 {
432 if (!m_finalized)
433 return false;
434 if (joints.rows() != m_qKdl.rows())
435 return false;
436 m_qKdl = joints;
437 updateJacobian();
438 return true;
439 }
440
getJointArray()441 const KDL::JntArray& Armature::getJointArray()
442 {
443 return m_qKdl;
444 }
445
updateJoint(const Timestamp & timestamp,JointLockCallback & callback)446 bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
447 {
448 if (!m_finalized)
449 return false;
450
451 // integration and joint limit
452 // for spherical joint we must use a more sophisticated method
453 unsigned int q_nr;
454 double* qdot=m_qdotKdl(0);
455 double* q=m_qKdl(0);
456 double* newq=m_newqKdl(0);
457 double norm, qx, qz, CX, CZ, sx, sz;
458 bool locked = false;
459 int unlocked = 0;
460
461 for (q_nr=0; q_nr<m_nq; ++q_nr)
462 qdot[q_nr]=m_qdot[q_nr];
463
464 for (q_nr=0; q_nr<m_nq; ) {
465 Joint_struct* joint = &m_joints[q_nr];
466 if (!joint->locked) {
467 switch (joint->type) {
468 case KDL::Joint::Swing:
469 {
470 KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
471 (base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
472 if (joint[0].useLimit) {
473 if (joint[1].useLimit) {
474 // elliptical limit
475 sx = sz = 1.0;
476 qx = newq[0];
477 qz = newq[1];
478 // determine in which quadrant we are
479 if (qx > 0.0 && qz > 0.0) {
480 CX = joint[0].max;
481 CZ = joint[1].max;
482 } else if (qx <= 0.0 && qz > 0.0) {
483 CX = -joint[0].min;
484 CZ = joint[1].max;
485 qx = -qx;
486 sx = -1.0;
487 } else if (qx <= 0.0 && qz <= 0.0) {
488 CX = -joint[0].min;
489 CZ = -joint[1].min;
490 qx = -qx;
491 qz = -qz;
492 sx = sz = -1.0;
493 } else {
494 CX = joint[0].max;
495 CZ = -joint[0].min;
496 qz = -qz;
497 sz = -1.0;
498 }
499 if (CX < KDL::epsilon || CZ < KDL::epsilon) {
500 // quadrant is degenerated
501 if (qx > CX) {
502 newq[0] = CX*sx;
503 joint[0].locked = true;
504 }
505 if (qz > CZ) {
506 newq[1] = CZ*sz;
507 joint[0].locked = true;
508 }
509 } else {
510 // general case
511 qx /= CX;
512 qz /= CZ;
513 norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
514 if (norm > 1.0) {
515 norm = 1.0/norm;
516 newq[0] = qx*norm*CX*sx;
517 newq[1] = qz*norm*CZ*sz;
518 joint[0].locked = true;
519 }
520 }
521 } else {
522 // limit on X only
523 qx = newq[0];
524 if (qx > joint[0].max) {
525 newq[0] = joint[0].max;
526 joint[0].locked = true;
527 } else if (qx < joint[0].min) {
528 newq[0] = joint[0].min;
529 joint[0].locked = true;
530 }
531 }
532 } else if (joint[1].useLimit) {
533 // limit on Z only
534 qz = newq[1];
535 if (qz > joint[1].max) {
536 newq[1] = joint[1].max;
537 joint[0].locked = true;
538 } else if (qz < joint[1].min) {
539 newq[1] = joint[1].min;
540 joint[0].locked = true;
541 }
542 }
543 if (joint[0].locked) {
544 // check the difference from previous position
545 locked = true;
546 norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]);
547 if (norm < KDL::epsilon2) {
548 // joint didn't move, no need to update the jacobian
549 callback.lockJoint(q_nr, 2);
550 } else {
551 // joint moved, compute the corresponding velocity
552 double deltaq[2];
553 (base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq);
554 deltaq[0] /= timestamp.realTimestep;
555 deltaq[1] /= timestamp.realTimestep;
556 callback.lockJoint(q_nr, 2, deltaq);
557 // no need to update the other joints, it will be done after next rerun
558 goto end_loop;
559 }
560 } else
561 unlocked++;
562 break;
563 }
564 case KDL::Joint::Sphere:
565 {
566 (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
567 // no limit on this joint
568 unlocked++;
569 break;
570 }
571 default:
572 for (unsigned int i=0; i<joint->ndof; i++) {
573 newq[i] = q[i]+qdot[i]*timestamp.realTimestep;
574 if (joint[i].useLimit) {
575 if (newq[i] > joint[i].max) {
576 newq[i] = joint[i].max;
577 joint[0].locked = true;
578 } else if (newq[i] < joint[i].min) {
579 newq[i] = joint[i].min;
580 joint[0].locked = true;
581 }
582 }
583 }
584 if (joint[0].locked) {
585 locked = true;
586 norm = 0.0;
587 // compute delta to locked position
588 for (unsigned int i=0; i<joint->ndof; i++) {
589 qdot[i] = newq[i] - q[i];
590 norm += qdot[i]*qdot[i];
591 }
592 if (norm < KDL::epsilon2) {
593 // joint didn't move, no need to update the jacobian
594 callback.lockJoint(q_nr, joint->ndof);
595 } else {
596 // solver needs velocity, compute equivalent velocity
597 for (unsigned int i=0; i<joint->ndof; i++) {
598 qdot[i] /= timestamp.realTimestep;
599 }
600 callback.lockJoint(q_nr, joint->ndof, qdot);
601 goto end_loop;
602 }
603 } else
604 unlocked++;
605 }
606 }
607 qdot += joint->ndof;
608 q += joint->ndof;
609 newq += joint->ndof;
610 q_nr += joint->ndof;
611 }
612 end_loop:
613 // check if there any other unlocked joint
614 for ( ; q_nr<m_nq; ) {
615 Joint_struct* joint = &m_joints[q_nr];
616 if (!joint->locked)
617 unlocked++;
618 q_nr += joint->ndof;
619 }
620 // if all joints have been locked no need to run the solver again
621 return (unlocked) ? locked : false;
622 }
623
updateKinematics(const Timestamp & timestamp)624 void Armature::updateKinematics(const Timestamp& timestamp){
625
626 //Integrate m_qdot
627 if (!m_finalized)
628 return;
629
630 // the new joint value have been computed already, just copy
631 memcpy(m_qKdl(0), m_newqKdl(0), sizeof(double)*m_qKdl.rows());
632 pushCache(timestamp);
633 updateJacobian();
634 // here update the desired output.
635 // We assume constant desired output for the joint limit constraint, no need to update it.
636 }
637
updateJacobian()638 void Armature::updateJacobian()
639 {
640 //calculate pose and jacobian
641 for (unsigned int ee=0; ee<m_nee; ee++) {
642 m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
643 m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
644 // get the jacobian for the base point, to prepare transformation to world reference
645 changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac);
646 //copy to Jq:
647 e_matrix& Jq = m_JqArray[ee];
648 for(unsigned int i=0;i<6;i++) {
649 for(unsigned int j=0;j<m_nq;j++)
650 Jq(i,j)=(*m_jac)(i,j);
651 }
652 }
653 // remember that this object has moved
654 m_updated = true;
655 }
656
getPose(const unsigned int ee)657 const Frame& Armature::getPose(const unsigned int ee)
658 {
659 if (!m_finalized)
660 return F_identity;
661 return (ee >= m_nee) ? F_identity : m_effectors[ee].pose;
662 }
663
getRelativeFrame(Frame & result,const std::string & segment_name,const std::string & base_name)664 bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name)
665 {
666 if (!m_finalized)
667 return false;
668 return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true;
669 }
670
updateControlOutput(const Timestamp & timestamp)671 void Armature::updateControlOutput(const Timestamp& timestamp)
672 {
673 if (!m_finalized)
674 return;
675
676
677 if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
678 popQ(timestamp.cacheTimestamp);
679 //popConstraints(timestamp.cacheTimestamp);
680 }
681
682 if (!timestamp.substep) {
683 // save previous joint state for getMaxJointChange()
684 memcpy(m_oldqKdl(0), m_qKdl(0), sizeof(double)*m_qKdl.rows());
685 for (unsigned int i=0; i<m_neffector; i++) {
686 m_effectors[i].oldpose = m_effectors[i].pose;
687 }
688 }
689
690 // remove all joint lock
691 for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) {
692 (*jit).locked = false;
693 }
694
695 JointConstraintList::iterator it;
696 unsigned int iConstraint;
697
698 // scan through the constraints and call the callback functions
699 for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
700 JointConstraint_struct* pConstraint = *it;
701 unsigned int nr, i;
702 for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
703 *(double *)&pConstraint->value[i].y = m_qKdl[nr];
704 *(double *)&pConstraint->value[i].ydot = m_qdotKdl[nr];
705 }
706 if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
707 (*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
708 }
709 // recompute the weight in any case, that's the most likely modification
710 for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) {
711 m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
712 m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y);
713 }
714 }
715 }
716
setControlParameter(unsigned int constraintId,unsigned int valueId,ConstraintAction action,double value,double timestep)717 bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep)
718 {
719 unsigned int lastid, i;
720 if (constraintId == CONSTRAINT_ID_ALL) {
721 constraintId = 0;
722 lastid = m_nconstraint;
723 } else if (constraintId < m_nconstraint) {
724 lastid = constraintId+1;
725 } else {
726 return false;
727 }
728 for ( ; constraintId<lastid; ++constraintId) {
729 JointConstraint_struct* pConstraint = m_constraints[constraintId];
730 if (valueId == ID_JOINT) {
731 for (i=0; i<pConstraint->v_nr; i++) {
732 switch (action) {
733 case ACT_TOLERANCE:
734 pConstraint->values[i].tolerance = value;
735 break;
736 case ACT_FEEDBACK:
737 pConstraint->values[i].feedback = value;
738 break;
739 case ACT_ALPHA:
740 pConstraint->values[i].alpha = value;
741 break;
742 default:
743 break;
744 }
745 }
746 } else {
747 for (i=0; i<pConstraint->v_nr; i++) {
748 if (valueId == pConstraint->value[i].id) {
749 switch (action) {
750 case ACT_VALUE:
751 pConstraint->value[i].yd = value;
752 break;
753 case ACT_VELOCITY:
754 pConstraint->value[i].yddot = value;
755 break;
756 case ACT_TOLERANCE:
757 pConstraint->values[i].tolerance = value;
758 break;
759 case ACT_FEEDBACK:
760 pConstraint->values[i].feedback = value;
761 break;
762 case ACT_ALPHA:
763 pConstraint->values[i].alpha = value;
764 break;
765 case ACT_NONE:
766 break;
767 }
768 }
769 }
770 }
771 if (m_finalized) {
772 for (i=0; i<pConstraint->v_nr; i++)
773 m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
774 }
775 }
776 return true;
777 }
778
779 }
780
781