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