optimization

CMakeLists.txt

@@ -129,11 +129,13 @@ install(FILES
 	data/meshes/darpareduced.stl
 	data/meshes/car.stl
 	data/meshes/car_simple.stl
+	data/meshes/ground.stl
+	data/meshes/ground_table.stl
 	data/meshes/hrp2_trunk.stl
 	data/meshes/hrp2_trunk_body.stl
 	data/meshes/hrp2_trunk_torso.stl
-        data/meshes/hrp2_trunk_body_view.dae
-        data/meshes/hrp2_trunk_torso_view.dae
+	data/meshes/hrp2_trunk_body_view.dae
+	data/meshes/hrp2_trunk_torso_view.dae
 	data/meshes/hrp2_rom.stl
 	data/meshes/hrp2_larm_rom.stl
 	data/meshes/hrp2_rarm_rom.stl
@@ -149,7 +151,7 @@ install(FILES
 	data/meshes/truck.stl
 	data/meshes/truck_vision.stl
 	data/meshes/stair_bauzil.stl
-        data/meshes/stair_bauzil_reduced.stl
+	data/meshes/stair_bauzil_reduced.stl
 	data/meshes/climb.stl
 	data/meshes/stepladder.stl
 	data/meshes/chair.stl

data/srdf/scene_wall2.srdf

+<?xml version="1.0"?>
+<robot name="scene_wall2">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>

data/urdf/ground.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>

data/urdf/scene_wall2.urdf

+<robot name="scene_wall2">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -291,6 +291,24 @@ module hpp
     /// \return id of the root path computed
     short comRRT(in double state1, in double state2,  in  unsigned short comPath, in unsigned short numOptimizations) raises (Error);
 
+    /// Provided a path has already been computed and interpolated, generate a continuous path
+    /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
+    /// Will fail if the index of the states do not exist
+    /// The path of the COM of thr robot and limbs not considered by the contact transitions between
+    /// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
+    /// It must be valid in the sense of the active PathValidation.
+    /// \param state1 index of first state.
+    /// \param rootPositions1 com positions to track for 1st phase
+    /// \param rootPositions1 com positions to track for 2nd phase
+    /// \param rootPositions1 com positions to track for 3rd phase
+    /// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+    /// \return id of the root path computed
+    short comRRTFromPos(in double state1,
+                        in  floatSeqSeq rootPositions1,
+                        in  floatSeqSeq rootPositions2,
+                        in  floatSeqSeq rootPositions3,
+                        in unsigned short numOptimizations) raises (Error);
+
     /// Project a given state into a given COM position
     /// between two indicated states. The states do not need to be consecutive, but increasing in Id.
     /// Will fail if the index of the state does not exist.

script/scenarios/darpa_hyq_interp.py

@@ -82,7 +82,7 @@ r(q_init)
 # computing the contact sequence
 configs = fullBody.interpolate(0.1, 1, 0)
 
-r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
+#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
 
 # calling draw with increasing i will display the sequence
 i = 0;
@@ -90,3 +90,106 @@ fullBody.draw(configs[i],r); i=i+1; i-1
 
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (ps.robot.client.basic, r)
+
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
+
+pid = 3
+
+def optimize_animate(i):
+	res = draw_trajectory(fullBody, configs, i, False, 0.5, False)
+	#~ res = gen_trajectory(fullBody, configs, i, False, 1, False)
+	pos = [c.tolist() for c in res[0]['c']]
+	fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
+	fullBody.interpolateBetweenStatesFromPath(i,i+1,2,0)
+	fullBody.interpolateBetweenStatesFromPath(i,i+1,2,1)
+	fullBody.interpolateBetweenStatesFromPath(i,i+1,2,10)
+	fullBody.interpolateBetweenStatesFromPath(i,i+1,2,50)
+	global pid
+	#~ pp(pid);pp(pid+1);pp(pid+2);
+	pid = pid + 3
+
+print "diff position"
+from numpy import array
+#~ print (array(pos[-1]) - array(configs[6][0:3]))
+#~ print (array(pos[-1]) - array(configs[6][0:3])).norm()
+def play(i):
+	fullBody.setCurrentConfig(configs[i])
+	r(configs[i])
+	import time
+	for j,_ in enumerate (pos):
+		q=fullBody.getCurrentConfig()
+		q[0:3] = pos[j]
+		r(q)
+		time.sleep(0.25)		
+	q[3:] = configs[i+1][3:]
+	r(q)
+	
+res = []
+
+def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
+	pathPos=[]
+	length = pp.end*pp.client.problem.pathLength (pathId)
+	t = pp.start*pp.client.problem.pathLength (pathId)
+	while t < length :
+		q = pp.client.problem.configAtParam (pathId, t)
+		pp.publisher.robot.setCurrentConfig(q)
+		q = pp.publisher.robot.getCenterOfMass()
+		pathPos = pathPos + [q[:3]]
+		t += pp.dt
+	nameCurve = "path_"+str(pathId)+"_com"
+	pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
+	pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
+	pp.publisher.client.gui.refresh()
+
+res = []
+from hpp import Error as hpperr
+import sys
+def act(i, optim):
+	try:
+		pid = solve_com_RRT(fullBody, configs, i, True, 0.3, 0.2, False, optim, False, True)
+		displayComPath(pid)
+		#~ pp(pid)
+		global res
+		res = res + [pid]
+	except hpperr as e:
+		print "failed at id " + str(i) , e.strerror
+	except ValueError as e:
+		print "failed at id " + str(i) , e
+	except IndexError as e:
+		print "failed at id " + str(i) , e
+	except Exception as e:
+		print e
+	except:
+		return
+	
+for i in range(30,47):
+	act(i, 50)
+#~ for i in range(5,35):
+	#~ act(i, 50)
+
+
+	
+def displayInSave(pp, pathId, configs):
+	length = pp.end*pp.client.problem.pathLength (pathId)
+	t = pp.start*pp.client.problem.pathLength (pathId)
+	while t < length :
+		q = pp.client.problem.configAtParam (pathId, t)
+		configs.append(q)
+		t += (pp.dt * pp.speed)
+
+respath = []
+for p in res:
+	print p
+	displayInSave(pp,p, respath)
+
+for p in res:
+	pp(p)
+	
+fullBody.exportAll(r, respath, 'darpa_hyq_full');
+
+#~ from hpp.gepetto.blender.exportmotion import exportPath
+#~ for p in res:
+	#~ exportPath(r,fullBody.client.basic.robot,fullBody.client.basic.problem,p,0.1,'test'+str(p)+'.txt')
+
+print "tg"

script/scenarios/darpa_hyq_path.py

@@ -46,7 +46,7 @@ ps.addGoalConfig (q_goal)
 
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
-afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.loadObstacleModel (packageName, "darpa", "pling", r)
 afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
 
 # Choosing RBPRM shooter and path validation methods.
@@ -62,3 +62,7 @@ t = ps.solve ()
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
 #~ pp (1)
+
+q_far = q_init [::]
+q_far [0:3] = [-2, -3, 0.63]; 
+r(q_far)

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -339,6 +339,21 @@ class FullBody (object):
     def comRRT(self, state1, state2, path, numOptim = 10):
 		return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)		
 		
+			
+	## Provided a path has already been computed and interpolated, generate a continuous path
+    # between two indicated states. The states do not need to be consecutive, but increasing in Id.
+    # Will fail if the index of the states do not exist
+    # The path of the COM of thr robot and limbs not considered by the contact transitions between
+    # two states is assumed to be already computed, and registered in the solver under the id specified by the user.
+    # It must be valid in the sense of the active PathValidation.
+    # \param state1 index of first state.
+    # \param rootPositions1 com positions to track for 1st phase
+    # \param rootPositions1 com positions to track for 2nd phase
+    # \param rootPositions1 com positions to track for 3rd phase
+    # \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
+    # \return id of the root path computed
+    def comRRTFromPos(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
+		return self.client.rbprm.rbprm.comRRTFromPos(state1, comPos1, comPos2, comPos3, numOptim)
 		
 	## Project a given state into a given COM position
     # between two indicated states. The states do not need to be consecutive, but increasing in Id.

src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py

@@ -5,19 +5,16 @@ def __get_com(robot, config):
 	save = robot.getCurrentConfig()
 	robot.setCurrentConfig(config)
 	com = robot.getCenterOfMass()
-	print 'COM', com
-	#~ com = config[0:3]	 #assimilate root and com
 	robot.setCurrentConfig(save)
 	return com
 
-def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
+def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False):
 	init_com = __get_com(fullBody, states[state_id])
 	end_com = __get_com(fullBody, states[state_id+1])
 	p, N = fullBody.computeContactPoints(state_id)
 	mass = fullBody.getMass()
 	t_end_phases = [0]
 	[t_end_phases.append(t_end_phases[-1]+1) for _ in range(len(p))]
-	dt = 0.1
 	cones = None
 	if(computeCones):
 		cones = [fullBody.getContactCone(state_id, mu)[0]]
@@ -25,10 +22,11 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, red
 			cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
 		if(len(p) > len(cones)):
 			cones.append(fullBody.getContactCone(state_id+1, mu)[0])
-	return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq)
+	print "num cones ", len(cones)
+	return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq, verbose)
 
