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