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