-def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
-	var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , reduce_ineq)
+def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1,  dt=0.2, reduce_ineq = True, verbose = False):
+	var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, reduce_ineq, verbose)
 	p, N = fullBody.computeContactPoints(state_id)
 	import numpy as np
 	from mpl_toolkits.mplot3d import Axes3D
@@ -58,27 +56,28 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, re
 	plt.show()
 	return var_final, params
 	
-def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0, draw = False):
+def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False):
 	print "callgin gen ",state_id
 	if(draw):
-		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)		
+		res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose)		
 	else:
-		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)
+		res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose)
 	t = res[1]["t_init_phases"];
 	dt = res[1]["dt"];
 	final_state = res[0]
 	c0 =  res[1]["x_init"][0:3]
-	print "c0", c0
 	comPos = [c0] + [c.tolist() for c in final_state['c']]
 	comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1],dt)] for index, _ in enumerate(t[:-1]) ]
 	comPosPerPhase[-1].append(comPos[-1])
 	assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
-	assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
+	#~ assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
 	print "done. generating state path ",state_id
 	
-	pid = fullBody.generateRootPathStates(comPos, states[state_id], states[state_id+1])
-	print "done. shortcuting ",state_id	
+	return fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)
 	
-	return fullBody.comRRT(state_id,state_id+1,pid,num_optims)
+	#~ pid = fullBody.generateRootPathStates(comPos, states[state_id], states[state_id+1])
+	#~ print "done. shortcuting ",state_id	
+	#~ 
+	#~ return fullBody.comRRT(state_id,state_id+1,pid,num_optims)
 
 	

