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