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