diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index b1fa4c416a150e0d48bbf3a5d6aa5689b6b91ba6..76c261149a85eac358d469e915bc10ce2af847a7 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -121,7 +121,7 @@ module hpp
///
/// \param stateId target state
/// \param com target com
- double projectStateToCOM(in unsigned short stateId, in floatSeq com) raises (Error);
+ double projectStateToCOM(in unsigned short stateId, in floatSeq com, in unsigned short max_num_sample) raises (Error);
/// Project a state into a COM
///
@@ -413,7 +413,7 @@ module hpp
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.
+ /// between two indicated states. The states do need to be consecutive.
/// 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.
@@ -430,6 +430,26 @@ module hpp
in unsigned short comTraj3,
in unsigned short numOptimizations) raises (Error);
+
+ /// Provided a path has already been computed and interpolated, generate a continuous path
+ /// between three indicated states. The states do not need to be consecutive.
+ /// 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 state2 index of end 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 comRRTFromPosBetweenState(in double state1,in double state2,
+ in unsigned short comTraj1,
+ in unsigned short comTraj2,
+ in unsigned short comTraj3,
+ 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
@@ -474,8 +494,9 @@ module hpp
/// Will fail if the index of the state does not exist.
/// \param state index of first state.
/// \param targetCom 3D vector for the com position
+ /// \param max_num_sample number of samples that can be used at worst before failing
/// \return projected configuration
- floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
+ floatSeq projectToCom(in double state, in floatSeq targetCom, in unsigned short max_num_sample) raises (Error);
/// Retrieve the configuration at a given state
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
@@ -495,6 +516,12 @@ module hpp
/// \return whether the limb is in contact at this state
short isLimbInContactIntermediary(in string limbname, in double state1) raises (Error);
+ /// Generate intermediary state
+ /// \param state1 initial state considered
+ /// \param state2 target state considered
+ /// \return whether the limb is in contact at this state
+ short computeIntermediary(in unsigned short state1, in unsigned short state2) raises (Error);
+
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
diff --git a/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py b/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
index eb2562cc6485d33583d961b691251b369e7f17f1..e076b7c730bb67cb9f4eab43826dfcfe91cac6ed 100644
--- a/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
+++ b/script/scenarios/demos/siggraph_asia/twister/bezier_traj.py
@@ -2,14 +2,7 @@ from gen_data_from_rbprm import *
from hpp.corbaserver.rbprm.tools.com_constraints import get_com_constraint
from hpp.gepetto import PathPlayer
-
-#computing com bounds 0 and 1
-def __get_com(robot, config):
- save = robot.getCurrentConfig()
- robot.setCurrentConfig(config)
- com = robot.getCenterOfMass()
- robot.setCurrentConfig(save)
- return com
+from hpp.corbaserver.rbprm.state_alg import computeIntermediateState
from numpy import matrix, asarray
from numpy.linalg import norm
@@ -47,18 +40,21 @@ def play_all_paths_qs():
if i % 2 == 0 :
ppl(pid)
-def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0, effector = False, mu=0.5) :
- com_1 = __get_com(fullBody, fullBody.getConfigAtState(stateid))
- com_2 = __get_com(fullBody, fullBody.getConfigAtState(stateid+1))
+def test(s1,s2, path = False, use_rand = False, just_one_curve = False, num_optim = 0, effector = False, mu=0.5) :
+ q1 = s1.q()
+ q2 = s2.q()
+ stateid = s1.sId
+ stateid1 = s2.sId
+ sInt = computeIntermediateState(s1,s2)
+ com_1 = s1.getCenterOfMass()
+ com_2 = s2.getCenterOfMass()
createPtBox(viewer.client.gui, 0, com_1, 0.01, [0,1,1,1.])
createPtBox(viewer.client.gui, 0, com_2, 0.01, [0,1,1,1.])
- data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = mu)
- c_bounds_1 = get_com_constraint(fullBody, stateid, fullBody.getConfigAtState(stateid), limbsCOMConstraints, interm = False)
- c_bounds_mid = get_com_constraint(fullBody, stateid, fullBody.getConfigAtState(stateid), limbsCOMConstraints, interm = True)
- c_bounds_2 = get_com_constraint(fullBody, stateid, fullBody.getConfigAtState(stateid+1), limbsCOMConstraints, interm = False)
+ data = gen_sequence_data_from_state_objects(s1,s2,sInt,mu = mu)
+ c_bounds_1 = s1.getComConstraint(limbsCOMConstraints)
+ c_bounds_mid = sInt.getComConstraint(limbsCOMConstraints)
+ c_bounds_2 = s2.getComConstraint(limbsCOMConstraints)
success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = mu)
- #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
- #~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ calling effector", effector
paths_ids = []
@@ -71,15 +67,24 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
createPtBox(viewer.client.gui, 0, c_mid_1[0].tolist(), 0.01, [0,1,0,1.])
createPtBox(viewer.client.gui, 0, c_mid_2[0].tolist(), 0.01, [0,1,0,1.])
- partions = [0.,0.2,0.8,1.]
+ partions = [0.,0.1,0.9,1.]
p0 = fullBody.generateCurveTrajParts(bezier_0,partions)
#testing intermediary configurations
- print 'wtf', partions[1], " "
+ print 'paritions:', partions[1], " "
com_interm1 = curve(partions[1])
com_interm2 = curve(partions[2])
print "com_1", com_1
+ print "com_1", curve(partions[0])
+ print "com_interm1", com_interm1
+ print "com_interm2", com_interm2
+ print "com_2", com_2
+ print "com_2", curve(partions[-1])
success_proj1 = project_com_colfree(fullBody, stateid , asarray((com_interm1).transpose()).tolist()[0])
- success_proj2 = project_com_colfree(fullBody, stateid+1, asarray((com_interm2).transpose()).tolist()[0])
+ success_proj2 = project_com_colfree(fullBody, stateid1 , asarray((com_interm2).transpose()).tolist()[0])
+
+ #~ if success_proj1:
+ #~ q_1 = fullBody.projectToCom(stateid, asarray((com_interm1).transpose()).tolist()[0])
+ #~ viewer(q_1)
if not success_proj1:
print "proj 1 failed"
@@ -92,20 +97,20 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
print "p0", p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
- ppl.displayPath(p0)
- #~ pp.displayPath(p0+1)
- #~ pp.displayPath(p0+2)
- #~ pp.displayPath(p0+3)
+ #~ ppl.displayPath(p0)
+ ppl.displayPath(p0+1)
+ ppl.displayPath(p0+2)
+ ppl.displayPath(p0+3)
if(effector):
- print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ calling effector"
- paths_ids = [int(el) for el in fullBody.effectorRRT(stateid,p0+1,p0+2,p0+3,num_optim)]
+ assert False, "Cant deal with effectors right now"
+ #~ paths_ids = [int(el) for el in fullBody.effectorRRT(stateid,p0+1,p0+2,p0+3,num_optim)]
else:
- paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)]
+ paths_ids = [int(el) for el in fullBody.comRRTFromPosBetweenState(stateid,stateid1,p0+1,p0+2,p0+3,num_optim)]
else:
success_proj1 = project_com_colfree(fullBody, stateid , c_mid_1[0].tolist())
- success_proj2 = project_com_colfree(fullBody, stateid+1, c_mid_2[0].tolist())
+ success_proj2 = project_com_colfree(fullBody, stateid1 , c_mid_2[0].tolist())
if not success_proj1:
print "proj 1 failed"
@@ -127,7 +132,7 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
ppl.displayPath(p0)
ppl.displayPath(p0+1)
ppl.displayPath(p0+2)
- paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2,num_optim)]
+ paths_ids = [int(el) for el in fullBody.comRRTFromPosBetweenState(stateid,stateid1, p0,p0+1,p0+2,num_optim)]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
@@ -144,14 +149,14 @@ from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
def createPtBox(gui, winId, config, res = 0.01, color = [1,1,1,0.3]):
resolution = res
- #~ global scene
- #~ global b_id
- #~ boxname = scene+"/"+str(b_id)
- #~ b_id += 1
- #~ gui.addBox(boxname,resolution,resolution,resolution, color)
- #~ gui.applyConfiguration(boxname,[config[0],config[1],config[2],1,0,0,0])
- #~ gui.addSceneToWindow(scene,winId)
- #~ gui.refresh()
+ global scene
+ global b_id
+ boxname = scene+"/"+str(b_id)
+ b_id += 1
+ gui.addBox(boxname,resolution,resolution,resolution, color)
+ gui.applyConfiguration(boxname,[config[0],config[1],config[2],1,0,0,0])
+ gui.addSceneToWindow(scene,winId)
+ gui.refresh()
def test_ineq(stateid, constraints, n_samples = 10, color=[1,1,1,1.]):
Kin = get_com_constraint(fullBody, stateid, fullBody.getConfigAtState(stateid), constraints, interm = False)
@@ -174,27 +179,27 @@ def test_ineq(stateid, constraints, n_samples = 10, color=[1,1,1,1.]):
#~ test_ineq(0, limbsCOMConstraints, 1000, [0,1,1,1])
-def gen(start = 0, len_con = 1, num_optim = 0, ine_curve =True, s = 1., effector = False, mu =0.5, gen_traj = True):
+def gen(s1, s2, num_optim = 0, ine_curve =True, s = 1., effector = False, mu =0.5, gen_traj = True):
n_fail = 0;
- for i in range (start, start+len_con):
- #~ viewer(configs[i])
- res = test(i, True, False, ine_curve,num_optim, effector, mu)
- if(not res[0]):
- print "lp failed"
- createPtBox(viewer.client.gui, 0, res[1][0], 0.01, [1,0,0,1.])
- createPtBox(viewer.client.gui, 0, res[2][0], 0.01, [1,0,0,1.])
- found = False
- for j in range(10):
- res = test(i, True, True, ine_curve, num_optim, effector, mu)
- createPtBox(viewer.client.gui, 0, res[1][0], 0.01, [0,1,0,1.])
- createPtBox(viewer.client.gui, 0, res[2][0], 0.01, [0,1,0,1.])
- if res[0]:
- break
- if not res[0]:
- n_fail += 1
+ #~ viewer(configs[i])
+ res = test(s1, s2, True, False, ine_curve,num_optim, effector, mu)
+ if(not res[0]):
+ print "lp failed"
+ createPtBox(viewer.client.gui, 0, res[1][0], 0.01, [1,0,0,1.])
+ createPtBox(viewer.client.gui, 0, res[2][0], 0.01, [1,0,0,1.])
+ found = False
+ for j in range(1):
+ res = test(s1, s2, True, True, ine_curve, num_optim, effector, mu)
+ createPtBox(viewer.client.gui, 0, res[1][0], 0.01, [0,1,0,1.])
+ createPtBox(viewer.client.gui, 0, res[2][0], 0.01, [0,1,0,1.])
+ if res[0]:
+ break
+ if not res[0]:
+ n_fail += 1
print "n_fail ", n_fail
if(gen_traj):
a = gen_trajectory_to_play(fullBody, ppl, allpaths, flatten([[s*0.2, s* 0.6, s* 0.2] for _ in range(len(allpaths) / 3)]))
+ #~ a = gen_trajectory_to_play(fullBody, ppl, allpaths, flatten([[s] for _ in range(len(allpaths) )]))
return a
@@ -432,83 +437,23 @@ com_acc = [0.,0.,0.]
vels = []
accs = []
-#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
-#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
-#~ gen(0,1)
-
path = []
a_s = []
-def go(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- global a_s
- a = []
- for l in range(sid,sid+rg):
- print "STATE ", l
- s = max(norm(array(fullBody.getConfigAtState(sid+1)) - array(fullBody.getConfigAtState(sid))), 1.) * 1
- a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- print "STATE ", sid+rg
- #~ path,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- return a
-
-def go_stop(sid, rg = 2, num_optim = 0, mu = 0.6, window = 2, s = None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- global a_s
- a = []
- for l in range(sid,sid+rg):
- print "STATE ", l
- s = max(norm(array(fullBody.getConfigAtState(sid+1)) - array(fullBody.getConfigAtState(sid))), 1.) * 1
- a,com_vel,com_acc = gen_several_states_partial(l,window,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc, path=True)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- print "STATE ", sid+rg
- s = max(norm(array(configs[sid+rg+1]) - array(configs[sid+rg])), 1.) * 1
- a,com_vel,com_acc = gen_several_states(sid+rg,1,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- a_s+=[a]
- vels += [com_vel[:]]
- accs += [com_acc[:]]
- return a
-
-def go0(sid, rg, num_optim = 0, mu = 0.6, s =None):
- global com_vel
- global com_acc
- global vels
- global accs
- global path
- if s == None:
- s = max(norm(array(fullBody.getConfigAtState(sid+1)) - array(fullBody.getConfigAtState(sid))), 1.) * 1.5
- print "$$$$$$$$$$$$$$$ S $$$$$$$$ *********************444444444444444444444444444 ", s
- for i in range(rg):
- path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s)
- return path
-def go2(sid, rg = 1, num_optim = 0, mu = 0.5, t =2, s =None):
+
+def go0(states, num_optim = 0, mu = 0.6, s =None):
global com_vel
global com_acc
global vels
global accs
- global path
- for i in range(rg):
- if s == None:
- s = max(norm(array(fullBody.getConfigAtState(sid+1)) - array(fullBody.getConfigAtState(sid))), 1.) * 0.6
- print "$$$$$$$$$$$$$$$ S $$$$$$$$ ", s
- path,com_vel,com_acc = gen_several_states(sid+i,sid+i+t,mu=mu,num_optim=num_optim, s=s,init_vel=com_vel, init_acc=com_acc)
- vels += [com_vel[:]]
- accs += [com_acc[:]]
+ global path
+ sc = s
+ for i, el in enumerate(states[:-1]):
+ if s == None:
+ sc = max(norm(array(states[i+1].q()) - array(el.q())), 1.) * 1
+ path = gen(el,states[i+1],mu=mu,num_optim=num_optim, s=sc)
return path
+
def reset():
global com_vel
diff --git a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
index da4764fa39aa11f3e32285497cec8a0bb4f1d92c..5ebb8f1c9c1d023f2b24083af71f8054f60815e7 100644
--- a/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
+++ b/script/scenarios/demos/siggraph_asia/twister/twister_spidey.py
@@ -26,7 +26,7 @@ q_0 = fullBody.getCurrentConfig(); r(q_0)
q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
-#~ q_init = fullBody.generateContacts(q_init, [0,0,1])
+q_init = fullBody.generateContacts(q_init, [0,0,1])
qs = []
fb = fullBody
ttp = path_planner
@@ -45,45 +45,44 @@ s1 = State(fullBody,q=q_init, limbsIncontact = [rLegId])
q0 = s1.q()[:]
-def test(p, n):
- global s1
- t0 = time.clock()
- a, success = addNewContact(s1,rLegId,p, n)
- print "projecttion successfull ? ", success
- t1 = time.clock()
- val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints)
- t2 = time.clock()
- print 'is reachable ? ', val, com
- if(val):
- com[2]+=0.1
- if(success > 0):
- print 'a > 0'
- t3 = time.clock()
- q = fullBody.projectStateToCOM(a.sId, com.tolist())
- t4= time.clock()
- r(a.q())
- else:
- print "using s1 ", s1.sId
- t3 = time.clock()
- q = fullBody.projectStateToCOM(s1.sId, com.tolist())
- t4= time.clock()
- a, succ = addNewContact(s1,rLegId,p, n)
- print "success projection ??", succ
- if (succ):
- r(a.q())
- print "time to addnc", t2 - t1
- print "projectcom succesfull ?", q
- print "time to check reachable", t3- t2
- print "time to project", t4- t3
+#~ def test(p, n):
+ #~ global s1
+ #~ t0 = time.clock()
+ #~ a, success = addNewContact(s1,rLegId,p, n)
+ #~ print "projecttion successfull ? ", success
+ #~ t1 = time.clock()
+ #~ val, com = isContactReachable(s1, rLegId,p, n, limbsCOMConstraints)
+ #~ t2 = time.clock()
+ #~ print 'is reachable ? ', val, com
+ #~ if(val):
+ #~ com[2]+=0.1
+ #~ if(success > 0):
+ #~ print 'a > 0'
+ #~ t3 = time.clock()
+ #~ q = fullBody.projectStateToCOM(a.sId, com.tolist())
+ #~ t4= time.clock()
+ #~ r(a.q())
+ #~ else:
+ #~ print "using s1 ", s1.sId
+ #~ t3 = time.clock()
+ #~ q = fullBody.projectStateToCOM(s1.sId, com.tolist())
+ #~ t4= time.clock()
+ #~ a, succ = addNewContact(s1,rLegId,p, n)
+ #~ if (succ):
+ #~ r(a.q())
+ #~ print "time to addnc", t2 - t1
+ #~ print "projectcom succesfull ?", q
+ #~ print "time to check reachable", t3- t2
+ #~ print "time to project", t4- t3
a = computeAffordanceCentroids(tp.afftool, ["Support"])
-def computeNext(state, limb, max_num_samples = 10):
+def computeNext(state, limb, projectToCom = False, max_num_samples = 10):
global a
t1 = time.clock()
#~ candidates = [el for el in a if isContactReachable(state, limb, el[0], el[1], limbsCOMConstraints)[0] ]
#~ print "num candidates", len(candidates)
#~ t3 = time.clock()
- results = [addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints, max_num_samples) for el in a]
+ results = [addNewContactIfReachable(state, limb, el[0], el[1], limbsCOMConstraints, projectToCom, max_num_samples) for el in a]
t2 = time.clock()
#~ t4 = time.clock()
resultsFinal = [el[0] for el in results if el[1]]
@@ -114,11 +113,34 @@ def plot_feasible(state):
i = 0
s0 = removeContact(s1,rLegId)[0]
-res = computeNext(s1, rLegId)
-s1 = res[2]
-s2 = removeContact(s1,lLegId)[0]
-res = computeNext(s0,larmId)
+s_init = computeNext(s0,rLegId, True,20)
+res = computeNext(s0,larmId, True,20)
+s1 = res[0]
+#~ res2 = computeNext(s1,rarmId, True,20)
+s2 = computeNext(s1,rarmId, True,100)[0]
+
+all_states=[s1,s2]
#~ s2 = removeContact(s2,rLegId)[0]
#~ s3 = computeNext(s2, larmId)[0]
#~ go0(s2.sId,1, s=1)
#~ plot_feasible(s1)
+from time import sleep
+def play():
+ for i,el in enumerate(all_states):
+ r(el.q())
+ sleep(0.5)
+ i = len(all_states)-1;
+ for j in range(i+1):
+ print "ij,sum", i, j, i-j
+ r(all_states[i-j].q())
+ sleep(0.5)
+
+play()
+
+print "init valid ?", fullBody.isConfigValid(s1.q())
+print "end valid ?", fullBody.isConfigValid(s2.q())
+
+r(q_init)
+path = go0([s2,s1], mu=0.3,num_optim=1)
+
+
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index eb01bde4c1cd5db4e834ab4058dcaeb839790a8f..16271ea497d5792a26074af2a61d2d6cbd429304 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -572,6 +572,21 @@ 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 comRRTFromPosBetweenState(self, state1, state2, comPos1, comPos2, comPos3, numOptim = 10):
+ return self.client.rbprm.rbprm.comRRTFromPosBetweenState(state1, state2, 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.
@@ -610,8 +625,8 @@ class FullBody (object):
# \param state index of first state.
# \param targetCom 3D vector for the com position
# \return projected configuration
- def projectToCom(self, state, targetCom):
- return self.client.rbprm.rbprm.projectToCom(state, targetCom)
+ def projectToCom(self, state, targetCom, maxNumSample = 0):
+ return self.client.rbprm.rbprm.projectToCom(state, targetCom, maxNumSample)
## Returns the configuration at a given state
# Will fail if the index of the state does not exist.
@@ -625,8 +640,8 @@ class FullBody (object):
# \param state index of first state.
# \param targetCom 3D vector for the com position
# \return whether the projection was successful
- def projectStateToCOM(self, state, targetCom):
- return self.client.rbprm.rbprm.projectStateToCOM(state, targetCom) > 0
+ def projectStateToCOM(self, state, targetCom, maxNumSample = 0):
+ return self.client.rbprm.rbprm.projectStateToCOM(state, targetCom, maxNumSample) > 0
## Project a given state into a given COM position
# and update the state configuration.
diff --git a/src/hpp/corbaserver/rbprm/rbprmstate.py b/src/hpp/corbaserver/rbprm/rbprmstate.py
index 81c0a218da27eaf46b990492438bed70f960285f..93c1b6796dca090aa87d9b9e6f3e25c8a0d38578 100644
--- a/src/hpp/corbaserver/rbprm/rbprmstate.py
+++ b/src/hpp/corbaserver/rbprm/rbprmstate.py
@@ -132,5 +132,11 @@ class State (object):
self.H_h = array(rawdata)
return self.H_h[:,:-1], self.H_h[:, -1]
+ def projectToCOM(self, targetCom, toNewState=False):
+ if toNewState:
+ return self.client.rbprm.rbprm.projectToCom(self.sId, targetCom)
+ else:
+ return self.client.rbprm.rbprm.projectStateToCOM(self.sId, targetCom) > 0
+
def getComConstraint(self, limbsCOMConstraints, exceptList = []):
return get_com_constraint(self.fullBody, self.sId, self.cl.getConfigAtState(self.sId), limbsCOMConstraints, interm = self.isIntermediate, exceptList = exceptList)
diff --git a/src/hpp/corbaserver/rbprm/state_alg.py b/src/hpp/corbaserver/rbprm/state_alg.py
index 6566e0732ac3125ec60d5cf24fce7912d478194d..e27db3552c34e4608734138cb15227eb68db01f3 100644
--- a/src/hpp/corbaserver/rbprm/state_alg.py
+++ b/src/hpp/corbaserver/rbprm/state_alg.py
@@ -17,7 +17,7 @@
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm.rbprmstate import State
-from lp_find_point import find_valid_c_cwc, lp_ineq_4D
+from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
from hpp.corbaserver.rbprm.tools.com_constraints import *
## algorithmic methods on state
@@ -57,6 +57,15 @@ def isContactReachable(state, limbName, p, n, limbsCOMConstraints):
return (res[3] >= 0), res[0:3]
+## Computes the intermediary state between two states
+## that is the state where the broken configuration have been remove
+# \param sfrom init state
+# \param sto target state
+# \return whether the creation was successful, as well as the new state
+def computeIntermediateState(sfrom, sto):
+ sid = sfrom.cl.computeIntermediary(sfrom.sId, sto.sId)
+ return State(sfrom.fullBody, sid, False)
+
## tries to add a new contact to the state
## if the limb is already in contact, replace the
## previous contact. Only considers kinematic limitations.
@@ -66,8 +75,8 @@ def isContactReachable(state, limbName, p, n, limbsCOMConstraints):
# \param p 3d position of the point
# \param n 3d normal of the contact location center
# \return (State, success) whether the creation was successful, as well as the new state
-def addNewContact(state, limbName, p, n):
- sId = state.cl.addNewContact(state.sId, limbName, p, n)
+def addNewContact(state, limbName, p, n, num_max_sample = 0):
+ sId = state.cl.addNewContact(state.sId, limbName, p, n, num_max_sample)
if(sId != -1):
return State(state.fullBody, sId = sId), True
return state, False
@@ -79,10 +88,14 @@ def addNewContact(state, limbName, p, n):
# \param state State considered
# \param limbName name of the considered limb to create contact with
# \return (State, success) whether the removal was successful, as well as the new state
-def removeContact(state, limbName):
+def removeContact(state, limbName, projectToCOM = False):
sId = state.cl.removeContact(state.sId, limbName)
- if(sId != -1):
- return State(state.fullBody, sId = sId), True
+ if(sId != -1):
+ s = State(state.fullBody, sId = sId)
+ if projectToCOM:
+ return s, projectToFeasibleCom(s, ddc =[0.,0.,0.], max_num_samples = 10, friction = 0.6)
+ else:
+ return s, True
return state, False
## tries to add a new contact to the state
@@ -94,10 +107,37 @@ def removeContact(state, limbName):
# \param limbName name of the considered limb to create contact with
# \param p 3d position of the point
# \param n 3d normal of the contact location center
+# \param max_num_samples max number of sampling in case projection ends up in collision
# \return (State, success) whether the creation was successful, as well as the new state
-def addNewContactIfReachable(state, limbName, p, n, limbsCOMConstraints):
+def addNewContactIfReachable(state, limbName, p, n, limbsCOMConstraints, projectToCom = False, max_num_samples = 0):
ok, res = isContactReachable(state, limbName, p, n, limbsCOMConstraints)
if(ok):
- return addNewContact(state, limbName, p, n)
+ s, success = addNewContact(state, limbName, p, n, max_num_samples)
+ if success and projectToCom:
+ success = projectToFeasibleCom(s, ddc =[0.,0.,0.], max_num_samples = max_num_samples, friction = 0.6)
+ return s, success
else:
return state, False
+
+## Project a state to a static equilibrium location
+# \param state State considered
+# \param ddc name considered acceleeration (null by default)
+# \param max_num_samples max number of sampling in case projection ends up in collision
+# \param friction considered friction coefficient
+# \return whether the projection was successful
+def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction = 0.6):
+ H, h = state.getContactCone(friction)
+ c_ref = state.getCenterOfMass()
+ res = find_valid_c_cwc_qp(H, c_ref, ddc, state.fullBody.getMass())
+ if res['success']:
+ x = res['x'].tolist()
+ x[2] += 0.5
+ for i in range(10):
+ if state.fullBody.projectStateToCOM(state.sId ,x, max_num_samples):
+ print "success after " + str(i) + " trials"
+ return True
+ else:
+ x[2]-=0.05
+ else:
+ print "qp failed"
+ return False;
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index e13e7aea7f829e5d646d2b626e277df5bee1530c..449c21bd15174d71308f9bced523870c44e40fd4 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -605,7 +605,26 @@ namespace hpp {
}
- double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target) throw (hpp::Error)
+ rbprm::T_Limb GetFreeLimbs(const RbPrmFullBodyPtr_t fullBody, const hpp::rbprm::State &from, const hpp::rbprm::State &to)
+ {
+ rbprm::T_Limb res;
+ std::vector<std::string> fixedContacts = to.fixedContacts(from);
+ std::vector<std::string> variations = to.contactVariations(from);
+ for(rbprm::CIT_Limb cit = fullBody->GetLimbs().begin();
+ cit != fullBody->GetLimbs().end(); ++cit)
+ {
+ if(std::find(fixedContacts.begin(), fixedContacts.end(), cit->first) == fixedContacts.end())
+ {
+ if(std::find(variations.begin(), variations.end(), cit->first) != variations.end())
+ {
+ res.insert(*cit);
+ }
+ }
+ }
+ return res;
+ }
+
+ double RbprmBuilder::projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples) throw (hpp::Error)
{
try
{
@@ -614,9 +633,27 @@ namespace hpp {
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
State s = lastStatesComputed_[stateId];
- bool succes (false);
- hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
- if(succes)
+// /hpp::model::Configuration_t c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
+ rbprm::projection::ProjectionReport rep = rbprm::projection::projectToComPosition(fullBody_,com_target,s);
+ hpp::model::Configuration_t& c = rep.result_.configuration_;
+ ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
+ CollisionValidationPtr_t val = fullBody_->GetCollisionValidation();
+ rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
+ if (! rep.success_ && maxNumeSamples>0)
+ {
+ Configuration_t head = s.configuration_.head<7>();
+ BasicConfigurationShooterPtr_t shooter = BasicConfigurationShooter::create(fullBody_->device_);
+ for(std::size_t i =0; !rep.success_ && i< maxNumeSamples; ++i)
+ {
+ s.configuration_ = *shooter->shoot();
+ s.configuration_.head<7>() = head;
+ //c = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s,com_target,succes);
+ rep = rbprm::projection::projectToComPosition(fullBody_,com_target,s);
+ rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
+ c = rep.result_.configuration_;
+ }
+ }
+ if(rep.success_)
{
lastStatesComputed_[stateId].configuration_ = c;
lastStatesComputedTime_[stateId].second.configuration_ = c;
@@ -655,10 +692,10 @@ namespace hpp {
return lastStatesComputed_.size()-1;
}
- double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error)
+ double RbprmBuilder::projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error)
{
model::Configuration_t com_target = dofArrayToConfig (3, com);
- return projectStateToCOMEigen(stateId, com_target);
+ return projectStateToCOMEigen(stateId, com_target, max_num_sample);
}
hpp::floatSeq* RbprmBuilder::generateContacts(const hpp::floatSeq& configuration,
@@ -1779,25 +1816,30 @@ assert(s2 == s1 +1);
}
}
- core::Configuration_t project_or_throw(rbprm::RbPrmFullBodyPtr_t fulllBody, ProblemPtr_t problem, const State& state, const fcl::Vec3f& targetCom)
+ core::Configuration_t project_or_throw(rbprm::RbPrmFullBodyPtr_t fulllBody, const State& state, const fcl::Vec3f& targetCom, const bool checkCollision = false)
{
- bool success(false);
- core::Configuration_t res = rbprm::interpolation::projectOnCom(fulllBody, problem,state,targetCom, success);
- if(!success)
+ rbprm::projection::ProjectionReport rep;
+ if(checkCollision)
+ rep =rbprm::projection::projectToColFreeComPosition(fulllBody, targetCom, state);
+ else
+ rep= rbprm::projection::projectToComPosition(fulllBody, targetCom, state);
+ core::Configuration_t res = rep.result_.configuration_;
+ if(!rep.success_)
{
+ std::cout << "projection failed in project or throw " << std::endl;
throw std::runtime_error("could not project state on COM constraint");
}
return res;
}
- hpp::floatSeq* RbprmBuilder::rrt(t_rrt functor, double state1,
+ hpp::floatSeq* RbprmBuilder::rrt(t_rrt functor, double state1, double state2,
unsigned short cT1, unsigned short cT2, unsigned short cT3,
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);
+ std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
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));
@@ -1807,24 +1849,60 @@ assert(s2 == s1 +1);
{
throw std::runtime_error("in comRRTFromPos, at least one com trajectory is not present in problem solver");
}
+
+ std::cout << "com 0 " << paths[cT1]->initial().head<3>().transpose() << std::endl;
+ std::cout << "com 1 " << paths[cT1]->end().head<3>().transpose() << std::endl;
+ std::cout << "com 2 " << paths[cT2]->initial().head<3>().transpose() << std::endl;
+ std::cout << "com 3 " << paths[cT2]->end().head<3>().transpose() << std::endl;
+ std::cout << "com 4 " << paths[cT3]->initial().head<3>().transpose() << std::endl;
+ std::cout << "com 5 " << paths[cT3]->end().head<3>().transpose() << std::endl;
+
State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
State s1Bis(state1);
- s1Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[cT1]->end().head<3>());
+ s1Bis.configuration_ = project_or_throw(fullBody_, s1Bis,paths[cT1]->end().head<3>(), true);
+ std::cout << "projection succeedded " << paths[cT1]->end().head<3>() << std::endl;
State s2Bis(state2);
- s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[cT2]->end().head<3>());
+ s2Bis.configuration_ = project_or_throw(fullBody_, s2Bis,paths[cT2]->end().head<3>(), true);
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
+ std::cout << "projection succeedded " << paths[cT2]->end().head<3>() << std::endl;
+ /* ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
-
- ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
- fullBody_->device_->currentConfiguration(s1Bis.configuration_);
- if(!(problemSolver_->problem()->configValidations()->validate(s1Bis.configuration_, rport)
- && problemSolver_->problem()->configValidations()->validate(s2Bis.configuration_, rport)))
+ BasicConfigurationShooterPtr_t shooter = BasicConfigurationShooter::create(fullBody_->device_);
+ bool found = false;
+ for (int i = 0; i< 100 && !found; ++i)
+ {
+ fullBody_->device_->currentConfiguration(s1Bis.configuration_);
+ found =problemSolver_->problem()->configValidations()->validate(s1Bis.configuration_, rport);
+ if(!found)
+ {
+ std::cout << "collission " << *rport << std::endl;
+ s1Bis.configuration_ = *shooter->shoot();
+ s1Bis.configuration_ = project_or_throw(fullBody_, s1Bis,paths[cT1]->end().head<3>());
+ std::cout << "projection succeedded " << paths[cT1]->end().head<3>() << std::endl;
+ }
+ }
+ if(found)
+ std::cout << "got out ! " << std::endl;
+ bool found2 = false;
+ for (int i = 0; i< 100 && found && !found2; ++i)
+ {
+ fullBody_->device_->currentConfiguration(s2Bis.configuration_);
+ found2 =problemSolver_->problem()->configValidations()->validate(s2Bis.configuration_, rport);
+ if(!found2)
+ {
+ std::cout << "collission " << *rport << std::endl;
+ s2Bis.configuration_ = *shooter->shoot();
+ s2Bis.configuration_ = project_or_throw(fullBody_, s2Bis,paths[cT2]->end().head<3>());
+ std::cout << "projection succeedded " << paths[cT2]->end().head<3>() << std::endl;
+ }
+ }
+ if(!found || !found2)
{
std::cout << "could not project without collision at state " << s1 << std::endl;
//throw std::runtime_error ("could not project without collision at state " + s1 );
- }
+ }*/
{
core::PathPtr_t p1 = interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[cT1],
@@ -1836,6 +1914,7 @@ assert(s2 == s1 +1);
PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p1,problemSolver_));
+ std::cout << "PATH 1 OK " << std::endl;
}
@@ -1849,6 +1928,7 @@ assert(s2 == s1 +1);
intervals.push_back(interval);
PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals);
resPath->appendPath(reducedPath);
+ std::cout << "PATH 2 OK " << std::endl;
}
//if(s2Bis.configuration_ != state2.configuration_)
@@ -1862,6 +1942,7 @@ assert(s2 == s1 +1);
PathPtr_t reducedPath = core::SubchainPath::create(p3,intervals);
resPath->appendPath(reducedPath);
pathsIds.push_back(AddPath(p3,problemSolver_));
+ std::cout << "PATH 3 OK " << std::endl;
}
pathsIds.push_back(AddPath(resPath,problemSolver_));
@@ -1885,7 +1966,17 @@ assert(s2 == s1 +1);
unsigned short cT3,
unsigned short numOptimizations) throw (hpp::Error)
{
- return rrt(&interpolation::comRRT, state1, cT1, cT2, cT3, numOptimizations);
+ return rrt(&interpolation::comRRT, state1,state1+1, cT1, cT2, cT3, numOptimizations);
+
+ }
+
+ hpp::floatSeq* RbprmBuilder::comRRTFromPosBetweenState(double state1, double state2,
+ unsigned short cT1,
+ unsigned short cT2,
+ unsigned short cT3,
+ unsigned short numOptimizations) throw (hpp::Error)
+ {
+ return rrt(&interpolation::comRRT, state1,state2, cT1, cT2, cT3, numOptimizations);
}
@@ -1895,7 +1986,7 @@ assert(s2 == s1 +1);
unsigned short cT3,
unsigned short numOptimizations) throw (hpp::Error)
{
- return rrt(&interpolation::effectorRRT, state1, cT1, cT2, cT3, numOptimizations);
+ return rrt(&interpolation::effectorRRT, state1, state1+1, cT1, cT2, cT3, numOptimizations);
}
hpp::floatSeq* RbprmBuilder::effectorRRTFromPath(double state1,
@@ -1954,7 +2045,7 @@ assert(s2 == s1 +1);
}
}
- hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error)
+ hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error)
{
try
{
@@ -1964,7 +2055,7 @@ assert(s2 == s1 +1);
}
model::Configuration_t config = dofArrayToConfig (std::size_t(3), targetCom);
fcl::Vec3f comTarget; for(int i =0; i<3; ++i) comTarget[i] = config[i];
- model::Configuration_t res = project_or_throw(fullBody_,problemSolver_->problem(), lastStatesComputed_[state],comTarget);
+ model::Configuration_t res = project_or_throw(fullBody_, lastStatesComputed_[state],comTarget);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(res.rows());
for(std::size_t i=0; i< res.rows(); ++i)
@@ -2201,6 +2292,30 @@ assert(s2 == s1 +1);
}
}
+
+ CORBA::Short RbprmBuilder::computeIntermediary(unsigned short stateFrom, unsigned short stateTo) throw (hpp::Error)
+ try
+ {
+ std::size_t s((std::size_t)stateFrom);
+ std::size_t s2((std::size_t)stateTo);
+ if(lastStatesComputed_.size () < s+1 || lastStatesComputed_.size () < s2+1)
+ {
+ throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s));
+ }
+ const State& state1 = lastStatesComputed_[s];
+ const State& state2 = lastStatesComputed_[s2];
+ bool unused;
+ short unsigned cId = s;
+ const State state = intermediary(state1, state2,cId,unused);
+ lastStatesComputed_.push_back(state);
+ lastStatesComputedTime_.push_back(std::make_pair(-1., state));
+ return lastStatesComputed_.size() -1;
+ }
+ catch(std::runtime_error& e)
+ {
+ throw Error(e.what());
+ }
+
hpp::floatSeq* RbprmBuilder::getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error)
{
try{
@@ -2418,13 +2533,19 @@ assert(s2 == s1 +1);
fcl::Vec3f n; for(int i =0; i<3; ++i) n[i] = config[i];
projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody_,limb, fullBody_->GetLimbs().at(limb), ns, n,p);
+ ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
+ CollisionValidationPtr_t val = fullBody_->GetCollisionValidation();
+ rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
if (!rep.success_ && max_num_sample > 0)
{
BasicConfigurationShooterPtr_t shooter = BasicConfigurationShooter::create(fullBody_->device_);
+ Configuration_t head = ns.configuration_.head<7>();
for(std::size_t i =0; !rep.success_ && i< max_num_sample; ++i)
{
ns.configuration_ = *shooter->shoot();
+ ns.configuration_.head<7>() = head;
rep = projection::projectStateToObstacle(fullBody_,limb, fullBody_->GetLimbs().at(limb), ns, n,p);
+ rep.success_ = rep.success_ && val->validate(rep.result_.configuration_,rport);
}
}
if(rep.success_)
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 4f290f2ef5a2ac8f70106828c6d9419ee8758f45..43fc6fba098f57ec0bffb1e84a49c344eb2ce68c 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -178,7 +178,7 @@ namespace hpp {
(RbPrmFullBodyPtr_t, core::ProblemPtr_t, const core::PathPtr_t,
const State &, const State &, const std::size_t, const bool);
- hpp::floatSeq* rrt(t_rrt functor ,double state1,
+ hpp::floatSeq* rrt(t_rrt functor ,double state1,double state2,
unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
unsigned short numOptimizations) throw (hpp::Error);
@@ -187,6 +187,11 @@ namespace hpp {
unsigned short comTraj2,
unsigned short comTraj3,
unsigned short numOptimizations) throw (hpp::Error);
+ virtual hpp::floatSeq* comRRTFromPosBetweenState(double state1,double state2,
+ unsigned short comTraj1,
+ unsigned short comTraj2,
+ unsigned short comTraj3,
+ unsigned short numOptimizations) throw (hpp::Error);
virtual hpp::floatSeq* effectorRRT(double state1,
unsigned short comTraj1,
unsigned short comTraj2,
@@ -201,17 +206,18 @@ namespace hpp {
unsigned short comTraj3,
unsigned short numOptimizations,
const hpp::Names_t& trackedEffectors) throw (hpp::Error);
- virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);
+ virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom, unsigned short max_num_sample) throw (hpp::Error);
virtual CORBA::Short createState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* getConfigAtState(unsigned short stateId) throw (hpp::Error);
virtual double setConfigAtState(unsigned short stateId, const hpp::floatSeq& config) throw (hpp::Error);
- double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target)throw (hpp::Error);
- virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com) throw (hpp::Error);
+ double projectStateToCOMEigen(unsigned short stateId, const model::Configuration_t& com_target, unsigned short maxNumeSamples)throw (hpp::Error);
+ virtual double projectStateToCOM(unsigned short stateId, const hpp::floatSeq& com, unsigned short max_num_sample) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeBox(const char* limbName, double sampleId) throw (hpp::Error);
virtual CORBA::Short isLimbInContact(const char* limbName, double state) throw (hpp::Error);
virtual CORBA::Short isLimbInContactIntermediary(const char* limbName, double state) throw (hpp::Error);
+ virtual CORBA::Short computeIntermediary(unsigned short state1, unsigned short state2) throw (hpp::Error);
virtual hpp::floatSeqSeq* getOctreeBoxes(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeTransform(const char* limbName, const hpp::floatSeq& configuration) throw (hpp::Error);
virtual CORBA::Short isConfigBalanced(const hpp::floatSeq& config, const hpp::Names_t& contactLimbs, double robustnessTreshold) throw (hpp::Error);