From b3375c90fbde42c0a8dc1545f6cc726d46656191 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 3 Feb 2017 20:04:54 +0100
Subject: [PATCH] update scripts for stairs
---
data/meshes/hrp2_trunk_body.stl | Bin 1884 -> 1884 bytes
data/urdf/hrp2_trunk_flexible.urdf | 1 +
script/dynamic/stair_bauzil_hrp2_interp.py | 10 +++++-----
script/dynamic/stair_bauzil_hrp2_path.py | 21 ++++++++++++---------
script/dynamic/wall_hrp2_interp.py | 8 ++++----
script/tools/displayCOntact.py | 4 ++--
6 files changed, 24 insertions(+), 20 deletions(-)
mode change 100755 => 100644 data/meshes/hrp2_trunk_body.stl
diff --git a/data/meshes/hrp2_trunk_body.stl b/data/meshes/hrp2_trunk_body.stl
old mode 100755
new mode 100644
index dbfe476c959e2d49c5bd188a0b71bf8a7d7ca3e0..fad9f4f69717e571867f608e7cd2befccf7628e5
GIT binary patch
delta 12
Tcmcb^cZY9+B%{SfDJOOS9rgpu
delta 12
Tcmcb^cZY9+B%|p@DJOOS9p?kc
diff --git a/data/urdf/hrp2_trunk_flexible.urdf b/data/urdf/hrp2_trunk_flexible.urdf
index 6dbbf2f2..f6ccb657 100755
--- a/data/urdf/hrp2_trunk_flexible.urdf
+++ b/data/urdf/hrp2_trunk_flexible.urdf
@@ -1,3 +1,4 @@
+
<robot name="hrp2_trunk_flexible">
<link name="base_link"/>
<joint name="WAIST" type="fixed">
diff --git a/script/dynamic/stair_bauzil_hrp2_interp.py b/script/dynamic/stair_bauzil_hrp2_interp.py
index 32a1b074..78d2c360 100644
--- a/script/dynamic/stair_bauzil_hrp2_interp.py
+++ b/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)
diff --git a/script/dynamic/stair_bauzil_hrp2_path.py b/script/dynamic/stair_bauzil_hrp2_path.py
index bf24bd09..d68f1fd6 100644
--- a/script/dynamic/stair_bauzil_hrp2_path.py
+++ b/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)
diff --git a/script/dynamic/wall_hrp2_interp.py b/script/dynamic/wall_hrp2_interp.py
index 5a5ad047..67552de4 100644
--- a/script/dynamic/wall_hrp2_interp.py
+++ b/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)
diff --git a/script/tools/displayCOntact.py b/script/tools/displayCOntact.py
index 4716022e..a73c95ff 100644
--- a/script/tools/displayCOntact.py
+++ b/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],]
""""""""""""""""""""""""""""""""""""""""""""
--
GitLab