46d114e6c3f1fe860ef60eb4b7a91b31186f01e0
[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 #include <stdlib.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_armlength(0.0),
39         m_jacsolver(NULL),
40         m_fksolver(NULL)
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         case Joint::None:
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         Joint_struct& p_joint = m_joints[segment_it->second.q_nr+dof];
344         p_joint.min = _min;
345         p_joint.max = _max;
346         p_joint.useLimit = true;
347         return 0;
348 }
349
350 int Armature::addEndEffector(const std::string& name)
351 {
352         const SegmentMap& segments = m_tree.getSegments();
353         if (segments.find(name) == segments.end())
354                 return -1;
355
356         EffectorList::const_iterator it;
357         int ee;
358         for (it=m_effectors.begin(), ee=0; it!=m_effectors.end(); it++, ee++) {
359                 if (it->name == name)
360                         return ee;
361         }
362         if (m_finalized)
363                 return -1;
364         Effector_struct effector(name);
365         m_effectors.push_back(effector);
366         return m_neffector++;
367 }
368
369 void Armature::finalize()
370 {
371         unsigned int i, j, c;
372         if (m_finalized)
373                 return;
374         initialize(m_njoint, m_noutput, m_neffector);
375         for (i=c=0; i<m_nconstraint; i++) {
376                 JointConstraint_struct* pConstraint = m_constraints[i];
377                 for (j=0; j<pConstraint->v_nr; j++, c++) {
378                         m_Cq(c,pConstraint->segment->second.q_nr+j) = 1.0;
379                         m_Wy(c) = pConstraint->values[j].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
380                 }
381         }
382         m_jacsolver= new KDL::TreeJntToJacSolver(m_tree);
383         m_fksolver = new KDL::TreeFkSolverPos_recursive(m_tree);
384         m_jac = new Jacobian(m_njoint);
385         m_qKdl.resize(m_njoint);
386         m_oldqKdl.resize(m_njoint);
387         m_newqKdl.resize(m_njoint);
388         m_qdotKdl.resize(m_njoint);
389         for (i=0; i<m_njoint; i++) {
390                 m_newqKdl(i) = m_oldqKdl(i) = m_qKdl(i) = m_joints[i].rest;
391         }
392         updateJacobian();
393         // estimate the maximum size of the robot arms
394         double length;
395         m_armlength = 0.0;
396         for (i=0; i<m_neffector; i++) {
397                 length = 0.0;
398                 KDL::SegmentMap::const_iterator sit = m_tree.getSegment(m_effectors[i].name);
399                 while (sit->first != "root") {
400                         Frame tip = sit->second.segment.pose(m_qKdl(sit->second.q_nr));
401                         length += tip.p.Norm();
402                         sit = sit->second.parent;
403                 }
404                 if (length > m_armlength)
405                         m_armlength = length;
406         }
407         if (m_armlength < KDL::epsilon)
408                 m_armlength = KDL::epsilon;
409         m_finalized = true;
410 }
411
412 void Armature::pushCache(const Timestamp& timestamp)
413 {
414         if (!timestamp.substep && timestamp.cache) {
415                 pushQ(timestamp.cacheTimestamp);
416                 //pushConstraints(timestamp.cacheTimestamp);
417         }
418 }
419
420 bool Armature::setJointArray(const KDL::JntArray& joints)
421 {
422         if (!m_finalized)
423                 return false;
424         if (joints.rows() != m_qKdl.rows())
425                 return false;
426         m_qKdl = joints;
427         updateJacobian();
428         return true;
429 }
430
431 const KDL::JntArray& Armature::getJointArray()
432 {
433         return m_qKdl;
434 }
435
436 bool Armature::updateJoint(const Timestamp& timestamp, JointLockCallback& callback)
437 {
438         if (!m_finalized)
439                 return false;
440         
441         // integration and joint limit
442         // for spherical joint we must use a more sophisticated method
443         unsigned int q_nr;
444         double* qdot=&m_qdotKdl(0);
445         double* q=&m_qKdl(0);
446         double* newq=&m_newqKdl(0);
447         double norm, qx, qz, CX, CZ, sx, sz;
448         bool locked = false;
449         int unlocked = 0;
450
451         for (q_nr=0; q_nr<m_nq; ++q_nr)
452                 m_qdotKdl(q_nr)=m_qdot(q_nr);
453
454         for (q_nr=0; q_nr<m_nq; ) {
455                 Joint_struct* joint = &m_joints[q_nr];
456                 if (!joint->locked) {
457                         switch (joint->type) {
458                         case KDL::Joint::Swing:
459                         {
460                                 KDL::Rotation base = KDL::Rot(KDL::Vector(q[0],0.0,q[1]));
461                                 (base*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(newq);
462                                 if (joint[0].useLimit) {
463                                         if (joint[1].useLimit) {
464                                                 // elliptical limit
465                                                 sx = sz = 1.0;
466                                                 qx = newq[0];
467                                                 qz = newq[1];
468                                                 // determine in which quadrant we are
469                                                 if (qx > 0.0 && qz > 0.0) {
470                                                         CX = joint[0].max;
471                                                         CZ = joint[1].max;
472                                                 } else if (qx <= 0.0 && qz > 0.0) {
473                                                         CX = -joint[0].min;
474                                                         CZ = joint[1].max;
475                                                         qx = -qx;
476                                                         sx = -1.0;
477                                                 } else if (qx <= 0.0 && qz <= 0.0) {
478                                                         CX = -joint[0].min;
479                                                         CZ = -joint[1].min;
480                                                         qx = -qx;
481                                                         qz = -qz;
482                                                         sx = sz = -1.0;
483                                                 } else {
484                                                         CX = joint[0].max;
485                                                         CZ = -joint[0].min;
486                                                         qz = -qz;
487                                                         sz = -1.0;
488                                                 }
489                                                 if (CX < KDL::epsilon || CZ < KDL::epsilon) {
490                                                         // quadrant is degenerated
491                                                         if (qx > CX) {
492                                                                 newq[0] = CX*sx;
493                                                                 joint[0].locked = true;
494                                                         }
495                                                         if (qz > CZ) {
496                                                                 newq[1] = CZ*sz;
497                                                                 joint[0].locked = true;
498                                                         }
499                                                 } else {
500                                                         // general case
501                                                         qx /= CX;
502                                                         qz /= CZ;
503                                                         norm = KDL::sqrt(KDL::sqr(qx)+KDL::sqr(qz));
504                                                         if (norm > 1.0) {
505                                                                 norm = 1.0/norm;
506                                                                 newq[0] = qx*norm*CX*sx;
507                                                                 newq[1] = qz*norm*CZ*sz;
508                                                                 joint[0].locked = true;
509                                                         }
510                                                 }
511                                         } else {
512                                                 // limit on X only
513                                                 qx = newq[0];
514                                                 if (qx > joint[0].max) {
515                                                         newq[0] = joint[0].max;
516                                                         joint[0].locked = true;
517                                                 } else if (qx < joint[0].min) {
518                                                         newq[0] = joint[0].min;
519                                                         joint[0].locked = true;
520                                                 }
521                                         }
522                                 } else if (joint[1].useLimit) {
523                                         // limit on Z only
524                                         qz = newq[1];
525                                         if (qz > joint[1].max) {
526                                                 newq[1] = joint[1].max;
527                                                 joint[0].locked = true;
528                                         } else if (qz < joint[1].min) {
529                                                 newq[1] = joint[1].min;
530                                                 joint[0].locked = true;
531                                         }
532                                 }
533                                 if (joint[0].locked) {
534                                         // check the difference from previous position
535                                         locked = true;
536                                         norm = KDL::sqr(newq[0]-q[0])+KDL::sqr(newq[1]-q[1]);
537                                         if (norm < KDL::epsilon2) {
538                                                 // joint didn't move, no need to update the jacobian
539                                                 callback.lockJoint(q_nr, 2);
540                                         } else {
541                                                 // joint moved, compute the corresponding velocity
542                                                 double deltaq[2];
543                                                 (base.Inverse()*KDL::Rot(KDL::Vector(newq[0],0.0,newq[1]))).GetXZRot().GetValue(deltaq);
544                                                 deltaq[0] /= timestamp.realTimestep;
545                                                 deltaq[1] /= timestamp.realTimestep;
546                                                 callback.lockJoint(q_nr, 2, deltaq);
547                                                 // no need to update the other joints, it will be done after next rerun
548                                                 goto end_loop;
549                                         }
550                                 } else
551                                         unlocked++;
552                                 break;
553                         }
554                         case KDL::Joint::Sphere:
555                         {
556                                 (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
557                                 // no limit on this joint
558                                 unlocked++;
559                                 break;
560                         }
561                         default:
562                                 for (unsigned int i=0; i<joint->ndof; i++) {
563                                         newq[i] = q[i]+qdot[i]*timestamp.realTimestep;
564                                         if (joint[i].useLimit) {
565                                                 if (newq[i] > joint[i].max) {
566                                                         newq[i] = joint[i].max;
567                                                         joint[0].locked = true;
568                                                 } else if (newq[i] < joint[i].min) {
569                                                         newq[i] = joint[i].min;
570                                                         joint[0].locked = true;
571                                                 }
572                                         }
573                                 }
574                                 if (joint[0].locked) {
575                                         locked = true;
576                                         norm = 0.0;
577                                         // compute delta to locked position
578                                         for (unsigned int i=0; i<joint->ndof; i++) {
579                                                 qdot[i] = newq[i] - q[i];
580                                                 norm += qdot[i]*qdot[i];
581                                         }
582                                         if (norm < KDL::epsilon2) {
583                                                 // joint didn't move, no need to update the jacobian
584                                                 callback.lockJoint(q_nr, joint->ndof);
585                                         } else {
586                                                 // solver needs velocity, compute equivalent velocity
587                                                 for (unsigned int i=0; i<joint->ndof; i++) {
588                                                         qdot[i] /= timestamp.realTimestep;
589                                                 }
590                                                 callback.lockJoint(q_nr, joint->ndof, qdot);
591                                                 goto end_loop;
592                                         }
593                                 } else 
594                                         unlocked++;
595                         }
596                 }
597                 qdot += joint->ndof;
598                 q    += joint->ndof;
599                 newq += joint->ndof;
600                 q_nr += joint->ndof;
601         }
602 end_loop:
603         // check if there any other unlocked joint
604         for ( ; q_nr<m_nq; ) {
605                 Joint_struct* joint = &m_joints[q_nr];
606                 if (!joint->locked)
607                         unlocked++;
608                 q_nr += joint->ndof;
609         }
610         // if all joints have been locked no need to run the solver again
611         return (unlocked) ? locked : false;
612 }
613
614 void Armature::updateKinematics(const Timestamp& timestamp){
615
616     //Integrate m_qdot
617         if (!m_finalized)
618                 return;
619
620         // the new joint value have been computed already, just copy
621         memcpy(&m_qKdl(0), &m_newqKdl(0), sizeof(double)*m_qKdl.rows());
622         pushCache(timestamp);
623         updateJacobian();
624         // here update the desired output.
625         // We assume constant desired output for the joint limit constraint, no need to update it.
626 }
627
628 void Armature::updateJacobian()
629 {
630     //calculate pose and jacobian
631         for (unsigned int ee=0; ee<m_nee; ee++) {
632                 m_fksolver->JntToCart(m_qKdl,m_effectors[ee].pose,m_effectors[ee].name,m_root);
633                 m_jacsolver->JntToJac(m_qKdl,*m_jac,m_effectors[ee].name);
634                 // get the jacobian for the base point, to prepare transformation to world reference
635                 changeRefPoint(*m_jac,-m_effectors[ee].pose.p,*m_jac);
636                 //copy to Jq:
637                 e_matrix& Jq = m_JqArray[ee];
638                 for(unsigned int i=0;i<6;i++) {
639                         for(unsigned int j=0;j<m_nq;j++)
640                                 Jq(i,j)=(*m_jac)(i,j);
641                 }
642         }
643         // remember that this object has moved 
644         m_updated = true;
645 }
646
647 const Frame& Armature::getPose(const unsigned int ee)
648 {
649         if (!m_finalized)
650                 return F_identity;
651         return (ee >= m_nee) ? F_identity : m_effectors[ee].pose;
652 }
653
654 bool Armature::getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name)
655 {
656         if (!m_finalized)
657                 return false;
658         return (m_fksolver->JntToCart(m_qKdl,result,segment_name,base_name) < 0) ? false : true;
659 }
660
661 void Armature::updateControlOutput(const Timestamp& timestamp)
662 {
663         if (!m_finalized)
664                 return;
665
666
667         if (!timestamp.substep && !timestamp.reiterate && timestamp.interpolate) {
668                 popQ(timestamp.cacheTimestamp);
669                 //popConstraints(timestamp.cacheTimestamp);
670         }
671
672         if (!timestamp.substep) {
673                 // save previous joint state for getMaxJointChange()
674                 memcpy(&m_oldqKdl(0), &m_qKdl(0), sizeof(double)*m_qKdl.rows());
675                 for (unsigned int i=0; i<m_neffector; i++) {
676                         m_effectors[i].oldpose = m_effectors[i].pose;
677                 }
678         }
679
680         // remove all joint lock
681         for (JointList::iterator jit=m_joints.begin(); jit!=m_joints.end(); ++jit) {
682                 (*jit).locked = false;
683         }
684
685         JointConstraintList::iterator it;
686         unsigned int iConstraint;
687
688         // scan through the constraints and call the callback functions
689         for (iConstraint=0, it=m_constraints.begin(); it!=m_constraints.end(); it++, iConstraint++) {
690                 JointConstraint_struct* pConstraint = *it;
691                 unsigned int nr, i;
692                 for (i=0, nr = pConstraint->segment->second.q_nr; i<pConstraint->v_nr; i++, nr++) {
693                         *(double*)&pConstraint->value[i].y = m_qKdl(nr);
694                         *(double*)&pConstraint->value[i].ydot = m_qdotKdl(nr);
695                 }
696                 if (pConstraint->function && (pConstraint->substep || (!timestamp.reiterate && !timestamp.substep))) {
697                         (*pConstraint->function)(timestamp, pConstraint->values, pConstraint->v_nr, pConstraint->param);
698                 }
699                 // recompute the weight in any case, that's the most likely modification
700                 for (i=0, nr=pConstraint->y_nr; i<pConstraint->v_nr; i++, nr++) {
701                         m_Wy(nr) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
702                         m_ydot(nr)=pConstraint->value[i].yddot+pConstraint->values[i].feedback*(pConstraint->value[i].yd-pConstraint->value[i].y);
703                 }
704         }
705 }
706
707 bool Armature::setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep)
708 {
709         unsigned int lastid, i;
710         if (constraintId == CONSTRAINT_ID_ALL) {
711                 constraintId = 0;
712                 lastid = m_nconstraint;
713         } else if (constraintId < m_nconstraint) {
714                 lastid = constraintId+1;
715         } else {
716                 return false;
717         }
718         for ( ; constraintId<lastid; ++constraintId) {
719                 JointConstraint_struct* pConstraint = m_constraints[constraintId];
720                 if (valueId == ID_JOINT) {
721                         for (i=0; i<pConstraint->v_nr; i++) {
722                                 switch (action) {
723                                 case ACT_TOLERANCE:
724                                         pConstraint->values[i].tolerance = value;
725                                         break;
726                                 case ACT_FEEDBACK:
727                                         pConstraint->values[i].feedback = value;
728                                         break;
729                                 case ACT_ALPHA:
730                                         pConstraint->values[i].alpha = value;
731                                         break;
732                                 default:
733                                         break;
734                                 }
735                         }
736                 } else {
737                         for (i=0; i<pConstraint->v_nr; i++) {
738                                 if (valueId == pConstraint->value[i].id) {
739                                         switch (action) {
740                                         case ACT_VALUE:
741                                                 pConstraint->value[i].yd = value;
742                                                 break;
743                                         case ACT_VELOCITY:
744                                                 pConstraint->value[i].yddot = value;
745                                                 break;
746                                         case ACT_TOLERANCE:
747                                                 pConstraint->values[i].tolerance = value;
748                                                 break;
749                                         case ACT_FEEDBACK:
750                                                 pConstraint->values[i].feedback = value;
751                                                 break;
752                                         case ACT_ALPHA:
753                                                 pConstraint->values[i].alpha = value;
754                                                 break;
755                                         case ACT_NONE:
756                                                 break;
757                                         }
758                                 }
759                         }
760                 }
761                 if (m_finalized) {
762                         for (i=0; i<pConstraint->v_nr; i++) 
763                                 m_Wy(pConstraint->y_nr+i) = pConstraint->values[i].alpha/*/(pConstraint->values.tolerance*pConstraint->values.feedback)*/;
764                 }
765         }
766         return true;
767 }
768
769 }
770