Commit db5376c8 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[demo] platform polytopes

parent c42dc451
......@@ -288,6 +288,7 @@ install(FILES
data/meshes/siggraph_asia/stairs_lower.stl
data/meshes/flat_hole.stl
data/meshes/cross_gap.stl
data/meshes/cross_gap2.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
......
......@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap2.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap2.stl"/>
</geometry>
</collision>
</link>
......
......@@ -126,8 +126,8 @@ r(q_goal)
r(q_init)
#fullBody.setStartState(q_init,[lLegId,rLegId])
#fullBody.setEndState(q_goal,[lLegId,rLegId])
fullBody.setStartState(q_init,[lLegId,rLegId])
fullBody.setEndState(q_goal,[lLegId,rLegId])
#p = fullBody.computeContactPointsAtState(init_sid)
......@@ -146,13 +146,13 @@ pp = PathPlayer (fullBody.client.basic, r)
import fullBodyPlayerHrp2
"""
tStart = time.time()
configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 1, filterStates = False)
configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = False)
tInterpolateConfigs = time.time() - tStart
print "number of configs :", len(configsFull)
"""
"""
q_init[0] += 0.05
createSphere('s',r)
......@@ -189,7 +189,7 @@ r(smid2.q())
sf2,success = StateHelper.removeContact(smid2,larmId)
sf2.projectToCOM([1.3,0,0.82])
"""
"""
com = fullBody.getCenterOfMass()
......
......@@ -30,7 +30,7 @@ urdfNameRom = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = omniORB.any.to_any(0.3);
aMax = omniORB.any.to_any(0.2);
aMax = omniORB.any.to_any(1);
#aMax = omniORB.any.to_any(0.3);
extraDof = 6
mu=omniORB.any.to_any(MU)
......@@ -40,7 +40,7 @@ rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, pa
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -0.5, 0.5, 0.5, 0.8])
rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.55,0.85])
rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
rbprmBuilder.setJointBounds('HEAD_JOINT1',[0,0])
......@@ -79,7 +79,7 @@ afftool.visualiseAffordances('Support', r, r.color.lightBrown)
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [1,0,0,0]
q_init [0:3] = [0.3, 0, 0.58]; r (q_init)
q_init[8] = 0.8
#q_init[3:7] = [0.7071,0,0,0.7071]
#q_init [0:3] = [1, 1, 0.65]
......@@ -89,7 +89,7 @@ q_goal = q_init [::]
q_goal[3:7] = [1,0,0,0]
q_goal [0:3] = [1.28, 0, 0.58]; r (q_goal)
q_goal[8] = 0
r (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
......@@ -119,8 +119,8 @@ tPlanning = time.time() -tStart
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp.dt=0.03
#pp.displayVelocityPath(0)
#r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.displayVelocityPath(0)
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
"""
......
......@@ -149,7 +149,8 @@ createSphere('s',r)
n = [0,0,1]
p = [0,0.1,0]
q_init[-6:-3] = [0.2,0,0]
q_goal[-6:-3] = [0.1,0,0]
sf = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
......@@ -164,11 +165,18 @@ assert(success)
r(smid.q())
sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
pid = fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
import disp_bezier
disp_bezier.showPath(r,pp,pid)
"""
com = fullBody.getCenterOfMass()
com[1] = 0
"""
"""
pids = []
pids += [fullBody.isDynamicallyReachableFromState(si.sId,smid.sId)]
pids += [fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId)]
......@@ -181,7 +189,7 @@ for pid in pids :
#r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
else:
print "fail."
"""
"""
n = [0,0,1]
......
......@@ -128,11 +128,28 @@ configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStat
r(configs[-1])
tInterpolateConfigs = time.time() - tStart
noCOQP = 0
for i in range(len(configs)-2):
pid = fullBody.isDynamicallyReachableFromState(i,i+1)
if len(pid)==0:
noCOQP +=1
f = open("/local/fernbac/bench_iros18/success/log_successSideWall.log","a")
f.write("num states : "+str(len(configs))+"\n")
if noCOQP>0:
f.write("fail, with infeasibles transitions "+str(noCOQP)+"\n")
else:
f.write("all transition feasibles\n")
f.close()
"""
print "number of configs =", len(configs)
r(configs[-1])
......@@ -143,6 +160,8 @@ from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = 1)
player.displayContactPlan()
"""
......@@ -196,7 +215,7 @@ camera = [0.5681925415992737,
r.client.gui.setCameraTransform(0,camera)
"""
"""
# infeasible :
r([1.46,0,0.851192,1,0,0,0,-0.32001,1.18232,-1.5504,-0.305161,-0.139056,1.37011,-0.36127,1.20462,-1.55472,-0.361812,0.407251,0.775043,0.2,0,0.000953452,0,0,0.000381381,])
......@@ -212,6 +231,6 @@ leg = rhLegId
configs=configs[6:8]
cs = generate_contact_sequence_hyq.generateContactSequence(fullBody,configs,6, 7,r)
"""
......@@ -169,15 +169,27 @@ r(configs[-1])
noCOQP = 0
for i in range(len(configs)-1):
pid = fullBody.isDynamicallyReachableFromState(i,i+1)
if len(pid)==0:
noCOQP +=1
f = open("/home/pfernbac/Documents/com_ineq_test/log_success.log","a")
f.write("num states : "+str(len(configs))+" \n")
f = open("/local/fernbac/bench_iros18/success/log_successStairsNoRamp.log","a")
if noCOQP>0:
f.write("fail, with "+str(noCOQP)+" infeasibles transitions\n")
else:
f.write("all transition feasibles\n")
f.close()
"""
from fullBodyPlayerHrp2 import Player
......
......@@ -159,11 +159,33 @@ from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
pp.dt=0.001
configs = fullBody.interpolate(0.005,pathId=0,robustnessTreshold = 0, filterStates = True)
configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStates = True)
print "number of configs :", len(configs)
r(configs[-1])
noCOQP = 0
for i in range(len(configs)-2):
pid = fullBody.isDynamicallyReachableFromState(i,i+1)
if len(pid)==0:
noCOQP +=1
f = open("/local/fernbac/bench_iros18/success/log_successStairs.log","a")
f.write("num states : "+str(len(configs))+"\n")
if noCOQP>0:
f.write("fail, with infeasibles transitions "+str(noCOQP)+"\n")
else:
f.write("all transition feasibles\n")
f.close()
"""
f = open("/home/pfernbac/Documents/com_ineq_test/log_success.log","a")
f.write("num states : "+str(len(configs))+" \n")
......@@ -172,7 +194,7 @@ f.close()
"""
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=False,pathId = 0)
......@@ -191,7 +213,7 @@ endState = 14
configs=configs[beginState:endState+1]
#cs = generateContactSequence(fullBody,configs,beginState, endState,r)
cs = generateContactSequenceWithInitGuess(ps,fullBody,configs,beginState, endState,r)
"""
"""
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
......
......@@ -33,7 +33,7 @@ def callMuscodBetweenTwoState(fullBody,s0,s1,c_qp = [], t_qp = []):
states = genStateWithOneStep(fullBody,limbs[0], 500,False)
states = genStateWithOneStep(fullBody,limbs[0], 100,False)
name = "/local/fernbac/bench_iros18/muscod_qp/one_step"
file_exist = True
......
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