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