Commit 2e6c5c10 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[demo] add scene for cross gap

parent e58cb752
......@@ -136,6 +136,7 @@ install(FILES
data/urdf/bauzil_stairs.urdf
data/urdf/flat_step.urdf
data/urdf/flat_hole.urdf
data/urdf/cross_gap.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
......@@ -209,6 +210,7 @@ install(FILES
data/srdf/bauzil_stairs.srdf
data/srdf/flat_step.srdf
data/srdf/flat_hole.srdf
data/srdf/cross_gap.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
......@@ -285,6 +287,7 @@ install(FILES
data/meshes/siggraph_asia/wall.stl
data/meshes/siggraph_asia/stairs_lower.stl
data/meshes/flat_hole.stl
data/meshes/cross_gap.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
......
<robot name="cross_gap">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.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/cross_gap.stl"/>
</geometry>
</collision>
</link>
</robot>
<robot name="cross_gap">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.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/cross_gap.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -21,7 +21,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.3,0.3, -0.3, 0.3, 0.5, 0.75])
fullBody.setJointBounds ("base_joint_xyz", [-0.3,0.3, -0.3, 0.3, 0.6, 0.7])
fullBody.setJointBounds ("CHEST_JOINT0", [0.,0.])
fullBody.setJointBounds ("CHEST_JOINT1", [0.,0.])
fullBody.setJointBounds ("HEAD_JOINT0", [0.,0.])
......@@ -217,7 +217,8 @@ def _boundSO3(q, num_limbs):
def _genbalance(limbs):
for i in range(10000):
q = fullBody.client.basic.robot.shootRandomConfig()
q = _boundSO3(q, len(limbs))
q[3:7] = [1,0,0,0]
#q = _boundSO3(q, len(limbs))
if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return q
......@@ -239,13 +240,15 @@ def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
states.append(s)
return states
def genStateWithOneStep(fullbody,limbs, num_samples = 1000, coplanar = True):
def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs=[]
states = []
assert(len(limbs) == 2 and "only implemented for 2 limbs in contact for now")
for i in range(num_samples):
it = 0
while len(states) < num_samples and it < 5000:
i = len(states)
if(coplanar):
q0 = fullBody.generateGroundContact(limbs)
else:
......@@ -273,6 +276,7 @@ def genStateWithOneStep(fullbody,limbs, num_samples = 1000, coplanar = True):
print "Step "+str(i)+" done."
else :
print "cannot make the step."
it +=1
print "Done generating pairs of states, found : "+str(len(states))+" pairs."
return states
......
......@@ -412,6 +412,7 @@ def generateContactSequence(fb,configs,beginId,endId,viewer=None, curves_initGue
phase_d.time_trajectory.append(t_init_guess[2])
time = np.linspace(t_init_guess[0]+t_init_guess[1],t_init_guess[2]+t_init_guess[1]+t_init_guess[0],num=NUM_NODES_FINAL,endpoint=True)
for t in time:
t = min(t,c_init_guess.max())
state = [0]*9
state[0:3] = c_init_guess(t).transpose().tolist()[0]
state[3:6] = dc_init_guess(t).transpose().tolist()[0]
......
......@@ -33,7 +33,7 @@ def callMuscodBetweenTwoState(fullBody,s0,s1,c_qp = [], t_qp = []):
states = genStateWithOneStep(fullBody,limbs[0], 20,False)
states = genStateWithOneStep(fullBody,limbs[0], 500,False)
name = "/local/fernbac/bench_iros18/muscod_qp/one_step"
file_exist = True
......@@ -50,8 +50,20 @@ cs_out = ContactSequenceHumanoid(0)
same_positif = 0
same_negatif = 0
false_positif = 0
false_positive_quasiStatic = 0
false_negatif = 0
muscod_converged = 0
muscod_converged_init_guess = 0
need_init_guess =0
wrong_init_guess = 0
total_success_qp = 0
for [s0,s1] in states:
f.write("Try for pair "+str(i)+" ------- \n")
f.write("q0 = "+str(s0.q())+"\n")
f.write("q1 = "+str(s1.q())+"\n")
success_quasiStatic = False
success_qp = False
success_muscod = False
......@@ -61,14 +73,24 @@ for [s0,s1] in states:
success_qp = (len(pid) > 0)
if success_qp :
#compute init guess curve for muscod :
#compute init guess curve for muscod :
total_success_qp +=1
c_qp = fullBody.getPathAsBezier(int(pid[0]))
t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]
success_muscod, ssh_ok = callMuscodBetweenTwoState(fullBody,s0,s1,[c_qp],[t_qp])
success_muscod, ssh_ok = callMuscodBetweenTwoState(fullBody,s0,s1)
f.write("Try for pair "+str(i)+" ------- \n")
f.write("q0 = "+str(s0.q())+"\n")
f.write("q1 = "+str(s1.q())+"\n")
success_muscod_initGuess, ssh_ok = callMuscodBetweenTwoState(fullBody,s0,s1,[c_qp],[t_qp])
success_muscod, ssh_ok = callMuscodBetweenTwoState(fullBody,s0,s1)
if success_muscod_initGuess and not success_muscod:
f.write("muscod converged only with initial guess\n")
need_init_guess +=1
if not success_muscod_initGuess and success_muscod:
f.write("muscod did not converge with initial guess\n")
wrong_init_guess += 1
success_muscod = success_muscod or success_muscod_initGuess
if success_muscod :
muscod_converged_init_guess +=1
else :
success_muscod, ssh_ok = callMuscodBetweenTwoState(fullBody,s0,s1)
if not ssh_ok :
f.write("Error in ssh connection to muscod server ... \n")
......@@ -83,6 +105,7 @@ for [s0,s1] in states:
else :
f.write("time_qp\n")
if success_muscod:
muscod_converged += 1
cs_out.loadFromXML(CONTACT_SEQUENCE_WHOLEBODY_FILE,CONTACT_SEQUENCE_XML_TAG)
t_muscod = []
for k in range(3):
......@@ -98,8 +121,13 @@ for [s0,s1] in states:
f.write("same results, reachable \n")
same_positif += 1
if success_qp and not success_muscod:
f.write("false positive \n")
false_positif += 1
if success_quasiStatic:
f.write("false positive but with quasi-static solution \n")
false_positive_quasiStatic += 1
else :
f.write("false positive \n")
if not success_qp and success_muscod:
f.write("false negative \n")
false_negatif +=1
......@@ -113,25 +141,36 @@ num_states = float(len(states))
print "for : "+str(num_states)+" states."
print "same result, unreachable : "+str(same_negatif) + " ; "+str((float(same_negatif)/num_states)*100.)+" % "
print "same result, reachable : "+str(same_positif) + " ; "+str((float(same_positif)/num_states)*100.)+" % "
print "false positive : "+str(false_positif) + " ; "+str((float(false_positif)/num_states)*100.)+" %"
print "false negative : "+str(false_negatif) + " ; "+str((float(false_negatif)/num_states)*100.)+" %"
print "false positive : "+str(false_positif) + " ; "+str((float(false_positif)/num_states)*100.)+" %"
f = open(filename+"C","w")
f.write( "for : "+str(num_states)+" states.\n")
f.write( "same result, unreachable : "+str(same_negatif) + " ; "+str((float(same_negatif)/num_states)*100.)+" % \n")
f.write( "same result, reachable : "+str(same_positif) + " ; "+str((float(same_positif)/num_states)*100.)+" % \n")
f.write( "false positive : "+str(false_positif) + " ; "+str((float(false_positif)/num_states)*100.)+" %\n")
f.write( "false negative : "+str(false_negatif) + " ; "+str((float(false_negatif)/num_states)*100.)+" %\n")
f.write( "false negative : "+str(false_negatif) + " ; "+str((float(false_negatif)/num_states)*100.)+" %\n\n")
f.write("##### Excluding unreachable states : \n")
num_states -= same_negatif
f.write( "for : "+str(num_states)+" states.\n")
f.write( "same result, reachable : "+str(same_positif) + " ; "+str((float(same_positif)/num_states)*100.)+" % \n")
f.write( "false positive : "+str(false_positif) + " ; "+str((float(false_positif)/num_states)*100.)+" %\n")
f.write( "false negative : "+str(false_negatif) + " ; "+str((float(false_negatif)/num_states)*100.)+" %\n")
f.write( "false positive : "+str(false_positif) + " ; "+str((float(false_positif)/num_states)*100.)+" %\n")
f.write( "but in : "+str(false_positive_quasiStatic) + " cases there was a quasi-static solution \n")
f.write( "false positive with no quasi-static solution : "+str(false_positif - false_positive_quasiStatic)+" ; "+str((float((false_positif - false_positive_quasiStatic))/num_states)*100.)+" %\n\n")
f.write("##### Muscod convergence : \n")
f.write(" Muscod converged a total of "+str(muscod_converged)+" times \n")
f.write(" Muscod converged a total of "+str(muscod_converged_init_guess)+" times for a problem where an initial guess was available\n")
f.write(" This mean that "+str((float(muscod_converged_init_guess)/float(muscod_converged))*100.)+" % of the time where muscod converged, an initial guess was available\n")
f.write(" In "+str((float(wrong_init_guess)/float(total_success_qp))*100.)+" % the init guess prevented muscod to converge \n")
f.write(" In "+str((float(need_init_guess)/float(muscod_converged_init_guess))*100.)+" % of the case were an initial guess was provided, muscod only converged with the initial guess \n")
f.write(" In "+str((float(need_init_guess)/float(muscod_converged))*100.)+" % of all the cases muscod only converged with an initial guess \n")
f.close()
"""
from constraint_to_dae import *
from display_tools import *
from hpp.gepetto import PathPlayer
......@@ -181,4 +220,4 @@ s2 = State(fullBody,q= q1,limbsIncontact=limbs[0])
s1,success = StateHelper.removeContact(s0,lLegId)
"""
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment