Cleanup: typos & co in UI messages (and some other places).

2 files changed, 586 insertions, 495 deletions

diff --git a/intern/cycles/util/util_math_fast.h b/intern/cycles/util/util_math_fast.h
index e979bd9e0c0..4f92d2f10d3 100644
--- a/intern/cycles/util/util_math_fast.h
+++ b/intern/cycles/util/util_math_fast.h
@@ -87,7 +87,7 @@ ccl_device_inline int fast_rint(float x)
   /* Single roundps instruction on SSE4.1+ (for gcc/clang at least). */
   return float_to_int(rintf(x));
 #else
-  /* emulate rounding by adding/substracting 0.5. */
+  /* emulate rounding by adding/subtracting 0.5. */
   return float_to_int(x + copysignf(0.5f, x));
 #endif
 }
diff --git a/intern/itasc/Scene.cpp b/intern/itasc/Scene.cpp
index 5438a005d7c..0d2486ceac7 100644
--- a/intern/itasc/Scene.cpp
+++ b/intern/itasc/Scene.cpp
@@ -16,532 +16,623 @@
 namespace iTaSC {
 
 class SceneLock : public ControlledObject::JointLockCallback {
-private:
-	Scene* m_scene;
-	Range  m_qrange;
-
-public:
-	SceneLock(Scene* scene) :
-	  m_scene(scene), m_qrange(0,0) {}
-	virtual ~SceneLock() {}
-
-	void setRange(Range& range)
-	{
-		m_qrange = range;
-	}
-	// lock a joint, no need to update output
-	virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
-	{
-		q_nr += m_qrange.start;
-		project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
-	}
-	// lock a joint and update output in view of reiteration
-	virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double* qdot)
-	{
-		q_nr += m_qrange.start;
-		project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
-		// update the output vector so that the movement of this joint will be
-		// taken into account and we can put the joint back in its initial position
-		// which means that the jacobian doesn't need to be changed
-		for (unsigned int i=0 ;i<ndof ; ++i, ++q_nr) {
-			m_scene->m_ydot -= m_scene->m_A.col(q_nr)*qdot[i];
-		}
-	}
+ private:
+  Scene *m_scene;
+  Range m_qrange;
+
+ public:
+  SceneLock(Scene *scene) : m_scene(scene), m_qrange(0, 0)
+  {
+  }
+  virtual ~SceneLock()
+  {
+  }
+
+  void setRange(Range &range)
+  {
+    m_qrange = range;
+  }
+  // lock a joint, no need to update output
+  virtual void lockJoint(unsigned int q_nr, unsigned int ndof)
+  {
+    q_nr += m_qrange.start;
+    project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+  }
+  // lock a joint and update output in view of reiteration
+  virtual void lockJoint(unsigned int q_nr, unsigned int ndof, double *qdot)
+  {
+    q_nr += m_qrange.start;
+    project(m_scene->m_Wq, Range(q_nr, ndof), m_qrange).setZero();
+    // update the output vector so that the movement of this joint will be
+    // taken into account and we can put the joint back in its initial position
+    // which means that the jacobian doesn't need to be changed
+    for (unsigned int i = 0; i < ndof; ++i, ++q_nr) {
+      m_scene->m_ydot -= m_scene->m_A.col(q_nr) * qdot[i];
+    }
+  }
 };
 
-Scene::Scene():
-	m_A(), m_B(), m_Atemp(), m_Wq(), m_Jf(), m_Jq(), m_Ju(), m_Cf(), m_Cq(), m_Jf_inv(),
-	m_Vf(),m_Uf(), m_Wy(), m_ydot(), m_qdot(), m_xdot(), m_Sf(),m_tempf(),
-	m_ncTotal(0),m_nqTotal(0),m_nuTotal(0),m_nsets(0),
-	m_solver(NULL),m_cache(NULL) 
+Scene::Scene()
+    : m_A(),
+      m_B(),
+      m_Atemp(),
+      m_Wq(),
+      m_Jf(),
+      m_Jq(),
+      m_Ju(),
+      m_Cf(),
+      m_Cq(),
+      m_Jf_inv(),
+      m_Vf(),
+      m_Uf(),
+      m_Wy(),
+      m_ydot(),
+      m_qdot(),
+      m_xdot(),
+      m_Sf(),
+      m_tempf(),
+      m_ncTotal(0),
+      m_nqTotal(0),
+      m_nuTotal(0),
+      m_nsets(0),
+      m_solver(NULL),
+      m_cache(NULL)
 {
-	m_minstep = 0.01;
-	m_maxstep = 0.06;
+  m_minstep = 0.01;
+  m_maxstep = 0.06;
 }
 
-Scene::~Scene() 
+Scene::~Scene()
 {
-	ConstraintMap::iterator constraint_it;
-	while ((constraint_it = constraints.begin()) != constraints.end()) {
-		delete constraint_it->second;
-		constraints.erase(constraint_it);
-	}
-	ObjectMap::iterator object_it;
-	while ((object_it = objects.begin()) != objects.end()) {
-		delete object_it->second;
-		objects.erase(object_it);
-	}
+  ConstraintMap::iterator constraint_it;
+  while ((constraint_it = constraints.begin()) != constraints.end()) {
+    delete constraint_it->second;
+    constraints.erase(constraint_it);
+  }
+  ObjectMap::iterator object_it;
+  while ((object_it = objects.begin()) != objects.end()) {
+    delete object_it->second;
+    objects.erase(object_it);
+  }
 }
 
 bool Scene::setParam(SceneParam paramId, double value)
 {
-	switch (paramId) {
-	case MIN_TIMESTEP:
-		m_minstep = value;
-		break;
-	case MAX_TIMESTEP:		
-		m_maxstep = value;
-		break;
-	default:
-		return false;
-	}
-	return true;
+  switch (paramId) {
+    case MIN_TIMESTEP:
+      m_minstep = value;
+      break;
+    case MAX_TIMESTEP:
+      m_maxstep = value;
+      break;
+    default:
+      return false;
+  }
+  return true;
 }
 
-bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame)
+bool Scene::addObject(const std::string &name,
+                      Object *object,
+                      UncontrolledObject *base,
+                      const std::string &baseFrame)
 {
-	// finalize the object before adding
-	if (!object->finalize())
-		return false;
-    //Check if Object is controlled or uncontrolled.
-    if(object->getType()==Object::Controlled){
-		int baseFrameIndex = base->addEndEffector(baseFrame);
-		if (baseFrameIndex < 0)
-			return false;
-		std::pair<ObjectMap::iterator, bool> result;
-		if (base->getNrOfCoordinates() == 0) {
-			// base is fixed object, no coordinate range
-			result = objects.insert(ObjectMap::value_type(
-					name, new Object_struct(object,base,baseFrameIndex,
-						Range(m_nqTotal,object->getNrOfCoordinates()),
-						Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
-						Range(0,0))));
-		} else {
-			// base is a moving object, must be in list already
-		    ObjectMap::iterator base_it;
-			for (base_it=objects.begin(); base_it != objects.end(); base_it++) {
-				if (base_it->second->object == base)
-					break;
-			}
-			if (base_it == objects.end())
-				return false;
-			result = objects.insert(ObjectMap::value_type(
-					name, new Object_struct(object,base,baseFrameIndex,
-						Range(m_nqTotal,object->getNrOfCoordinates()),
-						Range(m_ncTotal,((ControlledObject*)object)->getNrOfConstraints()),
-						base_it->second->coordinaterange)));
-		}
-		if (!result.second) {
-			return false;
-		}
-        m_nqTotal+=object->getNrOfCoordinates();
-        m_ncTotal+=((ControlledObject*)object)->getNrOfConstraints();
-        return true;
+  // finalize the object before adding
+  if (!object->finalize())
+    return false;
+  // Check if Object is controlled or uncontrolled.
+  if (object->getType() == Object::Controlled) {
+    int baseFrameIndex = base->addEndEffector(baseFrame);
+    if (baseFrameIndex < 0)
+      return false;
+    std::pair<ObjectMap::iterator, bool> result;
+    if (base->getNrOfCoordinates() == 0) {
+      // base is fixed object, no coordinate range
+      result = objects.insert(ObjectMap::value_type(
+          name,
+          new Object_struct(object,
+                            base,
+                            baseFrameIndex,
+                            Range(m_nqTotal, object->getNrOfCoordinates()),
+                            Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
+                            Range(0, 0))));
+    }
+    else {
+      // base is a moving object, must be in list already
+      ObjectMap::iterator base_it;
+      for (base_it = objects.begin(); base_it != objects.end(); base_it++) {
+        if (base_it->second->object == base)
+          break;
+      }
+      if (base_it == objects.end())
+        return false;
+      result = objects.insert(ObjectMap::value_type(
+          name,
+          new Object_struct(object,
+                            base,
+                            baseFrameIndex,
+                            Range(m_nqTotal, object->getNrOfCoordinates()),
+                            Range(m_ncTotal, ((ControlledObject *)object)->getNrOfConstraints()),
+                            base_it->second->coordinaterange)));
     }
-    if(object->getType()==Object::UnControlled){
-		if ((WorldObject*)base != &Object::world)
-			return false;
-		std::pair<ObjectMap::iterator,bool> result = objects.insert(ObjectMap::value_type(
-			name,new Object_struct(object,base,0,
-						Range(0,0),
-						Range(0,0),
-						Range(m_nuTotal,object->getNrOfCoordinates()))));
-        if(!result.second)
-            return false;
-        m_nuTotal+=object->getNrOfCoordinates();
-        return true;
+    if (!result.second) {
+      return false;
     }
-    return false;
+    m_nqTotal += object->getNrOfCoordinates();
+    m_ncTotal += ((ControlledObject *)object)->getNrOfConstraints();
+    return true;
+  }
+  if (object->getType() == Object::UnControlled) {
+    if ((WorldObject *)base != &Object::world)
+      return false;
+    std::pair<ObjectMap::iterator, bool> result = objects.insert(
+        ObjectMap::value_type(name,
+                              new Object_struct(object,
+                                                base,
+                                                0,
+                                                Range(0, 0),
+                                                Range(0, 0),
+                                                Range(m_nuTotal, object->getNrOfCoordinates()))));
+    if (!result.second)
+      return false;
+    m_nuTotal += object->getNrOfCoordinates();
+    return true;
+  }
+  return false;
 }
 
-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)
+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)
 {
-	//Check if objects exist:
-	ObjectMap::iterator object1_it = objects.find(object1);
-	ObjectMap::iterator object2_it = objects.find(object2);
-	if(object1_it==objects.end()||object2_it==objects.end())
-		return false;
-	int ee1_index = object1_it->second->object->addEndEffector(ee1);
-	int ee2_index = object2_it->second->object->addEndEffector(ee2);
-	if (ee1_index < 0 || ee2_index < 0)
-		return false;
-	std::pair<ConstraintMap::iterator,bool> result = 
-		constraints.insert(ConstraintMap::value_type(name,new ConstraintSet_struct(
-			task,object1_it,ee1_index,object2_it,ee2_index,
-			Range(m_ncTotal,task->getNrOfConstraints()),Range(6*m_nsets,6))));
-	if(!result.second)
-		return false;
-	m_ncTotal+=task->getNrOfConstraints();
-	m_nsets+=1;
-	return true;
+  // Check if objects exist:
+  ObjectMap::iterator object1_it = objects.find(object1);
+  ObjectMap::iterator object2_it = objects.find(object2);
+  if (object1_it == objects.end() || object2_it == objects.end())
+    return false;
+  int ee1_index = object1_it->second->object->addEndEffector(ee1);
+  int ee2_index = object2_it->second->object->addEndEffector(ee2);
+  if (ee1_index < 0 || ee2_index < 0)
+    return false;
+  std::pair<ConstraintMap::iterator, bool> result = constraints.insert(ConstraintMap::value_type(
+      name,
+      new ConstraintSet_struct(task,
+                               object1_it,
+                               ee1_index,
+                               object2_it,
+                               ee2_index,
+                               Range(m_ncTotal, task->getNrOfConstraints()),
+                               Range(6 * m_nsets, 6))));
+  if (!result.second)
+    return false;
+  m_ncTotal += task->getNrOfConstraints();
+  m_nsets += 1;
+  return true;
 }
 
