From 080a7af813387181ab015b846f669fa26e270dd2 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 26 Feb 2016 10:03:59 +0100
Subject: [PATCH] add debug method/display in scripts
---
data/srdf/ground_jump_bar.urdf | 19 -----------------
script/tests/darpa_hyq_path.py | 6 +++---
script/tests/robot_jumpEasy_path.py | 8 +++++--
script/tests/robot_withBridge_path.py | 30 ++++++++++++++++-----------
script/tests/truck_hrp2_path.py | 1 +
5 files changed, 28 insertions(+), 36 deletions(-)
delete mode 100644 data/srdf/ground_jump_bar.urdf
diff --git a/data/srdf/ground_jump_bar.urdf b/data/srdf/ground_jump_bar.urdf
deleted file mode 100644
index 91f5b37f..00000000
--- a/data/srdf/ground_jump_bar.urdf
+++ /dev/null
@@ -1,19 +0,0 @@
-<robot name="ground_jump_bar">
- <link name="base_link">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry>
- <mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_bar.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/ground_jump_bar.stl"/>
- </geometry>
- </collision>
- </link>
-</robot>
diff --git a/script/tests/darpa_hyq_path.py b/script/tests/darpa_hyq_path.py
index ad9c2101..cb1b984c 100644
--- a/script/tests/darpa_hyq_path.py
+++ b/script/tests/darpa_hyq_path.py
@@ -69,12 +69,12 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~
#~ pp (2)
#~ pp (0)
-pp.displayPath(0)
-r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
+pp.displayPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp (1)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_1_root",r.sceneName)
+#r.client.gui.removeFromGroup("path_1_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
diff --git a/script/tests/robot_jumpEasy_path.py b/script/tests/robot_jumpEasy_path.py
index 647cf1fa..9562a489 100644
--- a/script/tests/robot_jumpEasy_path.py
+++ b/script/tests/robot_jumpEasy_path.py
@@ -43,7 +43,7 @@ q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
-q_goal [0:3] = [1, 1, 0.9]; r (q_goal) # pont
+q_goal [0:3] = [3, 1, 0.9]; r (q_goal) # pont
#~ ps.addPathOptimizer("GradientBased")
@@ -59,6 +59,10 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r(q_init)
ps.client.problem.prepareSolveStepByStep()
+i = 0
+r.displayRoadmap("rm"+str(i),0.02)
+ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
+
r.solveAndDisplay("rm",1,0.02)
@@ -67,8 +71,8 @@ r.solveAndDisplay("rm",1,0.02)
#r.displayRoadmap("rm",0.02)
-r.displayPathMap("rmPath",0,0.02)
+r.displayPathMap("rmPath",0,0.02)
from hpp.gepetto import PathPlayer
diff --git a/script/tests/robot_withBridge_path.py b/script/tests/robot_withBridge_path.py
index f479d8ea..636982c6 100644
--- a/script/tests/robot_withBridge_path.py
+++ b/script/tests/robot_withBridge_path.py
@@ -21,7 +21,7 @@ srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 1.5])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 3])
rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
@@ -41,16 +41,16 @@ r.loadObstacleModel (packageName, "ground_with_bridge", "planning")
q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init = [-4,1,0.9,1,0,0,0,0,0,1]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
#q_goal [0:3] = [-2, 1, 0.6]; r (q_goal) # tryDirectPath
-q_goal [0:3] = [4, -1, 0.9]; r (q_goal)
+q_goal= [4,-1,0.9,1,0,0,0,0,0,1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
+#ps.addPathOptimizer("RandomShortcut")
#ps.client.problem.selectSteeringMethod("SteeringParabola")
ps.selectPathPlanner("RRTdynamic")
ps.setInitialConfig (q_init)
@@ -61,18 +61,20 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r(q_init)
-r.solveAndDisplay("rm",1,0.02)
+#r.solveAndDisplay("rm",1,0.02)
-#t = ps.solve ()
-#r.displayRoadmap("rm",0.005)
+t = ps.solve ()
+r.displayRoadmap("rm",0.005)
#####
-# i = 0
-# ps.client.problem.prepareSolveStepByStep()
-# r.displayRoadmap("rm"+str(i),0.02)
-# ps.client.problem.executeOneStep() ; r.displayRoadmap("rm"+str(i),0.005) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ; i = i+1
+i = 0
+ps.client.problem.prepareSolveStepByStep()
+r.displayRoadmap("rm"+str(i),0.02)
+ps.client.problem.executeOneStep() ;i = i+1; r.displayRoadmap("rm"+str(i),0.02) ; r.client.gui.removeFromGroup("rm"+str(i-1),r.sceneName) ;
+
+
r.displayPathMap("rmPath",0,0.025)
@@ -91,6 +93,10 @@ pp (1)
#r.client.gui.removeFromGroup("rm",r.sceneName)
r.client.gui.removeFromGroup("rmPath",r.sceneName)
-r.client.gui.removeFromGroup("path_1_root",r.sceneName)
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+i=0
+
+ps.clearRoadmap(); ps.solve(); r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName); pp.displayPath(i+1,r.color.lightGreen); i=i+1;
+
diff --git a/script/tests/truck_hrp2_path.py b/script/tests/truck_hrp2_path.py
index 1754dfb8..3e43c0fd 100755
--- a/script/tests/truck_hrp2_path.py
+++ b/script/tests/truck_hrp2_path.py
@@ -67,5 +67,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (2)
r.displayRoadmap("rm",0.02)
r.displayPathMap("rmPath",0,0.025)
+pp.displayPath(0)
pp (0)
--
GitLab