doxygen: add newline after \file
[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 #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
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
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
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
132 Armature::JointConstraint_struct::~JointConstraint_struct()
133 {
134         if (freeParam && param)
135                 free(param);
136 }
137
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
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 */
171 bool Armature::popQ(CacheTS timestamp)
172 {
173         if (m_qCCh >= 0) {
174                 double* item;
175                 item = (double *)m_cache->getPreviousCacheItem(this, m_qCCh, &timestamp);
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, &timestamp);
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
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
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
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
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
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
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
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
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
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
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
441 const KDL::JntArray& Armature::getJointArray()
442 {
443         return m_qKdl;
444 }
445
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
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
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
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
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
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
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