-bool Scene::addSolver(Solver* _solver){
-    if(m_solver==NULL){
-        m_solver=_solver;
-        return true;
-    }
-    else
-        return false;
+bool Scene::addSolver(Solver *_solver)
+{
+  if (m_solver == NULL) {
+    m_solver = _solver;
+    return true;
+  }
+  else
+    return false;
 }
 
-bool Scene::addCache(Cache* _cache){
-    if(m_cache==NULL){
-        m_cache=_cache;
-        return true;
-    }
-    else
-        return false;
+bool Scene::addCache(Cache *_cache)
+{
+  if (m_cache == NULL) {
+    m_cache = _cache;
+    return true;
+  }
+  else
+    return false;
 }
 
-bool Scene::initialize(){
-
-    //prepare all matrices:
-	if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
-		return false;
-
-    m_A = e_zero_matrix(m_ncTotal,m_nqTotal);
-	if (m_nuTotal > 0) {
-		m_B = e_zero_matrix(m_ncTotal,m_nuTotal);
-		m_xdot = e_zero_vector(m_nuTotal);
-		m_Ju = e_zero_matrix(6*m_nsets,m_nuTotal);
-	}
-    m_Atemp = e_zero_matrix(m_ncTotal,6*m_nsets);
-    m_ydot = e_zero_vector(m_ncTotal);
-    m_qdot = e_zero_vector(m_nqTotal);
-    m_Wq = e_zero_matrix(m_nqTotal,m_nqTotal);
-    m_Wy = e_zero_vector(m_ncTotal);
-    m_Jq = e_zero_matrix(6*m_nsets,m_nqTotal);
-    m_Jf = e_zero_matrix(6*m_nsets,6*m_nsets);
-    m_Jf_inv = m_Jf;
-    m_Cf = e_zero_matrix(m_ncTotal,m_Jf.rows());
-    m_Cq = e_zero_matrix(m_ncTotal,m_nqTotal);
-
-    bool result=true;
-	// finalize all objects
-	for (ObjectMap::iterator it=objects.begin(); it!=objects.end(); ++it) {
-		Object_struct* os = it->second;
-
-		os->object->initCache(m_cache);
-		if (os->constraintrange.count > 0)
-			project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
-	}
-
-	m_ytask.resize(m_ncTotal);
-	bool toggle=true;
-	int cnt = 0;
-    //Initialize all ConstraintSets:
-    for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
-        //Calculate the external pose:
-		ConstraintSet_struct* cs = it->second;
-		Frame external_pose;
-		getConstraintPose(cs->task, cs, external_pose);
-        result&=cs->task->initialise(external_pose);
-		cs->task->initCache(m_cache);
-		for (int i=0; i<cs->constraintrange.count; i++, cnt++) {
-			m_ytask[cnt] = toggle;
-		}
-		toggle = !toggle;
-		project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
-    }
+bool Scene::initialize()
+{
 
-    if(m_solver!=NULL)
-        m_solver->init(m_nqTotal,m_ncTotal,m_ytask);
-    else
-        return false;
+  // prepare all matrices:
+  if (m_ncTotal == 0 || m_nqTotal == 0 || m_nsets == 0)
+    return false;
 
+  m_A = e_zero_matrix(m_ncTotal, m_nqTotal);
+  if (m_nuTotal > 0) {
+    m_B = e_zero_matrix(m_ncTotal, m_nuTotal);
+    m_xdot = e_zero_vector(m_nuTotal);
+    m_Ju = e_zero_matrix(6 * m_nsets, m_nuTotal);
+  }
+  m_Atemp = e_zero_matrix(m_ncTotal, 6 * m_nsets);
+  m_ydot = e_zero_vector(m_ncTotal);
+  m_qdot = e_zero_vector(m_nqTotal);
+  m_Wq = e_zero_matrix(m_nqTotal, m_nqTotal);
+  m_Wy = e_zero_vector(m_ncTotal);
+  m_Jq = e_zero_matrix(6 * m_nsets, m_nqTotal);
+  m_Jf = e_zero_matrix(6 * m_nsets, 6 * m_nsets);
+  m_Jf_inv = m_Jf;
+  m_Cf = e_zero_matrix(m_ncTotal, m_Jf.rows());
+  m_Cq = e_zero_matrix(m_ncTotal, m_nqTotal);
+
+  bool result = true;
+  // finalize all objects
+  for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+    Object_struct *os = it->second;
+
+    os->object->initCache(m_cache);
+    if (os->constraintrange.count > 0)
+      project(m_Cq,
+              os->constraintrange,
+              os->jointrange) = (((ControlledObject *)(os->object))->getCq());
+  }
+
+  m_ytask.resize(m_ncTotal);
+  bool toggle = true;
+  int cnt = 0;
+  // Initialize all ConstraintSets:
+  for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+    // Calculate the external pose:
+    ConstraintSet_struct *cs = it->second;
+    Frame external_pose;
+    getConstraintPose(cs->task, cs, external_pose);
+    result &= cs->task->initialise(external_pose);
+    cs->task->initCache(m_cache);
+    for (int i = 0; i < cs->constraintrange.count; i++, cnt++) {
+      m_ytask[cnt] = toggle;
+    }
+    toggle = !toggle;
+    project(m_Cf, cs->constraintrange, cs->featurerange) = cs->task->getCf();
+  }
 
-    return result;
+  if (m_solver != NULL)
+    m_solver->init(m_nqTotal, m_ncTotal, m_ytask);
+  else
+    return false;
+
+  return result;
 }
 
