Skip to content

GitLab

Commit 74e77191 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

effector spline interpolation now functional

parent 803738f1
Hide whitespace changes
Inline Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ 74e77191
......@@ -328,6 +328,24 @@ module hpp
in floatSeqSeq rootPositions3,
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
floatSeq effectorRRT(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
View file @ 74e77191
......@@ -81,7 +81,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.09, 1, 10)
configs = fullBody.interpolate(0.12, 1, 10)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
......@@ -112,15 +112,16 @@ def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pp.publisher.client.gui.refresh()
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
res = []
trajec = []
trajec_mil = []
contacts = []
pos = []
normals = []
......@@ -138,27 +139,30 @@ def getContactPerPhase(stateid):
contacts[2]+=[v['effector']]
return contacts
def gencontactsPerFrame(stateid, path_ids, total_time_per_path, dt_framerate=1./24.):
def gencontactsPerFrame(stateid, path_ids, times, dt_framerate=1./24.):
contactsPerPhase = getContactPerPhase(stateid)
config_size = len(fullBody.getCurrentConfig())
interpassed = False
res = []
for path_id in path_ids:
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time_per_path / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [1]
num_frames_required_fly = times[1] / dt_framerate
num_frames_required_support = times[0] / dt_framerate
dt_fly = float(length) / num_frames_required_fly
dt_support = float(length) / num_frames_required_support
dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
interpassed = True
res+= [contactsPerPhase[1] for t in dt_finals]
res+= [contactsPerPhase[1] for t in dt_finals_fly]
elif interpassed:
res+= [contactsPerPhase[2] for t in dt_finals]
res+= [contactsPerPhase[2] for t in dt_finals_support]
else:
res+= [contactsPerPhase[0] for t in dt_finals]
res+= [contactsPerPhase[0] for t in dt_finals_support]
return res
def genPandNperFrame(stateid, path_ids, total_time_per_path, dt_framerate=1./24.):
def genPandNperFrame(stateid, path_ids, times, dt_framerate=1./24.):
p, N= fullBody.computeContactPoints(stateid)
config_size = len(fullBody.getCurrentConfig())
interpassed = False
......@@ -166,72 +170,144 @@ def genPandNperFrame(stateid, path_ids, total_time_per_path, dt_framerate=1./24.
nRes = []
for path_id in path_ids:
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time_per_path / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [1]
num_frames_required_fly = times[1] / dt_framerate
num_frames_required_support = times[0] / dt_framerate
dt_fly = float(length) / num_frames_required_fly
dt_support = float(length) / num_frames_required_support
dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
interpassed = True
pRes+= [p[1] for t in dt_finals]
nRes+= [N[1] for t in dt_finals]
pRes+= [p[1] for t in dt_finals_fly]
nRes+= [N[1] for t in dt_finals_fly]
elif interpassed:
pRes+= [p[2] for t in dt_finals]
nRes+= [N[2] for t in dt_finals]
pRes+= [p[2] for t in dt_finals_support]
nRes+= [N[2] for t in dt_finals_support]
else:
pRes+= [p[0] for t in dt_finals]
nRes+= [N[0] for t in dt_finals]
pRes+= [p[0] for t in dt_finals_support]
nRes+= [N[0] for t in dt_finals_support]
return pRes, nRes
from hpp import Error as hpperr
import sys
numerror = 0
def act(i, optim):
def act(i, optim, time_scale = 20.):
global numerror
global errorid
fail = 0
try:
total_time_per_path = 1.2
pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.4, 0.2, total_time_per_path, False, optim, False, False)
print "distance", fullBody.getEffectorDistance(i,i+1)
trunk_distance = np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
dist = int(distance * time_scale)#heuristic
while(dist %4 != 0):
dist +=1
total_time_flying_path = max(float(dist)/10., 0.3)
total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.2)*10.))) / 10.
times = [total_time_support_path, total_time_flying_path]
if(total_time_flying_path>= 1.):
dt = 0.1
elif total_time_flying_path<= 0.3:
dt = 0.05
else:
dt = 0.1
print 'time per path', times
print 'dt', dt
#~ total_time_per_path = 1.2
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 1, 0.2, total_time_per_path, False, optim, False, False)
if(distance > 0.0001):
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
pid, trajectory = solve_effector_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
global trajec_mil
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, times)
trajec_mil = trajec_mil + gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
global contacts
contacts += gencontactsPerFrame(i, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(i, trajectory, times, 1./1000.)
global pos
pos += Ps
global normals
normals+= Ns
assert(len(contacts) == len(trajec_mil) and len(contacts) == len(pos) and len(normals) == len(pos))
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
except hpperr as e:
print "hpperr failed at id " + str(i) , e.strerror
numerror+=1
errorid += [i]
fail+=1
#~ except ValueError as e:
#~ print "ValueError failed at id " + str(i) , e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except IndexError as e:
#~ print "IndexError failed at id " + str(i) , e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except Exception as e:
#~ print e
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
#~ except:
#~ numerror+=1
#~ errorid += [i]
#~ fail+=1
return fail
def actu(i, optim):
global numerror
global errorid
fail = 0
print "distance", fullBody.getEffectorDistance(i,i+1)
trunk_distance = np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
dist = int(distance * 20.)#heuristic
while(dist %4 != 0):
dist +=1
total_time_flying_path = max(float(dist)/10., 0.3)
total_time_support_path = float((int)(math.ceil(min(total_time_flying_path /2., 0.4)*10.))) / 10.
times = [total_time_support_path, total_time_flying_path]
if(total_time_flying_path>= 1.):
dt = 0.2
elif total_time_flying_path<= 0.3:
dt = 0.05
else:
dt = 0.1
print 'time per path', times
print 'dt', dt
#~ total_time_per_path = 1.2
#~ pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 1, 0.2, total_time_per_path, False, optim, False, False)
if(distance > 0.0001):
pid, trajectory = solve_com_RRT(fullBody, configs, i, True, 0.4, dt, times, False, optim, False, False)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
global trajec
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, total_time_per_path)
global trajec_mil
trajec = trajec + gen_trajectory_to_play(fullBody, pp, trajectory, times)
trajec_mil = trajec_mil + gen_trajectory_to_play(fullBody, pp, trajectory, times, 1./1000.)
global contacts
contacts += gencontactsPerFrame(i, trajectory, total_time_per_path)
Ps, Ns = genPandNperFrame(i, trajectory, total_time_per_path)
contacts += gencontactsPerFrame(i, trajectory, times, 1./1000.)
Ps, Ns = genPandNperFrame(i, trajectory, times, 1./1000.)
global pos
pos += Ps
global normals
normals+= Ns
assert(len(contacts) == len(trajec) and len(contacts) == len(pos) and len(normals) == len(pos))
except hpperr as e:
print "failed at id " + str(i) , e.strerror
numerror+=1
errorid += [i]
fail+=1
except ValueError as e:
print "failed at id " + str(i) , e
numerror+=1
errorid += [i]
fail+=1
except IndexError as e:
print "failed at id " + str(i) , e
numerror+=1
errorid += [i]
fail+=1
except Exception as e:
print e
numerror+=1
errorid += [i]
fail+=1
except:
numerror+=1
errorid += [i]
fail+=1
return fail
assert(len(contacts) == len(trajec_mil) and len(contacts) == len(pos) and len(normals) == len(pos))
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
def displayInSave(pp, pathId, configs):
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
......@@ -248,7 +324,7 @@ import time
from pickle import dump
def saveToPinocchio(filename):
res = []
for i, q_gep in enumerate(trajec):
for i, q_gep in enumerate(trajec_mil):
#invert to pinocchio config:
q = q_gep[:]
quat_end = q[4:7]
......@@ -263,15 +339,18 @@ def saveToPinocchio(filename):
def clean():
global res
global trajec
global trajec_mil
global contacts
global errorid
global pos
global normals
res = []
trajec = []
trajec_mil = []
contacts = []
errorid = []
pos = []
normals = []
#~ saveToPinocchio('darpahyq_andrea')
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
script/scenarios/darpa_hyq_path.py
View file @ 74e77191
......@@ -38,6 +38,7 @@ q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
......@@ -46,14 +47,14 @@ ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "pling", r)
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
#~ r.loadObstacleModel (packageName, "darpa", "planning")
# Solve the problem
t = ps.solve ()
......
script/scenarios/darpa_hyq_quan.py 0 → 100644
View file @ 74e77191
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
from quang_info import write_state
#calling script darpa_hyq_path to compute root path
import darpa_hyq_path as tp
packageName = "hyq_description"
meshPackageName = "hyq_description"
rootJointType = "freeflyer"
# Information to retrieve urdf and srdf files.
urdfName = "hyq"
urdfSuffix = ""
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
# Setting a number of sample configurations used
nbSamples = 20000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps)
rootName = 'base_joint_xyz'
# Creating limbs
# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
cType = "_3_DOF"
# string identifying the limb
rLegId = 'rfleg'
# First joint of the limb, as in urdf file
rLeg = 'rf_haa_joint'
# Last joint of the limb, as in urdf file
rfoot = 'rf_foot_joint'
# Specifying the distance between last joint and contact surface
offset = [0.,-0.021,0.]
# Specifying the contact surface direction when the limb is in rest pose
normal = [0,1,0]
# Specifying the rectangular contact surface length
legx = 0.02; legy = 0.02
# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
lLegId = 'lhleg'
lLeg = 'lh_haa_joint'
lfoot = 'lh_foot_joint'
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},
lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},
rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},
larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
i = 0;
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 *
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:
write_state(r, "darpa"+str(i), fullBody, configs, i, True, 0.5, 0.2, True, False, limbsCOMConstraints)
#~ pid = solve_com_RRT(fullBody, configs, i, True, 1, 0.2, False, optim, False, True, limbsCOMConstraints)
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"
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 74e77191
......@@ -410,6 +410,22 @@ class FullBody (object):
def comRRTFromPos(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
return self.client.rbprm.rbprm.comRRTFromPos(state1, comPos1, comPos2, comPos3, 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 effectorRRT(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
return self.client.rbprm.rbprm.effectorRRT(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.
# Will fail if the index of the state does not exist.
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ 74e77191
......@@ -178,6 +178,14 @@ def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1]
def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
comPosPerPhase = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints)
print "done. generating state trajectory ",state_id
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1]
#~ from multiprocessing import Process
#~ def solve_com_RRTs(fullBody, states, state_ids, computeCones = False, mu = 1, dt =0.1, phase_dt = 1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
......
src/rbprmbuilder.impl.cc
View file @ 74e77191
......@@ -24,6 +24,7 @@
#include "hpp/rbprm/interpolation/rbprm-path-interpolation.hh"
#include "hpp/rbprm/interpolation/limb-rrt.hh"
#include "hpp/rbprm/interpolation/com-rrt.hh"
#include "hpp/rbprm/interpolation/spline/effector-rrt.hh"
#include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
......@@ -1296,6 +1297,99 @@ assert(s2 == s1 +1);
}
}
hpp::floatSeq* RbprmBuilder::effectorRRT(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
unsigned short numOptimizations) throw (hpp::Error)
{
try
{
std::vector<CORBA::Short> pathsIds;
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);