Blender  V3.3
Scene.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Ruben Smits. */
3 
8 #include "Scene.hpp"
9 #include "ControlledObject.hpp"
11 #include <cstdio>
12 
13 namespace iTaSC {
14 
16  private:
17  Scene *m_scene;
18  Range m_qrange;
19 
20  public:
21  SceneLock(Scene *scene) : m_scene(scene), m_qrange(0, 0)
22  {
23  }
24  virtual ~SceneLock()
25  {
26  }
27 
28  void setRange(Range &range)
29  {
30  m_qrange = range;
31  }
32  // lock a joint, no need to update output
33  virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
34  {
35  q_nr += m_qrange.start;
36  project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
37  }
38  // lock a joint and update output in view of reiteration
39  virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot)
40  {
41  q_nr += m_qrange.start;
42  project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
43  // update the output vector so that the movement of this joint will be
44  // taken into account and we can put the joint back in its initial position
45  // which means that the jacobian doesn't need to be changed
46  for (unsigned int i = 0; i < ndof; ++i, ++q_nr) {
47  m_scene->m_ydot -= m_scene->m_A.col(q_nr) * qdot[i];
48  }
49  }
50 };
51 
53  : m_A(),
54  m_B(),
55  m_Atemp(),
56  m_Wq(),
57  m_Jf(),
58  m_Jq(),
59  m_Ju(),
60  m_Cf(),
61  m_Cq(),
62  m_Jf_inv(),
63  m_Vf(),
64  m_Uf(),
65  m_Wy(),
66  m_ydot(),
67  m_qdot(),
68  m_xdot(),
69  m_Sf(),
70  m_tempf(),
71  m_ncTotal(0),
72  m_nqTotal(0),
73  m_nuTotal(0),
74  m_nsets(0),
75  m_solver(NULL),
76  m_cache(NULL)
77 {
78  m_minstep = 0.01;
79  m_maxstep = 0.06;
80 }
81 
83 {
84  ConstraintMap::iterator constraint_it;
85  while ((constraint_it = constraints.begin()) != constraints.end()) {
86  delete constraint_it->second;
87  constraints.erase(constraint_it);
88  }
89  ObjectMap::iterator object_it;
90  while ((object_it = objects.begin()) != objects.end()) {
91  delete object_it->second;
92  objects.erase(object_it);
93  }
94 }
95 
96 bool Scene::setParam(SceneParam paramId, double value)
97 {
98  switch (paramId) {
99  case MIN_TIMESTEP:
100  m_minstep = value;
101  break;
102  case MAX_TIMESTEP:
103  m_maxstep = value;
104  break;
105  default:
106  return false;
107  }
108  return true;
109 }
110 
111 bool Scene::addObject(const std::string &name,
112  Object *object,
113  UncontrolledObject *base,
114  const std::string &baseFrame)
115 {
116  // finalize the object before adding
117  if (!object->finalize())
118  return false;
119  // Check if Object is controlled or uncontrolled.
120  if (object->getType() == Object::Controlled) {
121  int baseFrameIndex = base->addEndEffector(baseFrame);
122  if (baseFrameIndex < 0)
123  return false;
124  std::pair<ObjectMap::iterator, bool> result;
125  if (base->getNrOfCoordinates() == 0) {
126  // base is fixed object, no coordinate range
127  result = objects.insert(ObjectMap::value_type(
128  name,
129  new Object_struct(object,
130  base,
131  baseFrameIndex,
132  Range(m_nqTotal, object->getNrOfCoordinates()),
133  Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
134  Range(0, 0))));
135  }
136  else {
137  // base is a moving object, must be in list already
138  ObjectMap::iterator base_it;
139  for (base_it = objects.begin(); base_it != objects.end(); base_it++) {
140  if (base_it->second->object == base)
141  break;
142  }
143  if (base_it == objects.end())
144  return false;
145  result = objects.insert(ObjectMap::value_type(
146  name,
147  new Object_struct(object,
148  base,
149  baseFrameIndex,
150  Range(m_nqTotal, object->getNrOfCoordinates()),
151  Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
152  base_it->second->coordinaterange)));
153  }
154  if (!result.second) {
155  return false;
156  }
157  m_nqTotal += object->getNrOfCoordinates();
158  m_ncTotal += ((ControlledObject *)object)->getNrOfConstraints();
159  return true;
160  }
161  if (object->getType() == Object::UnControlled) {
162  if ((WorldObject *)base != &Object::world)
163  return false;
164  std::pair<ObjectMap::iterator, bool> result = objects.insert(
165  ObjectMap::value_type(name,
166  new Object_struct(object,
167  base,
168  0,
169  Range(0, 0),
170  Range(0, 0),
171  Range(m_nuTotal, object->getNrOfCoordinates()))));
172  if (!result.second)
173  return false;
174  m_nuTotal += object->getNrOfCoordinates();
175  return true;
176  }
177  return false;
178 }
179 
180 bool Scene::addConstraintSet(const std::string &name,
182  const std::string &object1,
183  const std::string &object2,
184  const std::string &ee1,
185  const std::string &ee2)
186 {
187  // Check if objects exist:
188  ObjectMap::iterator object1_it = objects.find(object1);
189  ObjectMap::iterator object2_it = objects.find(object2);
190  if (object1_it == objects.end() || object2_it == objects.end())
191  return false;
192  int ee1_index = object1_it->second->object->addEndEffector(ee1);
193  int ee2_index = object2_it->second->object->addEndEffector(ee2);
194  if (ee1_index < 0 || ee2_index < 0)
195  return false;
196  std::pair<ConstraintMap::iterator, bool> result = constraints.insert(ConstraintMap::value_type(
197  name,
198  new ConstraintSet_struct(task,
199  object1_it,
200  ee1_index,
201  object2_it,
202  ee2_index,
203  Range(m_ncTotal, task->getNrOfConstraints()),
204  Range(6 * m_nsets, 6))));
205  if (!result.second)
206  return false;
207  m_ncTotal += task->getNrOfConstraints();
208  m_nsets += 1;
209  return true;
210 }
211 
212 bool Scene::addSolver(Solver *_solver)
213 {
214  if (m_solver == NULL) {
215  m_solver = _solver;
216  return true;
217  }
218  else
219  return false;
220 }
221 
222 bool Scene::addCache(Cache *_cache)
223 {
224  if (m_cache == NULL) {
225  m_cache = _cache;
226  return true;
227  }
228  else
229  return false;
230 }
231 
233 {
234 
235  // prepare all matrices:
236  if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
237  return false;
238 
239  m_A = e_zero_matrix(m_ncTotal, m_nqTotal);
240  if (m_nuTotal > 0) {
241  m_B = e_zero_matrix(m_ncTotal, m_nuTotal);
242  m_xdot = e_zero_vector(m_nuTotal);
243  m_Ju = e_zero_matrix(6 * m_nsets, m_nuTotal);
244  }
245  m_Atemp = e_zero_matrix(m_ncTotal, 6 * m_nsets);
246  m_ydot = e_zero_vector(m_ncTotal);
247  m_qdot = e_zero_vector(m_nqTotal);
248  m_Wq = e_zero_matrix(m_nqTotal, m_nqTotal);
249  m_Wy = e_zero_vector(m_ncTotal);
250  m_Jq = e_zero_matrix(6 * m_nsets, m_nqTotal);
251  m_Jf = e_zero_matrix(6 * m_nsets, 6 * m_nsets);
252  m_Jf_inv = m_Jf;
253  m_Cf = e_zero_matrix(m_ncTotal, m_Jf.rows());
254  m_Cq = e_zero_matrix(m_ncTotal, m_nqTotal);
255 
256  bool result = true;
257  // finalize all objects
258  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
259  Object_struct *os = it->second;
260 
261  os->object->initCache(m_cache);
262  if (os->constraintrange.count > 0)
263  project(m_Cq,
264  os->constraintrange,
265  os->jointrange) = (((ControlledObject *)(os->object))->getCq());
266  }
267 
268  m_ytask.resize(m_ncTotal);
269  bool toggle = true;
270  int count = 0;
271  // Initialize all ConstraintSets:
272  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
273  // Calculate the external pose:
274  ConstraintSet_struct *cs = it->second;
275  Frame external_pose;
276  getConstraintPose(cs->task, cs, external_pose);
277  result &= cs->task->initialise(external_pose);
278  cs->task->initCache(m_cache);
279  for (int i = 0; i < cs->constraintrange.count; i++, count++) {
280  m_ytask[count] = toggle;
281  }
282  toggle = !toggle;
283  project(m_Cf, cs->constraintrange, cs->featurerange) = cs->task->getCf();
284  }
285 
286  if (m_solver != NULL)
287  m_solver->init(m_nqTotal, m_ncTotal, m_ytask);
288  else
289  return false;
290 
291  return result;
292 }
293 
294 bool Scene::getConstraintPose(ConstraintSet *constraint, void *_param, KDL::Frame &_pose)
295 {
296  // function called from constraint when they need to get the external pose
297  ConstraintSet_struct *cs = (ConstraintSet_struct *)_param;
298  // verification, the pointer MUST match
299  assert(constraint == cs->task);
300  Object_struct *ob1 = cs->object1->second;
301  Object_struct *ob2 = cs->object2->second;
302  // Calculate the external pose:
303  _pose =
304  (ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index)).Inverse() *
305  (ob2->base->getPose(ob2->baseFrameIndex) * ob2->object->getPose(cs->ee2index));
306  return true;
307 }
308 
309 bool Scene::update(double timestamp,
310  double timestep,
311  unsigned int numsubstep,
312  bool reiterate,
313  bool cache,
314  bool interpolate)
315 {
316  // we must have valid timestep and timestamp
317  if (timestamp < KDL::epsilon || timestep < 0.0)
318  return false;
319  Timestamp ts;
320  ts.realTimestamp = timestamp;
321  // initially we start with the full timestep to allow velocity estimation over the full interval
322  ts.realTimestep = timestep;
323  setCacheTimestamp(ts);
324  ts.substep = 0;
325  // for reiteration don't load cache
326  // reiteration=additional iteration with same timestamp if application finds the convergence not
327  // good enough
328  ts.reiterate = (reiterate) ? 1 : 0;
329  ts.interpolate = (interpolate) ? 1 : 0;
330  ts.cache = (cache) ? 1 : 0;
331  ts.update = 1;
332  ts.numstep = (numsubstep & 0xFF);
333  bool autosubstep = (numsubstep == 0) ? true : false;
334  if (numsubstep < 1)
335  numsubstep = 1;
336  double timesubstep = timestep / numsubstep;
337  double timeleft = timestep;
338 
339  if (timeleft == 0.0) {
340  // this special case correspond to a request to cache data
341  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
342  it->second->object->pushCache(ts);
343  }
344  // Update the Constraints
345  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
346  it->second->task->pushCache(ts);
347  }
348  return true;
349  }
350 
351  // double maxqdot; // UNUSED
352  e_scalar nlcoef;
353  SceneLock lockCallback(this);
354  Frame external_pose;
355  bool locked;
356 
357  // initially we keep timestep unchanged so that update function compute the velocity over
358  while (numsubstep > 0) {
359  // get objects
360  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
361  Object_struct *os = it->second;
362  if (os->object->getType() == Object::Controlled) {
363  ((ControlledObject *)(os->object))->updateControlOutput(ts);
364  if (os->constraintrange.count > 0) {
365  project(m_ydot,
366  os->constraintrange) = ((ControlledObject *)(os->object))->getControlOutput();
367  project(m_Wy, os->constraintrange) = ((ControlledObject *)(os->object))->getWy();
368  // project(m_Cq,os->constraintrange,os->jointrange) =
369  // (((ControlledObject*)(os->object))->getCq());
370  }
371  if (os->jointrange.count > 0) {
372  project(
373  m_Wq, os->jointrange, os->jointrange) = ((ControlledObject *)(os->object))->getWq();
374  }
375  }
376  if (os->object->getType() == Object::UnControlled &&
377  ((UncontrolledObject *)os->object)->getNrOfCoordinates() != 0) {
378  ((UncontrolledObject *)(os->object))->updateCoordinates(ts);
379  if (!ts.substep) {
380  // velocity of uncontrolled object remains constant during substepping
381  project(m_xdot, os->coordinaterange) = ((UncontrolledObject *)(os->object))->getXudot();
382  }
383  }
384  }
385 
386  // get new Constraints values
387  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
388  ConstraintSet_struct *cs = it->second;
389  Object_struct *ob1 = cs->object1->second;
390  Object_struct *ob2 = cs->object2->second;
391 
392  if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() ||
393  ob2->object->updated()) {
394  // the object from which the constraint depends have changed position
395  // recompute the constraint pose
396  getConstraintPose(cs->task, cs, external_pose);
397  cs->task->initialise(external_pose);
398  }
399  cs->task->updateControlOutput(ts);
400  project(m_ydot, cs->constraintrange) = cs->task->getControlOutput();
401  if (!ts.substep || cs->task->substep()) {
402  project(m_Wy, cs->constraintrange) = (cs->task)->getWy();
403  // project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
404  }
405 
406  project(m_Jf, cs->featurerange, cs->featurerange) = cs->task->getJf();
407  // std::cout << "Jf = " << Jf << std::endl;
408  // Transform the reference frame of this jacobian to the world reference frame
409  Eigen::Block<e_matrix> Jf_part = project(m_Jf, cs->featurerange, cs->featurerange);
410  changeBase(Jf_part,
411  ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index));
412  // std::cout << "Jf_w = " << Jf << std::endl;
413 
414  // calculate the inverse of Jf
416  project(m_Jf, cs->featurerange, cs->featurerange), m_Uf, m_Sf, m_Vf, m_tempf);
417  for (unsigned int i = 0; i < 6; ++i)
418  if (m_Sf(i) < KDL::epsilon)
419  m_Uf.col(i).setConstant(0.0);
420  else
421  m_Uf.col(i) *= (1 / m_Sf(i));
422  project(m_Jf_inv, cs->featurerange, cs->featurerange).noalias() = m_Vf * m_Uf.transpose();
423 
424  // Get the robotjacobian associated with this constraintset
425  // Each jacobian is expressed in robot base frame => convert to world reference
426  // and negate second robot because it is taken reversed when closing the loop:
427  if (ob1->object->getType() == Object::Controlled) {
428  project(m_Jq,
429  cs->featurerange,
430  ob1->jointrange) = (((ControlledObject *)(ob1->object))->getJq(cs->ee1index));
431  // Transform the reference frame of this jacobian to the world reference frame:
432  Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob1->jointrange);
433  changeBase(Jq_part, ob1->base->getPose(ob1->baseFrameIndex));
434  // if the base of this object is moving, get the Ju part
435  if (ob1->base->getNrOfCoordinates() != 0) {
436  // Ju is already computed for world reference frame
437  project(m_Ju, cs->featurerange, ob1->coordinaterange) = ob1->base->getJu(
438  ob1->baseFrameIndex);
439  }
440  }
441  else if (ob1->object->getType() == Object::UnControlled &&
442  ((UncontrolledObject *)ob1->object)->getNrOfCoordinates() != 0) {
443  // object1 is uncontrolled moving object
444  project(m_Ju,
445  cs->featurerange,
446  ob1->coordinaterange) = ((UncontrolledObject *)ob1->object)->getJu(cs->ee1index);
447  }
448  if (ob2->object->getType() == Object::Controlled) {
449  // Get the robotjacobian associated with this constraintset
450  // process a special case where object2 and object1 are equal but using different end
451  // effector
452  if (ob1->object == ob2->object) {
453  // we must create a temporary matrix
454  e_matrix JqTemp(((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
455  // Transform the reference frame of this jacobian to the world reference frame:
456  changeBase(JqTemp, ob2->base->getPose(ob2->baseFrameIndex));
457  // subtract in place
458  project(m_Jq, cs->featurerange, ob2->jointrange) -= JqTemp;
459  }
460  else {
461  project(m_Jq, cs->featurerange, ob2->jointrange) = -(
462  ((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
463  // Transform the reference frame of this jacobian to the world reference frame:
464  Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob2->jointrange);
465  changeBase(Jq_part, ob2->base->getPose(ob2->baseFrameIndex));
466  }
467  if (ob2->base->getNrOfCoordinates() != 0) {
468  // if base is the same as first object or first object base,
469  // that portion of m_Ju has been set already => subtract inplace
470  if (ob2->base == ob1->base || ob2->base == ob1->object) {
471  project(m_Ju, cs->featurerange, ob2->coordinaterange) -= ob2->base->getJu(
472  ob2->baseFrameIndex);
473  }
474  else {
475  project(m_Ju, cs->featurerange, ob2->coordinaterange) = -ob2->base->getJu(
476  ob2->baseFrameIndex);
477  }
478  }
479  }
480  else if (ob2->object->getType() == Object::UnControlled &&
481  ((UncontrolledObject *)ob2->object)->getNrOfCoordinates() != 0) {
482  if (ob2->object == ob1->base || ob2->object == ob1->object) {
483  project(m_Ju, cs->featurerange, ob2->coordinaterange) -=
484  ((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
485  }
486  else {
487  project(m_Ju, cs->featurerange, ob2->coordinaterange) =
488  -((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
489  }
490  }
491  }
492 
493  // Calculate A
494  m_Atemp.noalias() = m_Cf * m_Jf_inv;
495  m_A.noalias() = m_Cq - (m_Atemp * m_Jq);
496  if (m_nuTotal > 0) {
497  m_B.noalias() = m_Atemp * m_Ju;
498  m_ydot.noalias() += m_B * m_xdot;
499  }
500 
501  // Call the solver with A, Wq, Wy, ydot to solver qdot:
502  if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
503  // this should never happen
504  return false;
505  // send result to the objects
506  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
507  Object_struct *os = it->second;
508  if (os->object->getType() == Object::Controlled)
509  ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
510  }
511  // compute the constraint velocity
512  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
513  ConstraintSet_struct *cs = it->second;
514  Object_struct *ob1 = cs->object1->second;
515  Object_struct *ob2 = cs->object2->second;
516  // Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
517  e_vector6 external_vel = e_zero_vector(6);
518  if (ob1->jointrange.count > 0)
519  external_vel.noalias() += (project(m_Jq, cs->featurerange, ob1->jointrange) *
520  project(m_qdot, ob1->jointrange));
521  if (ob2->jointrange.count > 0)
522  external_vel.noalias() += (project(m_Jq, cs->featurerange, ob2->jointrange) *
523  project(m_qdot, ob2->jointrange));
524  if (ob1->coordinaterange.count > 0)
525  external_vel.noalias() += (project(m_Ju, cs->featurerange, ob1->coordinaterange) *
526  project(m_xdot, ob1->coordinaterange));
527  if (ob2->coordinaterange.count > 0)
528  external_vel.noalias() += (project(m_Ju, cs->featurerange, ob2->coordinaterange) *
529  project(m_xdot, ob2->coordinaterange));
530  // the twist caused by the constraint must be opposite because of the closed loop
531  // estimate the velocity of the joints using the inverse jacobian
532  e_vector6 estimated_chidot = project(m_Jf_inv, cs->featurerange, cs->featurerange) *
533  (-external_vel);
534  cs->task->setJointVelocity(estimated_chidot);
535  }
536 
537  if (autosubstep) {
538  // automatic computing of substep based on maximum joint change
539  // and joint limit gain variation
540  // We will pass the joint velocity to each object and they will recommend a maximum timestep
541  timesubstep = timeleft;
542  // get armature max joint velocity to estimate the maximum duration of integration
543  // maxqdot = m_qdot.cwise().abs().maxCoeff(); // UNUSED
544  double maxsubstep = nlcoef * m_maxstep;
545  if (maxsubstep < m_minstep)
546  maxsubstep = m_minstep;
547  if (timesubstep > maxsubstep)
548  timesubstep = maxsubstep;
549  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
550  Object_struct *os = it->second;
551  if (os->object->getType() == Object::Controlled)
552  ((ControlledObject *)(os->object))->getMaxTimestep(timesubstep);
553  }
554  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
555  ConstraintSet_struct *cs = it->second;
556  cs->task->getMaxTimestep(timesubstep);
557  }
558  // use substep that are even dividers of timestep for more regularity
559  maxsubstep = 2.0 * floor(timestep / 2.0 / timesubstep - 0.66666);
560  timesubstep = (maxsubstep < 0.0) ? timestep : timestep / (2.0 + maxsubstep);
561  if (timesubstep >= timeleft - (m_minstep / 2.0)) {
562  timesubstep = timeleft;
563  numsubstep = 1;
564  timeleft = 0.;
565  }
566  else {
567  numsubstep = 2;
568  timeleft -= timesubstep;
569  }
570  }
571  if (numsubstep > 1) {
572  ts.substep = 1;
573  }
574  else {
575  // set substep to false for last iteration so that controlled output
576  // can be updated in updateKinematics() and model_update)() before next call to
577  // Secne::update()
578  ts.substep = 0;
579  }
580  // change timestep so that integration is done correctly
581  ts.realTimestep = timesubstep;
582 
583  do {
584  ObjectMap::iterator it;
585  Object_struct *os;
586  locked = false;
587  for (it = objects.begin(); it != objects.end(); ++it) {
588  os = it->second;
589  if (os->object->getType() == Object::Controlled) {
590  lockCallback.setRange(os->jointrange);
591  if (((ControlledObject *)os->object)->updateJoint(ts, lockCallback)) {
592  // this means one of the joint was locked and we must rerun
593  // the solver to update the remaining joints
594  locked = true;
595  break;
596  }
597  }
598  }
599  if (locked) {
600  // Some rows of m_Wq have been cleared so that the corresponding joint will not move
601  if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
602  // this should never happen
603  return false;
604 
605  // send result to the objects
606  for (it = objects.begin(); it != objects.end(); ++it) {
607  os = it->second;
608  if (os->object->getType() == Object::Controlled)
609  ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
610  }
611  }
612  } while (locked);
613 
614  // Update the Objects
615  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
616  it->second->object->updateKinematics(ts);
617  // mark this object not updated since the constraint will be updated anyway
618  // this flag is only useful to detect external updates
619  it->second->object->updated(false);
620  }
621  // Update the Constraints
622  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
623  ConstraintSet_struct *cs = it->second;
624  // Calculate the external pose:
625  getConstraintPose(cs->task, cs, external_pose);
626  cs->task->modelUpdate(external_pose, ts);
627  // update the constraint output and cache
628  cs->task->updateKinematics(ts);
629  }
630  numsubstep--;
631  }
632  return true;
633 }
634 
635 } // namespace iTaSC
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
virtual const unsigned int getNrOfCoordinates()
Definition: Object.hpp:39
virtual bool finalize()
Definition: Object.hpp:36
static WorldObject world
Definition: Object.hpp:22
virtual const ObjectType getType()
Definition: Object.hpp:38
virtual int addEndEffector(const std::string &name)
Definition: Object.hpp:35
SceneLock(Scene *scene)
Definition: Scene.cpp:21
virtual ~SceneLock()
Definition: Scene.cpp:24
virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot)
Definition: Scene.cpp:39
void setRange(Range &range)
Definition: Scene.cpp:28
virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
Definition: Scene.cpp:33
bool addSolver(Solver *_solver)
Definition: Scene.cpp:212
bool setParam(SceneParam paramId, double value)
Definition: Scene.cpp:96
bool addConstraintSet(const std::string &name, ConstraintSet *task, const std::string &object1, const std::string &object2, const std::string &ee1="", const std::string &ee2="")
Definition: Scene.cpp:180
virtual ~Scene()
Definition: Scene.cpp:82
@ MIN_TIMESTEP
Definition: Scene.hpp:27
@ MAX_TIMESTEP
Definition: Scene.hpp:28
bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true)
Definition: Scene.cpp:309
bool addCache(Cache *_cache)
Definition: Scene.cpp:222
bool initialize()
Definition: Scene.cpp:232
bool addObject(const std::string &name, Object *object, UncontrolledObject *base=&Object::world, const std::string &baseFrame="")
Definition: Scene.cpp:111
virtual bool init(unsigned int nq, unsigned int nc, const std::vector< bool > &gc)=0
virtual bool solve(const e_matrix &A, const e_vector &Wy, const e_vector &ydot, const e_matrix &Wq, e_vector &qdot, e_scalar &nlcoef)=0
virtual const unsigned int getNrOfCoordinates()
Scene scene
#define e_vector6
Definition: eigen_types.hpp:46
#define e_scalar
Definition: eigen_types.hpp:37
#define e_zero_vector
Definition: eigen_types.hpp:39
#define e_zero_matrix
Definition: eigen_types.hpp:44
#define e_matrix
Definition: eigen_types.hpp:40
int count
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
int svd_eigen_HH(const Eigen::MatrixBase< MatrixA > &A, Eigen::MatrixBase< MatrixUV > &U, Eigen::MatrixBase< VectorS > &S, Eigen::MatrixBase< MatrixUV > &V, Eigen::MatrixBase< VectorS > &tmp, int maxiter=150)
struct blender::compositor::@179::@181 task
void interpolate(const Span< T > src, const Span< int > indices, const Span< float > factors, MutableSpan< T > dst)
T floor(const T &a)
void setCacheTimestamp(Timestamp &timestamp)
Definition: Cache.hpp:51
static int changeBase(Eigen::MatrixBase< Derived > &J, const Frame &T)
Definition: eigen_types.hpp:67
Eigen::Block< MatrixType > project(MatrixType &m, Range r)
Definition: eigen_types.hpp:57
unsigned int interpolate
Definition: Cache.hpp:44
double realTimestep
Definition: Cache.hpp:37
unsigned int cache
Definition: Cache.hpp:42
double realTimestamp
Definition: Cache.hpp:36
unsigned int reiterate
Definition: Cache.hpp:41
unsigned int substep
Definition: Cache.hpp:40
unsigned int numstep
Definition: Cache.hpp:39
unsigned int update
Definition: Cache.hpp:43