-bool Scene::getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose)
+bool Scene::getConstraintPose(ConstraintSet *constraint, void *_param, KDL::Frame &_pose)
 {
-	// function called from constraint when they need to get the external pose
-	ConstraintSet_struct* cs = (ConstraintSet_struct*)_param;
-	// verification, the pointer MUST match
-	assert (constraint == cs->task);
-	Object_struct* ob1 = cs->object1->second;
-	Object_struct* ob2 = cs->object2->second;
-	//Calculate the external pose:
-	_pose=(ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index)).Inverse()*(ob2->base->getPose(ob2->baseFrameIndex)*ob2->object->getPose(cs->ee2index));
-	return true;
+  // function called from constraint when they need to get the external pose
+  ConstraintSet_struct *cs = (ConstraintSet_struct *)_param;
+  // verification, the pointer MUST match
+  assert(constraint == cs->task);
+  Object_struct *ob1 = cs->object1->second;
+  Object_struct *ob2 = cs->object2->second;
+  // Calculate the external pose:
+  _pose =
+      (ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index)).Inverse() *
+      (ob2->base->getPose(ob2->baseFrameIndex) * ob2->object->getPose(cs->ee2index));
+  return true;
 }
 
-bool Scene::update(double timestamp, double timestep, unsigned int numsubstep, bool reiterate, bool cache, bool interpolate)
+bool Scene::update(double timestamp,
+                   double timestep,
+                   unsigned int numsubstep,
+                   bool reiterate,
+                   bool cache,
+                   bool interpolate)
 {
-	// we must have valid timestep and timestamp
-	if (timestamp < KDL::epsilon || timestep < 0.0)
-		return false;
-	Timestamp ts;
-	ts.realTimestamp = timestamp;
-	// initially we start with the full timestep to allow velocity estimation over the full interval
-	ts.realTimestep = timestep;
-	setCacheTimestamp(ts);
-	ts.substep = 0;
-	// for reiteration don't load cache
-	// reiteration=additional iteration with same timestamp if application finds the convergence not good enough
-	ts.reiterate = (reiterate) ? 1 : 0;
-	ts.interpolate = (interpolate) ? 1 : 0;
-	ts.cache = (cache) ? 1 : 0;
-	ts.update = 1;
-	ts.numstep = (numsubstep & 0xFF);
-	bool autosubstep = (numsubstep == 0) ? true : false;
-	if (numsubstep < 1)
-		numsubstep = 1;
-	double timesubstep = timestep/numsubstep;
-	double timeleft = timestep;
-
-	if (timeleft == 0.0) {
-		// this special case correspond to a request to cache data
-		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
-			it->second->object->pushCache(ts);
-		}
-		//Update the Constraints
-		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
-			it->second->task->pushCache(ts);
-		}
-		return true;
-	}
-
-	// double maxqdot; // UNUSED
-	e_scalar nlcoef;
-	SceneLock lockCallback(this);
-	Frame external_pose;
-	bool locked;
-
-	// initially we keep timestep unchanged so that update function compute the velocity over
-	while (numsubstep > 0) {
-		// get objects
-		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
-			Object_struct* os = it->second;
-			if (os->object->getType()==Object::Controlled) {
-				((ControlledObject*)(os->object))->updateControlOutput(ts);
-				if (os->constraintrange.count > 0) {
-					project(m_ydot, os->constraintrange) = ((ControlledObject*)(os->object))->getControlOutput();
-					project(m_Wy, os->constraintrange) = ((ControlledObject*)(os->object))->getWy();
-					// project(m_Cq,os->constraintrange,os->jointrange) = (((ControlledObject*)(os->object))->getCq());
-				}
-				if (os->jointrange.count > 0) {
-					project(m_Wq,os->jointrange,os->jointrange) = ((ControlledObject*)(os->object))->getWq();
-				}
-			}
-			if (os->object->getType()==Object::UnControlled && ((UncontrolledObject*)os->object)->getNrOfCoordinates() != 0) {
-	            ((UncontrolledObject*)(os->object))->updateCoordinates(ts);
-				if (!ts.substep) {
-					// velocity of uncontrolled object remains constant during substepping
-					project(m_xdot,os->coordinaterange) = ((UncontrolledObject*)(os->object))->getXudot();
-				}
-			}
-		}
-
-		//get new Constraints values
-		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it) {
-			ConstraintSet_struct* cs = it->second;
-			Object_struct* ob1 = cs->object1->second;
-			Object_struct* ob2 = cs->object2->second;
-
-			if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() || ob2->object->updated()) {
-				// the object from which the constraint depends have changed position
-				// recompute the constraint pose
-				getConstraintPose(cs->task, cs, external_pose);
-				cs->task->initialise(external_pose);
-			}
-			cs->task->updateControlOutput(ts);
-	        project(m_ydot,cs->constraintrange)=cs->task->getControlOutput();
-			if (!ts.substep || cs->task->substep()) {
-				project(m_Wy,cs->constraintrange)=(cs->task)->getWy();
-				//project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
-			}
-
-			project(m_Jf,cs->featurerange,cs->featurerange)=cs->task->getJf();
-			//std::cout << "Jf = " << Jf << std::endl;
-			//Transform the reference frame of this jacobian to the world reference frame
-			Eigen::Block<e_matrix> Jf_part = project(m_Jf,cs->featurerange,cs->featurerange);
-			changeBase(Jf_part,ob1->base->getPose(ob1->baseFrameIndex)*ob1->object->getPose(cs->ee1index));
-			//std::cout << "Jf_w = " << Jf << std::endl;
-
-			//calculate the inverse of Jf
-			KDL::svd_eigen_HH(project(m_Jf,cs->featurerange,cs->featurerange),m_Uf,m_Sf,m_Vf,m_tempf);
-			for(unsigned int i=0;i<6;++i)
-				if(m_Sf(i)<KDL::epsilon)
-					m_Uf.col(i).setConstant(0.0);
-				else
-					m_Uf.col(i)*=(1/m_Sf(i));
-			project(m_Jf_inv,cs->featurerange,cs->featurerange).noalias()=m_Vf*m_Uf.transpose();
-
-			//Get the robotjacobian associated with this constraintset
-			//Each jacobian is expressed in robot base frame => convert to world reference
-			//and negate second robot because it is taken reversed when closing the loop:
-			if(ob1->object->getType()==Object::Controlled){
-				project(m_Jq,cs->featurerange,ob1->jointrange) = (((ControlledObject*)(ob1->object))->getJq(cs->ee1index));
-				//Transform the reference frame of this jacobian to the world reference frame:
-				Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob1->jointrange);
-				changeBase(Jq_part,ob1->base->getPose(ob1->baseFrameIndex));
-				// if the base of this object is moving, get the Ju part
-				if (ob1->base->getNrOfCoordinates() != 0) {
-					// Ju is already computed for world reference frame
-					project(m_Ju,cs->featurerange,ob1->coordinaterange)=ob1->base->getJu(ob1->baseFrameIndex);
-				}
-			} else if (ob1->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob1->object)->getNrOfCoordinates() != 0) {
-				// object1 is uncontrolled moving object
-				project(m_Ju,cs->featurerange,ob1->coordinaterange)=((UncontrolledObject*)ob1->object)->getJu(cs->ee1index);
-			}
-			if(ob2->object->getType()==Object::Controlled){
-				//Get the robotjacobian associated with this constraintset
-				// process a special case where object2 and object1 are equal but using different end effector
-				if (ob1->object == ob2->object) {
-					// we must create a temporary matrix
-					e_matrix JqTemp(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
-					//Transform the reference frame of this jacobian to the world reference frame:
-					changeBase(JqTemp,ob2->base->getPose(ob2->baseFrameIndex));
-					// substract in place
-					project(m_Jq,cs->featurerange,ob2->jointrange) -= JqTemp;
-				} else {
-					project(m_Jq,cs->featurerange,ob2->jointrange) = -(((ControlledObject*)(ob2->object))->getJq(cs->ee2index));
-					//Transform the reference frame of this jacobian to the world reference frame:
-					Eigen::Block<e_matrix> Jq_part = project(m_Jq,cs->featurerange,ob2->jointrange);
-					changeBase(Jq_part,ob2->base->getPose(ob2->baseFrameIndex));
-				}
-				if (ob2->base->getNrOfCoordinates() != 0) {
-					// if base is the same as first object or first object base, 
-					// that portion of m_Ju has been set already => substract inplace
-					if (ob2->base == ob1->base || ob2->base == ob1->object) {
-						project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ob2->base->getJu(ob2->baseFrameIndex);
-					} else {
-						project(m_Ju,cs->featurerange,ob2->coordinaterange) = -ob2->base->getJu(ob2->baseFrameIndex);
-					}
-				}
-			} else if (ob2->object->getType() == Object::UnControlled && ((UncontrolledObject*)ob2->object)->getNrOfCoordinates() != 0) {
-				if (ob2->object == ob1->base || ob2->object == ob1->object) {
-					project(m_Ju,cs->featurerange,ob2->coordinaterange) -= ((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
-				} else {
-					project(m_Ju,cs->featurerange,ob2->coordinaterange) = -((UncontrolledObject*)ob2->object)->getJu(cs->ee2index);
-				}
-			}
-		}
-
-	    //Calculate A
-		m_Atemp.noalias()=m_Cf*m_Jf_inv;
-		m_A.noalias() = m_Cq-(m_Atemp*m_Jq);
-		if (m_nuTotal > 0) {
-			m_B.noalias()=m_Atemp*m_Ju;
-			m_ydot.noalias() += m_B*m_xdot;
-		}
-
-		//Call the solver with A, Wq, Wy, ydot to solver qdot:
-		if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
-			// this should never happen
-			return false;
-		//send result to the objects
-		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it) {
-			Object_struct* os = it->second;
-			if(os->object->getType()==Object::Controlled)
-				((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
-		}
-		// compute the constraint velocity
-		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
-			ConstraintSet_struct* cs = it->second;
-			Object_struct* ob1 = cs->object1->second;
-			Object_struct* ob2 = cs->object2->second;
-			//Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
-			e_vector6 external_vel = e_zero_vector(6);
-			if (ob1->jointrange.count > 0)
-				external_vel.noalias() += (project(m_Jq,cs->featurerange,ob1->jointrange)*project(m_qdot,ob1->jointrange));
-			if (ob2->jointrange.count > 0)
-				external_vel.noalias() += (project(m_Jq,cs->featurerange,ob2->jointrange)*project(m_qdot,ob2->jointrange));
-			if (ob1->coordinaterange.count > 0)
-				external_vel.noalias() += (project(m_Ju,cs->featurerange,ob1->coordinaterange)*project(m_xdot,ob1->coordinaterange));
-			if (ob2->coordinaterange.count > 0)
-				external_vel.noalias() += (project(m_Ju,cs->featurerange,ob2->coordinaterange)*project(m_xdot,ob2->coordinaterange));
-			//the twist caused by the constraint must be opposite because of the closed loop
-			//estimate the velocity of the joints using the inverse jacobian
-			e_vector6 estimated_chidot = project(m_Jf_inv,cs->featurerange,cs->featurerange)*(-external_vel);
-			cs->task->setJointVelocity(estimated_chidot);
-		}
-
-		if (autosubstep) {
-			// automatic computing of substep based on maximum joint change
-			// and joint limit gain variation
-			// We will pass the joint velocity to each object and they will recommend a maximum timestep
-			timesubstep = timeleft;
-			// get armature max joint velocity to estimate the maximum duration of integration
-			// maxqdot = m_qdot.cwise().abs().maxCoeff(); // UNUSED
-			double maxsubstep = nlcoef*m_maxstep;
-			if (maxsubstep < m_minstep)
-				maxsubstep = m_minstep;
-			if (timesubstep > maxsubstep)
-				timesubstep = maxsubstep;
-			for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
-				Object_struct* os = it->second;
-				if(os->object->getType()==Object::Controlled)
-					((ControlledObject*)(os->object))->getMaxTimestep(timesubstep);
-			}
-			for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
-				ConstraintSet_struct* cs = it->second;
-				cs->task->getMaxTimestep(timesubstep);
-			}
-			// use substep that are even dividers of timestep for more regularity
-			maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666);
-			timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep);
-			if (timesubstep >= timeleft-(m_minstep/2.0)) {
-				timesubstep = timeleft;
-				numsubstep = 1;
-				timeleft = 0.;
-			} else {
-				numsubstep = 2;
-				timeleft -= timesubstep;
-			}
-		}
-		if (numsubstep > 1) {
-			ts.substep = 1;
-		} else {
-			// set substep to false for last iteration so that controlled output 
-			// can be updated in updateKinematics() and model_update)() before next call to Secne::update()
-			ts.substep = 0;
-		}
-		// change timestep so that integration is done correctly
-		ts.realTimestep = timesubstep;
-
-		do {
-			ObjectMap::iterator it;
-			Object_struct* os;
-			locked = false;
-			for(it=objects.begin();it!=objects.end();++it){
-				os = it->second;
-				if (os->object->getType()==Object::Controlled) {
-					lockCallback.setRange(os->jointrange);
-					if (((ControlledObject*)os->object)->updateJoint(ts, lockCallback)) {
-						// this means one of the joint was locked and we must rerun
-						// the solver to update the remaining joints
-						locked = true;
-						break;
-					}
-				}
-			}
-			if (locked) {
-				// Some rows of m_Wq have been cleared so that the corresponding joint will not move
-				if(!m_solver->solve(m_A,m_Wy,m_ydot,m_Wq,m_qdot,nlcoef))
-					// this should never happen
-					return false;
-
-				//send result to the objects
-				for(it=objects.begin();it!=objects.end();++it) {
-					os = it->second;
-					if(os->object->getType()==Object::Controlled)
-						((ControlledObject*)(os->object))->setJointVelocity(project(m_qdot,os->jointrange));
-				}
-			}
-		} while (locked);
-
-		//Update the Objects
-		for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
-			it->second->object->updateKinematics(ts);
-			// mark this object not updated since the constraint will be updated anyway
-			// this flag is only useful to detect external updates
-			it->second->object->updated(false);
-		}
-		//Update the Constraints
-		for(ConstraintMap::iterator it=constraints.begin();it!=constraints.end();++it){
-			ConstraintSet_struct* cs = it->second;
-			//Calculate the external pose:
-			getConstraintPose(cs->task, cs, external_pose);
-			cs->task->modelUpdate(external_pose,ts);
-			// update the constraint output and cache
-			cs->task->updateKinematics(ts);
-		}
-		numsubstep--;
-	}
-	return true;
-}
+  // we must have valid timestep and timestamp
+  if (timestamp < KDL::epsilon || timestep < 0.0)
+    return false;
+  Timestamp ts;
+  ts.realTimestamp = timestamp;
+  // initially we start with the full timestep to allow velocity estimation over the full interval
+  ts.realTimestep = timestep;
+  setCacheTimestamp(ts);
+  ts.substep = 0;
+  // for reiteration don't load cache
+  // reiteration=additional iteration with same timestamp if application finds the convergence not
+  // good enough
+  ts.reiterate = (reiterate) ? 1 : 0;
+  ts.interpolate = (interpolate) ? 1 : 0;
+  ts.cache = (cache) ? 1 : 0;
+  ts.update = 1;
+  ts.numstep = (numsubstep & 0xFF);
+  bool autosubstep = (numsubstep == 0) ? true : false;
+  if (numsubstep < 1)
+    numsubstep = 1;
+  double timesubstep = timestep / numsubstep;
+  double timeleft = timestep;
+
+  if (timeleft == 0.0) {
+    // this special case correspond to a request to cache data
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      it->second->object->pushCache(ts);
+    }
+    // Update the Constraints
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      it->second->task->pushCache(ts);
+    }
+    return true;
+  }
+
+  // double maxqdot; // UNUSED
+  e_scalar nlcoef;
+  SceneLock lockCallback(this);
+  Frame external_pose;
+  bool locked;
+
+  // initially we keep timestep unchanged so that update function compute the velocity over
+  while (numsubstep > 0) {
+    // get objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      Object_struct *os = it->second;
+      if (os->object->getType() == Object::Controlled) {
+        ((ControlledObject *)(os->object))->updateControlOutput(ts);
+        if (os->constraintrange.count > 0) {
+          project(m_ydot,
+                  os->constraintrange) = ((ControlledObject *)(os->object))->getControlOutput();
+          project(m_Wy, os->constraintrange) = ((ControlledObject *)(os->object))->getWy();
+          // project(m_Cq,os->constraintrange,os->jointrange) =
+          // (((ControlledObject*)(os->object))->getCq());
+        }
+        if (os->jointrange.count > 0) {
+          project(
+              m_Wq, os->jointrange, os->jointrange) = ((ControlledObject *)(os->object))->getWq();
+        }
+      }
+      if (os->object->getType() == Object::UnControlled &&
+          ((UncontrolledObject *)os->object)->getNrOfCoordinates() != 0) {
+        ((UncontrolledObject *)(os->object))->updateCoordinates(ts);
+        if (!ts.substep) {
+          // velocity of uncontrolled object remains constant during substepping
+          project(m_xdot, os->coordinaterange) = ((UncontrolledObject *)(os->object))->getXudot();
+        }
+      }
+    }
+
+    // get new Constraints values
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      Object_struct *ob1 = cs->object1->second;
+      Object_struct *ob2 = cs->object2->second;
+
+      if (ob1->base->updated() || ob1->object->updated() || ob2->base->updated() ||
+          ob2->object->updated()) {
+        // the object from which the constraint depends have changed position
+        // recompute the constraint pose
+        getConstraintPose(cs->task, cs, external_pose);
+        cs->task->initialise(external_pose);
+      }
+      cs->task->updateControlOutput(ts);
+      project(m_ydot, cs->constraintrange) = cs->task->getControlOutput();
+      if (!ts.substep || cs->task->substep()) {
+        project(m_Wy, cs->constraintrange) = (cs->task)->getWy();
+        // project(m_Cf,cs->constraintrange,cs->featurerange)=cs->task->getCf();
+      }
+
+      project(m_Jf, cs->featurerange, cs->featurerange) = cs->task->getJf();
+      // std::cout << "Jf = " << Jf << std::endl;
+      // Transform the reference frame of this jacobian to the world reference frame
+      Eigen::Block<e_matrix> Jf_part = project(m_Jf, cs->featurerange, cs->featurerange);
+      changeBase(Jf_part,
+                 ob1->base->getPose(ob1->baseFrameIndex) * ob1->object->getPose(cs->ee1index));
+      // std::cout << "Jf_w = " << Jf << std::endl;
+
+      // calculate the inverse of Jf
+      KDL::svd_eigen_HH(
+          project(m_Jf, cs->featurerange, cs->featurerange), m_Uf, m_Sf, m_Vf, m_tempf);
+      for (unsigned int i = 0; i < 6; ++i)
+        if (m_Sf(i) < KDL::epsilon)
+          m_Uf.col(i).setConstant(0.0);
+        else
+          m_Uf.col(i) *= (1 / m_Sf(i));
+      project(m_Jf_inv, cs->featurerange, cs->featurerange).noalias() = m_Vf * m_Uf.transpose();
+
+      // Get the robotjacobian associated with this constraintset
+      // Each jacobian is expressed in robot base frame => convert to world reference
+      // and negate second robot because it is taken reversed when closing the loop:
+      if (ob1->object->getType() == Object::Controlled) {
+        project(m_Jq,
+                cs->featurerange,
+                ob1->jointrange) = (((ControlledObject *)(ob1->object))->getJq(cs->ee1index));
+        // Transform the reference frame of this jacobian to the world reference frame:
+        Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob1->jointrange);
+        changeBase(Jq_part, ob1->base->getPose(ob1->baseFrameIndex));
+        // if the base of this object is moving, get the Ju part
+        if (ob1->base->getNrOfCoordinates() != 0) {
+          // Ju is already computed for world reference frame
+          project(m_Ju, cs->featurerange, ob1->coordinaterange) = ob1->base->getJu(
+              ob1->baseFrameIndex);
+        }
+      }
+      else if (ob1->object->getType() == Object::UnControlled &&
+               ((UncontrolledObject *)ob1->object)->getNrOfCoordinates() != 0) {
+        // object1 is uncontrolled moving object
+        project(m_Ju,
+                cs->featurerange,
+                ob1->coordinaterange) = ((UncontrolledObject *)ob1->object)->getJu(cs->ee1index);
+      }
+      if (ob2->object->getType() == Object::Controlled) {
+        // Get the robotjacobian associated with this constraintset
+        // process a special case where object2 and object1 are equal but using different end
+        // effector
+        if (ob1->object == ob2->object) {
+          // we must create a temporary matrix
+          e_matrix JqTemp(((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
+          // Transform the reference frame of this jacobian to the world reference frame:
+          changeBase(JqTemp, ob2->base->getPose(ob2->baseFrameIndex));
+          // subtract in place
+          project(m_Jq, cs->featurerange, ob2->jointrange) -= JqTemp;
+        }
+        else {
+          project(m_Jq, cs->featurerange, ob2->jointrange) = -(
+              ((ControlledObject *)(ob2->object))->getJq(cs->ee2index));
+          // Transform the reference frame of this jacobian to the world reference frame:
+          Eigen::Block<e_matrix> Jq_part = project(m_Jq, cs->featurerange, ob2->jointrange);
+          changeBase(Jq_part, ob2->base->getPose(ob2->baseFrameIndex));
+        }
+        if (ob2->base->getNrOfCoordinates() != 0) {
+          // if base is the same as first object or first object base,
+          // that portion of m_Ju has been set already => subtract inplace
+          if (ob2->base == ob1->base || ob2->base == ob1->object) {
+            project(m_Ju, cs->featurerange, ob2->coordinaterange) -= ob2->base->getJu(
+                ob2->baseFrameIndex);
+          }
+          else {
+            project(m_Ju, cs->featurerange, ob2->coordinaterange) = -ob2->base->getJu(
+                ob2->baseFrameIndex);
+          }
+        }
+      }
+      else if (ob2->object->getType() == Object::UnControlled &&
+               ((UncontrolledObject *)ob2->object)->getNrOfCoordinates() != 0) {
+        if (ob2->object == ob1->base || ob2->object == ob1->object) {
+          project(m_Ju, cs->featurerange, ob2->coordinaterange) -=
+              ((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
+        }
+        else {
+          project(m_Ju, cs->featurerange, ob2->coordinaterange) =
+              -((UncontrolledObject *)ob2->object)->getJu(cs->ee2index);
+        }
+      }
+    }
+
+    // Calculate A
+    m_Atemp.noalias() = m_Cf * m_Jf_inv;
+    m_A.noalias() = m_Cq - (m_Atemp * m_Jq);
+    if (m_nuTotal > 0) {
+      m_B.noalias() = m_Atemp * m_Ju;
+      m_ydot.noalias() += m_B * m_xdot;
+    }
 
+    // Call the solver with A, Wq, Wy, ydot to solver qdot:
+    if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
+      // this should never happen
+      return false;
+    // send result to the objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      Object_struct *os = it->second;
+      if (os->object->getType() == Object::Controlled)
+        ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
+    }
+    // compute the constraint velocity
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      Object_struct *ob1 = cs->object1->second;
+      Object_struct *ob2 = cs->object2->second;
+      // Calculate the twist of the world reference frame due to the robots (Jq*qdot+Ju*chiudot):
+      e_vector6 external_vel = e_zero_vector(6);
+      if (ob1->jointrange.count > 0)
+        external_vel.noalias() += (project(m_Jq, cs->featurerange, ob1->jointrange) *
+                                   project(m_qdot, ob1->jointrange));
+      if (ob2->jointrange.count > 0)
+        external_vel.noalias() += (project(m_Jq, cs->featurerange, ob2->jointrange) *
+                                   project(m_qdot, ob2->jointrange));
+      if (ob1->coordinaterange.count > 0)
+        external_vel.noalias() += (project(m_Ju, cs->featurerange, ob1->coordinaterange) *
+                                   project(m_xdot, ob1->coordinaterange));
+      if (ob2->coordinaterange.count > 0)
+        external_vel.noalias() += (project(m_Ju, cs->featurerange, ob2->coordinaterange) *
+                                   project(m_xdot, ob2->coordinaterange));
+      // the twist caused by the constraint must be opposite because of the closed loop
+      // estimate the velocity of the joints using the inverse jacobian
+      e_vector6 estimated_chidot = project(m_Jf_inv, cs->featurerange, cs->featurerange) *
+                                   (-external_vel);
+      cs->task->setJointVelocity(estimated_chidot);
+    }
+
+    if (autosubstep) {
+      // automatic computing of substep based on maximum joint change
+      // and joint limit gain variation
+      // We will pass the joint velocity to each object and they will recommend a maximum timestep
+      timesubstep = timeleft;
+      // get armature max joint velocity to estimate the maximum duration of integration
+      // maxqdot = m_qdot.cwise().abs().maxCoeff(); // UNUSED
+      double maxsubstep = nlcoef * m_maxstep;
+      if (maxsubstep < m_minstep)
+        maxsubstep = m_minstep;
+      if (timesubstep > maxsubstep)
+        timesubstep = maxsubstep;
+      for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+        Object_struct *os = it->second;
+        if (os->object->getType() == Object::Controlled)
+          ((ControlledObject *)(os->object))->getMaxTimestep(timesubstep);
+      }
+      for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+        ConstraintSet_struct *cs = it->second;
+        cs->task->getMaxTimestep(timesubstep);
+      }
+      // use substep that are even dividers of timestep for more regularity
+      maxsubstep = 2.0 * floor(timestep / 2.0 / timesubstep - 0.66666);
+      timesubstep = (maxsubstep < 0.0) ? timestep : timestep / (2.0 + maxsubstep);
+      if (timesubstep >= timeleft - (m_minstep / 2.0)) {
+        timesubstep = timeleft;
+        numsubstep = 1;
+        timeleft = 0.;
+      }
+      else {
+        numsubstep = 2;
+        timeleft -= timesubstep;
+      }
+    }
+    if (numsubstep > 1) {
+      ts.substep = 1;
+    }
+    else {
+      // set substep to false for last iteration so that controlled output
+      // can be updated in updateKinematics() and model_update)() before next call to
+      // Secne::update()
+      ts.substep = 0;
+    }
+    // change timestep so that integration is done correctly
+    ts.realTimestep = timesubstep;
+
+    do {
+      ObjectMap::iterator it;
+      Object_struct *os;
+      locked = false;
+      for (it = objects.begin(); it != objects.end(); ++it) {
+        os = it->second;
+        if (os->object->getType() == Object::Controlled) {
+          lockCallback.setRange(os->jointrange);
+          if (((ControlledObject *)os->object)->updateJoint(ts, lockCallback)) {
+            // this means one of the joint was locked and we must rerun
+            // the solver to update the remaining joints
+            locked = true;
+            break;
+          }
+        }
+      }
+      if (locked) {
+        // Some rows of m_Wq have been cleared so that the corresponding joint will not move
+        if (!m_solver->solve(m_A, m_Wy, m_ydot, m_Wq, m_qdot, nlcoef))
+          // this should never happen
+          return false;
+
+        // send result to the objects
+        for (it = objects.begin(); it != objects.end(); ++it) {
+          os = it->second;
+          if (os->object->getType() == Object::Controlled)
+            ((ControlledObject *)(os->object))->setJointVelocity(project(m_qdot, os->jointrange));
+        }
+      }
+    } while (locked);
+
+    // Update the Objects
+    for (ObjectMap::iterator it = objects.begin(); it != objects.end(); ++it) {
+      it->second->object->updateKinematics(ts);
+      // mark this object not updated since the constraint will be updated anyway
+      // this flag is only useful to detect external updates
+      it->second->object->updated(false);
+    }
+    // Update the Constraints
+    for (ConstraintMap::iterator it = constraints.begin(); it != constraints.end(); ++it) {
+      ConstraintSet_struct *cs = it->second;
+      // Calculate the external pose:
+      getConstraintPose(cs->task, cs, external_pose);
+      cs->task->modelUpdate(external_pose, ts);
+      // update the constraint output and cache
+      cs->task->updateKinematics(ts);
+    }
+    numsubstep--;
+  }
+  return true;
 }
+
+}  // namespace iTaSC