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