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