From 1928e6dba4734ff908266bc38f4447de165bdfec Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Tue, 26 Jan 2016 14:47:49 +0100
Subject: [PATCH] add environnement for jump and test script
---
CMakeLists.txt | 13 +++-
data/meshes/ground_bigStep.stl | Bin 0 -> 1284 bytes
data/meshes/ground_jump_easy.stl | Bin 0 -> 1284 bytes
data/meshes/ground_jump_med.stl | Bin 0 -> 1284 bytes
data/meshes/ground_square.stl | Bin 0 -> 684 bytes
data/srdf/ground_bigStep.srdf | 19 ++++++
data/srdf/ground_jump_easy.srdf | 19 ++++++
data/srdf/ground_jump_med.srdf | 19 ++++++
data/urdf/ground_bigStep.urdf | 19 ++++++
data/urdf/ground_jump_easy.urdf | 19 ++++++
data/urdf/ground_jump_med.urdf | 19 ++++++
script/tests/robot_bigStep_path .py | 88 +++++++++++++++++++++++++++
script/tests/robot_jumpEasy_path .py | 88 +++++++++++++++++++++++++++
13 files changed, 301 insertions(+), 2 deletions(-)
create mode 100644 data/meshes/ground_bigStep.stl
create mode 100644 data/meshes/ground_jump_easy.stl
create mode 100644 data/meshes/ground_jump_med.stl
create mode 100644 data/meshes/ground_square.stl
create mode 100644 data/srdf/ground_bigStep.srdf
create mode 100644 data/srdf/ground_jump_easy.srdf
create mode 100644 data/srdf/ground_jump_med.srdf
create mode 100644 data/urdf/ground_bigStep.urdf
create mode 100644 data/urdf/ground_jump_easy.urdf
create mode 100644 data/urdf/ground_jump_med.urdf
create mode 100644 script/tests/robot_bigStep_path .py
create mode 100644 script/tests/robot_jumpEasy_path .py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 81ca4de6..61bddc43 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -96,6 +96,9 @@ install(FILES
data/urdf/robot_test/robot_test_lleg_rom.urdf
data/urdf/robot_test/robot_test_rleg_rom.urdf
data/urdf/robot_test/robot_test_trunk.urdf
+ data/urdf/ground_bigStep.urdf
+ data/urdf/ground_jump_easy.urdf
+ data/urdf/ground_jump_med.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
@@ -130,6 +133,9 @@ install(FILES
data/srdf/robot_test/robot_test_lleg_rom.srdf
data/srdf/robot_test/robot_test_rleg_rom.srdf
data/srdf/robot_test/robot_test_trunk.srdf
+ data/srdf/ground_bigStep.srdf
+ data/srdf/ground_jump_easy.srdf
+ data/srdf/ground_jump_med.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
@@ -164,8 +170,11 @@ install(FILES
data/meshes/stepladder.stl
data/meshes/chair.stl
data/meshes/car.stl
- data/meshes/polaris.stl
- data/meshes/polaris_reduced.stl
+ data/meshes/polaris.stl
+ data/meshes/polaris_reduced.stl
+ data/meshes/ground_bigStep.stl
+ data/meshes/ground_jump_easy.stl
+ data/meshes/ground_jump_med.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
install(FILES
diff --git a/data/meshes/ground_bigStep.stl b/data/meshes/ground_bigStep.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0abbfcc0abfa93e0f8c49070b8c397bae18d93c2
GIT binary patch
literal 1284
zcmb7?Jx&5q6opBQt(C=ePDQkk7(z%4nF(lYT>vFvs6fo1iCb_J?1ag*Mpy#DMq*=2
z?K|j6&gIQ3oa*GA``y2}cQ23A{3#p8qkKAvZ^qffFv~BSjrMhXK6@U-R~NN^ebGVm
zN3%?eS-<;w8zt21<ME=_D$~=We!16{`E!(j4ilaBSNF};_;z`zKYLMf@7_;b4QN|J
z9crbaEy28Sln8gFj!v$bs%xNMjZ)eY>QG}>r2Wa&QGzr1ghp{D)Cd!FbXWVl@@MTF
znwJ`vp_wePIop=JD{2r+_g;NhbkI8qXukWGSYOQx-W4_G*uBq->bp{h8e1Uk-<3K_
zgu7CQbD($4WOqdey;GwTr7fWjHFibXzY=wn;7mTDQJe`i!UP@kPK~@u+d0&s#;!>J
G{*NDZB#Qt5
literal 0
HcmV?d00001
diff --git a/data/meshes/ground_jump_easy.stl b/data/meshes/ground_jump_easy.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8e9049c824708232603c101c6abd6a72abe6d9d3
GIT binary patch
literal 1284
zcmb7?KWoBJ6vYMU>L3mo->5^2bSO9&Ql(#@o$6SOBB+sQ!L?&Qlp$-!cJJ6hijyC}
z=1>lt>q`bR9p1UW`!DxlyGqhcJa&^bS-8PGUXJ7R`u66o=i2Mn7uW67F8YGAXg4{A
z*?JVc4}(7S_tktCO^;!#KFXmjLPrT`zWc;-_8WZ^H4a5C>Yw|d|5m&=iW<<ihC0+r
zLtBD*;V4nvl{z}PW-70Nel<#IOQ=JQU6J-DS4RoX<P#djnNXveprgA&UZw3E>QLi9
zYbHxHpY}QLiW<b}ai5pp6&>_W0-EpsT|L*nGTs$6=IES0v+}!AhZ<WT?cbF;N>q2H
z4(CAcn#t~p4tl3XCrVpF9ct`~w0|Y)D8ZS0LZdhnYE%<+&^tBqDsAUbhZ?&g{r^Ay
E0Kj;RRsaA1
literal 0
HcmV?d00001
diff --git a/data/meshes/ground_jump_med.stl b/data/meshes/ground_jump_med.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a9008681fa0911fd0c7f792d4e30ed8abc991184
GIT binary patch
literal 1284
zcmb7?y-osA6ogHPt<@GAsx7sE7zz^#0%)jcs42~^6@OVU@&HiS`U*Y*2_3MB6%Rpa
zsrJ}-CuWj4+}(n$CU?$v{`Pd{lW{sr;^-+I52O1*GK!P5Qmfw8qwDGOV|06San=`}
zL}$lxb35&~ULV2+_2F``>HU_QL8+fXTZE1h(0uoa`0b~)cQw9TF6z&A*m!a8SFQ%M
zt)ULJ($JP*UN}k=ccqR_u9@;{pkIwr+7jweV^^g8$<<MUGx>x@aVFF#Cg|v{kXLCt
zhdR{w&zi{+>yPh@cSQ~2-M#1E6&>_W0-EpsU3Hhij(0_kIaZ5cmw#94P-6?E{ku{}
ziQ=x*;T-5)Gud6yLGRS)L}^Q?LycXL_OC=8B{-8$XcT8cjbefhdZ$KSrR^N*P-9o5
H|NqAyDRGi?
literal 0
HcmV?d00001
diff --git a/data/meshes/ground_square.stl b/data/meshes/ground_square.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e7c34705f7100c4a2e0382e91f474eee25a862eb
GIT binary patch
literal 684
zcmZ>*D9A4=Nlj5mE6UGRaLP%|OGz!#HPSP)RM03cO;Rw>WFQIfps8lr5p!T)ag;-+
zvxF^-eU~+7@4f9Y2Sk{o4uIIGT3|BBIzVigIHnG<b<qwdfp)wFngwG&b(FBR0gCeh
z?EtY+?Ld}+v5|Fy*r+;S=7MC9b>MatvJA4%kxjyG2gtq1s*rV~>Ohu(u|Z)4(~aqK
rWEo@~Fh61HKvo6w6UYwSI$$!$VFfZ5SvRUbkY!+OP&^}>#J~Umqy@m6
literal 0
HcmV?d00001
diff --git a/data/srdf/ground_bigStep.srdf b/data/srdf/ground_bigStep.srdf
new file mode 100644
index 00000000..f8a2b31a
--- /dev/null
+++ b/data/srdf/ground_bigStep.srdf
@@ -0,0 +1,19 @@
+<robot name="ground_bigstep">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_bigStep.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_bigStep.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/srdf/ground_jump_easy.srdf b/data/srdf/ground_jump_easy.srdf
new file mode 100644
index 00000000..38317233
--- /dev/null
+++ b/data/srdf/ground_jump_easy.srdf
@@ -0,0 +1,19 @@
+<robot name="ground_jump_easy">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easy.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_easy.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/srdf/ground_jump_med.srdf b/data/srdf/ground_jump_med.srdf
new file mode 100644
index 00000000..c367dd06
--- /dev/null
+++ b/data/srdf/ground_jump_med.srdf
@@ -0,0 +1,19 @@
+<robot name="ground_jump_med">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_med.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_med.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/ground_bigStep.urdf b/data/urdf/ground_bigStep.urdf
new file mode 100644
index 00000000..f8a2b31a
--- /dev/null
+++ b/data/urdf/ground_bigStep.urdf
@@ -0,0 +1,19 @@
+<robot name="ground_bigstep">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_bigStep.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_bigStep.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/ground_jump_easy.urdf b/data/urdf/ground_jump_easy.urdf
new file mode 100644
index 00000000..38317233
--- /dev/null
+++ b/data/urdf/ground_jump_easy.urdf
@@ -0,0 +1,19 @@
+<robot name="ground_jump_easy">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_easy.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_easy.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/ground_jump_med.urdf b/data/urdf/ground_jump_med.urdf
new file mode 100644
index 00000000..c367dd06
--- /dev/null
+++ b/data/urdf/ground_jump_med.urdf
@@ -0,0 +1,19 @@
+<robot name="ground_jump_med">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/ground_jump_med.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_med.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/script/tests/robot_bigStep_path .py b/script/tests/robot_bigStep_path .py
new file mode 100644
index 00000000..ab83be9b
--- /dev/null
+++ b/script/tests/robot_bigStep_path .py
@@ -0,0 +1,88 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+white=[1.0,1.0,1.0,1.0]
+green=[0.23,0.75,0.2,0.5]
+yellow=[0.85,0.75,0.15,1]
+pink=[1,0.6,1,1]
+orange=[1,0.42,0,1]
+brown=[0.85,0.75,0.15,0.5]
+blue = [0.0, 0.0, 0.8, 1.0]
+grey = [0.7,0.7,0.7,1.0]
+red = [0.8,0.0,0.0,1.0]
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'robot_test_trunk'
+urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
+urdfSuffix = ""
+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.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)
+rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
+
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+r.loadObstacleModel (packageName, "ground_bigStep", "planning")
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+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] = [4, -1, 0.9]; r (q_goal) # pont
+
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.client.problem.selectSteeringMethod("SteeringDynamic")
+ps.selectPathPlanner("RRTdynamic")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+r(q_init)
+
+r.solveAndDisplay("rm",1,0.02)
+
+
+#t = ps.solve ()
+
+#r.displayRoadmap("rm",0.005)
+
+r.displayPathMap("rmPath",0,0.02)
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+pp(0)
+
+
+pp.displayPath(1,blue)
+r.client.gui.setVisibility("path_0_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)
+#~ 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
new file mode 100644
index 00000000..d7222046
--- /dev/null
+++ b/script/tests/robot_jumpEasy_path .py
@@ -0,0 +1,88 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+white=[1.0,1.0,1.0,1.0]
+green=[0.23,0.75,0.2,0.5]
+yellow=[0.85,0.75,0.15,1]
+pink=[1,0.6,1,1]
+orange=[1,0.42,0,1]
+brown=[0.85,0.75,0.15,0.5]
+blue = [0.0, 0.0, 0.8, 1.0]
+grey = [0.7,0.7,0.7,1.0]
+red = [0.8,0.0,0.0,1.0]
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'robot_test_trunk'
+urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
+urdfSuffix = ""
+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.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)
+rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
+
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+r.loadObstacleModel (packageName, "ground_jump_easy", "planning")
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+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] = [4, -1, 0.9]; r (q_goal) # pont
+
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.client.problem.selectSteeringMethod("SteeringDynamic")
+ps.selectPathPlanner("RRTdynamic")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+r(q_init)
+
+r.solveAndDisplay("rm",1,0.02)
+
+
+#t = ps.solve ()
+
+#r.displayRoadmap("rm",0.005)
+
+r.displayPathMap("rmPath",0,0.02)
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+pp(0)
+
+
+pp.displayPath(1,blue)
+r.client.gui.setVisibility("path_0_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)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+
--
GitLab