[demo] platform polytopes

CMakeLists.txt

@@ -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
   )
 

data/urdf/cross_gap.urdf

@@ -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>

script/dynamic/plateform_hand_hrp2_interp.py

@@ -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()

script/dynamic/plateform_hand_hrp2_path.py

@@ -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")
 
 
 """

script/dynamic/plateform_hrp2_interp_testTransition.py

@@ -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]

script/dynamic/sideWall_hyq_interpKino_intersection.py

@@ -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)
-
+"""
 
 

script/dynamic/stair_bauzil_hrp2_interp_noRamp.py

@@ -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

script/dynamic/stair_bauzil_hrp2_interp_testTransition.py

@@ -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")

script/tools/stat_muscod_qp.py

@@ -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