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