src/rbprmbuilder.impl.cc

@@ -736,11 +736,6 @@ namespace hpp {
         ++pit;
         for(;pit != positions.end()-1; ++pit, u+=size_step)
         {
-            std::cout << "position added \n" << ((*pit).head(3)) << std::endl;
-            if((*pit)(0) > 0.552368)
-            {
-                std::cout << "pos devant \n" << (*pit)(0) << std::endl;
-            }
             current = addRotation(pit, u, q1, q2, ref);
             res->appendPath(makePath(device,problem, previous, current));
             previous = current;
@@ -751,15 +746,29 @@ namespace hpp {
         return res;
     }
 
+   core::PathVectorPtr_t generateComPath(RbPrmFullBodyPtr_t& fullBody, core::ProblemSolverPtr_t problemSolver, const hpp::floatSeqSeq& rootPositions,
+                          const model::Configuration_t q1, const model::Configuration_t q2) throw (hpp::Error)
+    {
+        try
+        {
+            T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
+            return addRotations(positions,q1,q2,fullBody->device_->currentConfiguration(),
+                                                 fullBody->device_,problemSolver->problem());
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
+
     CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
                           const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
     {
         try
         {
             model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
-            T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
-            return problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
-                                                 fullBody_->device_,problemSolver_->problem()));
+            return problemSolver_->addPath(generateComPath(fullBody_, problemSolver_, rootPositions, q1, q2));
         }
         catch(std::runtime_error& e)
         {
@@ -789,10 +798,29 @@ namespace hpp {
                  lx, -ly, 0,
                 -lx, -ly, 0,
                 -lx,  ly, 0;
-            const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
-            for(int i =0; i< 3; ++i)
+            //create rotation matrix from normal
+            fcl::Vec3f z_fcl = state.contactNormals_.at(name);
+            Eigen::Vector3d z,x,y;
+            for(int i =0; i<3; ++i) z[i] = z_fcl[i];
+            x = z.cross(Eigen::Vector3d(0,-1,0));
+            if(x.norm() < 10e-6)
+            {
+                y = z.cross(fcl::Vec3f(1,0,0));
+                y.normalize();
+                x = y.cross(z);
+            }
+            else
+            {
+                x.normalize();
+                y = z.cross(x);
+            }
+            R.block<3,1>(0,0) = x;
+            R.block<3,1>(0,1) = y;
+            R.block<3,1>(0,2) = z;
+            //const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
+            /*for(int i =0; i< 3; ++i)
                 for(int j =0; j<3;++j)
-                    R(i,j) = fclRotation(i,j);
+                    R(i,j) = fclRotation(i,j);*/
             for(std::size_t i =0; i<4; ++i)
             {
                 res.push_back(position + (R*p.row(i).transpose()));
@@ -996,6 +1024,79 @@ assert(s2 == s1 +1);
         }
     }
 
+    CORBA::Short RbprmBuilder::comRRTFromPos(double state1,
+                                       const hpp::floatSeqSeq& rootPositions1,
+                                       const hpp::floatSeqSeq& rootPositions2,
+                                       const hpp::floatSeqSeq& rootPositions3,
+                                       unsigned short numOptimizations) throw (hpp::Error)
+    {
+        try
+        {
+            std::size_t s1((std::size_t)state1), s2((std::size_t)state1+1);
+            if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
+            {
+                throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
+            }
+            const State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
+            // first compute com paths.
+            std::vector<core::PathVectorPtr_t> paths;
+            paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions1,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
+            paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions2,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
+            paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions3,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
+
+            std::vector<State> states;
+            states.push_back(state1);
+            State s1Bis(state1);
+            s1Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
+            states.push_back(s1Bis);
+
+            State s1Ter(s1Bis);
+            s1Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
+            states.push_back(s1Ter);
+
+            State s2Bis(state2);
+            s2Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
+            states.push_back(s2Bis);
+
+            State s2Ter(s2Bis);
+            s2Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
+            states.push_back(s2Ter);
+
+            states.push_back(state2);
+            std::size_t i =0;
+            core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
+            /*for(std::vector<State>::const_iterator cit = states.begin(); cit != states.end();cit=cit+2, ++i)
+            {
+                resPath->appendPath(interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[i],
+                                                        *(cit),*(cit+1), numOptimizations));
+            }*/
+            core::WeighedDistancePtr_t distance = core::WeighedDistance::create(fullBody_->device_);
+            /*resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Bis.configuration_,
+                                                           (*distance)(state1.configuration_,s1Bis.configuration_)));
+            resPath->appendPath(core::StraightPath::create(fullBody_->device_,s1Bis.configuration_,s1Ter.configuration_,
+                                                           (*distance)(s1Bis.configuration_,s1Ter.configuration_)));*/
+            resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Ter.configuration_, 0.5));
+                                                                       //(*distance)(state1.configuration_,s1Ter.configuration_)));
+            resPath->appendPath(interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
+            s1Ter,s2Bis, numOptimizations));
+            /*resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Bis.configuration_,s2Ter.configuration_,
+                                                           (*distance)(s2Bis.configuration_,s2Ter.configuration_)));
+            resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Ter.configuration_,state2.configuration_,
+                                                           (*distance)(s2Ter.configuration_,state2.configuration_)));*/
+            resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Bis.configuration_,state2.configuration_, 0.5));
+                                                           //(*distance)(s2Bis.configuration_,state2.configuration_)));
+            /*T_State tg; tg.push_back(s1Bis);
+            tg.push_back(s2Bis);
+            resPath->appendPath(interpolation::limbRRT(fullBody_,problemSolver_->problem(),
+                                                    tg.begin(),tg.begin()+1, numOptimizations));*/
+            return AddPath(resPath,problemSolver_);
+        }
+        catch(std::runtime_error& e)
+        {
+            throw Error(e.what());
+        }
+    }
+
     hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error)
     {
         try

src/rbprmbuilder.impl.hh

@@ -149,6 +149,11 @@ namespace hpp {
         virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
         virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
         virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
+        virtual CORBA::Short comRRTFromPos(double state1,
+                                           const hpp::floatSeqSeq& rootPositions1,
+                                           const hpp::floatSeqSeq& rootPositions2,
+                                           const hpp::floatSeqSeq& rootPositions3,
+                                           unsigned short numOptimizations) throw (hpp::Error);
         virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);
         virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
         virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);