update scripts for stairs

data/urdf/hrp2_trunk_flexible.urdf

+
 <robot name="hrp2_trunk_flexible">
   <link name="base_link"/>
   <joint name="WAIST" type="fixed">

script/dynamic/stair_bauzil_hrp2_interp.py

@@ -28,15 +28,15 @@ r = tp.Viewer (ps,viewerClient=tp.r.client)
 #~ AFTER loading obstacles
 rLegId = '0rLeg'
 rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,-0.105,0,]
-rLegNormal = [0,1,0]
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
 rLegx = 0.09; rLegy = 0.05
 fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
 
 lLegId = '1lLeg'
 lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
 fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
 
@@ -131,7 +131,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId])
 
 
 
-configs = fullBody.interpolate(0.08,pathId=0,robustnessTreshold = 1, filterStates = True)
+configs = fullBody.interpolate(0.1,pathId=1,robustnessTreshold = 3, filterStates = True)
 print "number of configs :", len(configs)
 
 

script/dynamic/stair_bauzil_hrp2_path.py

@@ -31,7 +31,7 @@ srdfSuffix = ""
 rbprmBuilder = Builder ()
 
 rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [0,1.55, -0.8, -0.5, 0.50, 1.3])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,1.55, -0.7, -0.5, 0.50, 1.3])
 #rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
 rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
 rbprmBuilder.setJointBounds('CHEST_JOINT1',[0,0.45])
@@ -47,7 +47,7 @@ vMax = 1;
 aMax = 10;
 extraDof = 6
 rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
-rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-0.2,0.2,-vMax,vMax,0,0,0,0,0,0])
 indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
 
 #~ from hpp.corbaserver.rbprm. import ProblemSolver
@@ -67,6 +67,8 @@ q_init = rbprmBuilder.getCurrentConfig ();
 
 q_init [0:3] = [0, -0.65, 0.55];
 q_init[8] = 0.4
+#q_init [0:3] = [0, -0.65, 0.58];
+#q_init[8] = 0.43
 rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 
 q_goal = q_init [::]
@@ -97,22 +99,23 @@ ps.selectPathPlanner("DynamicPlanner")
 
 #ps.client.problem.finishSolveStepByStep()
 
-r.solveAndDisplay("rm",1,0.01)
+#r.solveAndDisplay("rm",1,0.01)
 
 ps.solve()
 
+# was 5.5
+ps.client.problem.extractPath(0,5.7,ps.client.problem.pathLength(0))
+
 
 
 # Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
 pp.dt=1/30.
-r.client.gui.removeFromGroup("rm",r.sceneName)
-pp.displayVelocityPath(0)
-pp.speed=1
-pp(0)
-
-
+#r.client.gui.removeFromGroup("rm",r.sceneName)
+pp.displayVelocityPath(1)
+pp.speed=0.25
+#pp(0)
 
 
 

script/dynamic/wall_hrp2_interp.py

@@ -28,15 +28,15 @@ r = tp.Viewer (ps,viewerClient=tp.r.client)
 #~ AFTER loading obstacles
 rLegId = '0rLeg'
 rLeg = 'RLEG_JOINT0'
-rLegOffset = [0,-0.105,0,]
-rLegNormal = [0,1,0]
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
 rLegx = 0.09; rLegy = 0.05
 fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
 
 lLegId = '1lLeg'
 lLeg = 'LLEG_JOINT0'
-lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
 fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
 

script/tools/displayCOntact.py

@@ -25,9 +25,9 @@ r.client.gui.refresh()
 
 """"""""""""""""""""""""""""""""""""""""""""
 # stairs :
-q = [[0.440782 , -0.629846 , 6.94538e-310],[0.440054 , -1.19294 , 0.325858],[0.447571 , -1.195 , 0.407322],]
+q= [[0.445947 , -0.487313 , 0.15],[0.413544 , -1.19627 , 0.82163],[0.436797 , -0.761877 , 0.15],]
 
-q2 = [[0.320782 , -0.559846 , 2.62221e-09],[0.320782 , -0.699846 , -2.62221e-09],[0.560782 , -0.559846 , 2.62221e-09],[0.560782 , -0.699846 , -2.62221e-09],[0.344786 , -1.13056 , 0.277322],[0.344786 , -1.2553 , 0.277311],[0.535321 , -1.13057 , 0.374404],[0.535321 , -1.25531 , 0.374393],[0.352304 , -1.13263 , 0.358786],[0.352304 , -1.25737 , 0.358776],[0.542839 , -1.13263 , 0.455868],[0.542839 , -1.25737 , 0.455858],]
+q2 = [[0.325947 , -0.417313 , 0.15],[0.325947 , -0.557313 , 0.15],[0.565947 , -0.417313 , 0.15],[0.565947 , -0.557313 , 0.15],[0.318277 , -1.1339 , 0.773095],[0.318277 , -1.25864 , 0.773084],[0.508811 , -1.13391 , 0.870177],[0.508811 , -1.25865 , 0.870166],[0.316797 , -0.691877 , 0.15],[0.316797 , -0.831877 , 0.15],[0.556797 , -0.691877 , 0.15],[0.556797 , -0.831877 , 0.15],]
 
 
 """"""""""""""""""""""""""""""""""""""""""""