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