From f4dbc05dae3f5ad26b457a151b8a81f7866b1f23 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 8 Feb 2017 14:06:13 +0100
Subject: [PATCH] down slope path work
---
CMakeLists.txt | 4 +
data/meshes/downSlope.stl | Bin 0 -> 2484 bytes
data/srdf/downSlope.srdf | 19 +++
data/urdf/downSlope.urdf | 19 +++
data/urdf/hyq/hyq_trunk.urdf | 4 +
script/dynamic/downSlope_hyq_pathKino.py | 184 ++++++++++++++++++++++
script/dynamic/sideWall_hyq_interpKino.py | 2 +-
script/tools/displayCOntact.py | 18 ++-
script/tools/screenCap.py | 2 +-
9 files changed, 246 insertions(+), 6 deletions(-)
create mode 100644 data/meshes/downSlope.stl
create mode 100755 data/srdf/downSlope.srdf
create mode 100755 data/urdf/downSlope.urdf
create mode 100644 script/dynamic/downSlope_hyq_pathKino.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2abc45..e435d4d 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -107,6 +107,7 @@ install(FILES
data/urdf/roomWall.urdf
data/urdf/sideWall.urdf
data/urdf/sideWall_long.urdf
+ data/urdf/downSlope.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
@@ -151,6 +152,7 @@ install(FILES
data/srdf/roomWall.srdf
data/srdf/sideWall.srdf
data/srdf/sideWall_long.srdf
+ data/srdf/downSlope.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
@@ -199,6 +201,8 @@ install(FILES
data/meshes/roomWall.stl
data/meshes/sideWall.stl
data/meshes/sideWall_long.stl
+ data/meshes/downSlope.stl
+ data/meshes/downSlope2.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
install(FILES
diff --git a/data/meshes/downSlope.stl b/data/meshes/downSlope.stl
new file mode 100644
index 0000000000000000000000000000000000000000..bcdec81c9d0714e7e7cdcfe62aa059587a147594
GIT binary patch
literal 2484
zcmb7EO-NKx6uv=g7fQ4mm_>mFh6t5`_$Hx(7J&t>T0}TTQ8`T(wTRxrqK!DzqD4PL
ziy${zjZ~1+d(g@RSGf>3A!^GA6biBFe&^0N@7$NO=&nBQJ>NOsdGDO>*sa2FaU?&O
z9V!lA$@X2&UmeUBJGu@W?#{Mfzi~d>xzGI9>6GX&BB^C^A`VZz>>0iHB>;YQa!2mn
zhpJ%Q12Z-h#3(Q_XWKjm*<iS6AV!hFyu9~nh%&rrRhmJJ%<j}`cO#vPJ+Tk1s=;V<
zt5p$$0c_08%+x1vUX2%`fr+1ivEQWh{zdiv>#g9ajXL-FyI|Y{yZr1P3`BwPsJuT1
ze5Y+W&}?H`GE=?aRmTg#=94`V)jhHnfPuVVXm+DUv{@^^(NSrIiK>dt4PMUnqjNzC
zqkL7CA^+m2Rj2!aLNK6Et3~!?uHwbh)%ZlI#|+&52z<O0=fcNdtGovW1IJAYqrf<H
z@pqsZ#7Q&|qsTx#<mG)*0~11qR;3xlh~o((ol9${+N6f#gaKc)Dq{2jY|O7pzbIN>
zXbpzO+_bk}n+x_H*&Sm_Hr8Xoq5aap4Mo9)rQ$&yWy*asV8k5{X1>jhRb}4hXYJQr
zsq_o)&q8aIj>_4*B^%{4p3m;8;4k(*T<HP>QOJvWR00Os#;hW@J?pj~ol7%3wP1*<
z4XX~FLoYhl^|V@KPv(ldjqTJ<MC^Hq)Zt5dx3yzP4T%Qg3mNEKW3PsEznx!EA=sKB
zwLHHfn|`$|<DYKRs!*@FU+E3@_Z)h#gNwBv(>hA;wss6-&eZ*CGUZp=8MG_83jB-u
zMKiQ_8Pm+KG+TR@FyvpP{YvN6^^Dm$?RIcJwS#&p{n>S$IKR>_n&HpxYJCT{Qah-p
z-QTFU^&PylRP=T*?GpI^&KcI4?4Zu8_t6;3kZHGD>+PVfp%u!6$kzp;55mc`+Yu+8
zXnj}e@Fneo+FhlFL<8}K47xV%(@yQ&fiF}=2Gu62kRQ{?8h9ZaxGBT)c1EQY8uJ$p
C%Cz+W
literal 0
HcmV?d00001
diff --git a/data/srdf/downSlope.srdf b/data/srdf/downSlope.srdf
new file mode 100755
index 0000000..89dff15
--- /dev/null
+++ b/data/srdf/downSlope.srdf
@@ -0,0 +1,19 @@
+<robot name="stair_bauzil">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/downSlope.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/downSlope.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/downSlope.urdf b/data/urdf/downSlope.urdf
new file mode 100755
index 0000000..8b448cf
--- /dev/null
+++ b/data/urdf/downSlope.urdf
@@ -0,0 +1,19 @@
+<robot name="stair_bauzil">
+ <link name="base_link">
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://hpp-rbprm-corba/meshes/downSlope2.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/downSlope2.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/hyq/hyq_trunk.urdf b/data/urdf/hyq/hyq_trunk.urdf
index c7b43f8..4473749 100755
--- a/data/urdf/hyq/hyq_trunk.urdf
+++ b/data/urdf/hyq/hyq_trunk.urdf
@@ -1,5 +1,9 @@
<robot name="hyq_trunk">
<link name="base_link">
+ <inertial>
+ <mass value="50.26"/>
+ <inertia ixx="1." ixy="0.0" ixz="0.0" iyy="1." iyz="0.0" izz="1."/>
+ </inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
diff --git a/script/dynamic/downSlope_hyq_pathKino.py b/script/dynamic/downSlope_hyq_pathKino.py
new file mode 100644
index 0000000..99e85a4
--- /dev/null
+++ b/script/dynamic/downSlope_hyq_pathKino.py
@@ -0,0 +1,184 @@
+## Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+import time
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+# URDF file describing the trunk of the robot HyQ
+urdfName = 'hyq_trunk'
+# URDF files describing the reachable workspace of each limb of HyQ
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+vMax = 4;
+aMax = 5;
+extraDof = 6
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,5, 0, 2, 0.3, 1.8])
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso. (z, y, x)
+rbprmBuilder.boundSO3([-0.1,0.1,-0.65,0.65,-0.2,0.2])
+rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-1,1,-2,2,0,0,0,0,0,0])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",aMax)
+ps.client.problem.setParameter("vMax",vMax)
+r = Viewer (ps)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "downSlope", "planning", r)
+#r.loadObstacleModel (packageName, "ground", "planning")
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+r.addLandmark(r.sceneName,1)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[3:7] = [0.9659,0,0.2588,0]
+q_init [0:3] = [-1.25, 1, 1.7]; r (q_init)
+#q_init[3:7] = [0.7071,0,0,0.7071]
+#q_init [0:3] = [1, 1, 0.65]
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+#q_goal[3:7] = [0.7071,0,0,0.7071]
+#q_goal [0:3] = [1, 5, 0.65]; r(q_goal)
+q_goal[3:7] = [1,0,0,0]
+q_goal [0:3] = [2, 1, 0.60]; r(q_goal)
+#q_goal[3:7] = [0.9659,0,0.2588,0]
+#q_goal[7:10] = [vMax,0,-2]
+#q_goal [0:3] = [0, 1, 0.8]; r(q_goal)
+
+r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
+
+# Choosing a path optimizer
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+# Choosing RBPRM shooter and path validation methods.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+# Choosing kinodynamic methods :
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+
+#ps.client.problem.prepareSolveStepByStep()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+#r.solveAndDisplay("rm",1,0.01)
+
+
+ps.solve ()
+
+# seed = 1486546394 (hyq_trunk, 2m)
+
+"""
+camera = [0.6293167471885681,
+ -9.560577392578125,
+ 10.504343032836914,
+ 0.9323806762695312,
+ 0.36073973774909973,
+ 0.008668755181133747,
+ 0.02139890193939209]
+r.client.gui.setCameraTransform(0,camera)
+"""
+
+"""
+r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmstart",r.sceneName)
+r.client.gui.removeFromGroup("rmgoal",r.sceneName)
+for i in range(0,ps.numberNodes()):
+ r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
+
+"""
+
+
+# Playing the computed path
+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")
+#display path
+pp.speed=0.3
+#pp (0)
+
+#display path with post-optimisation
+
+
+ps.client.problem.extractPath(0,0,2.6)
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+#pp (1)
+
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+"""
+for i in range(1,10):
+ rbprmBuilder.client.basic.problem.optimizePath(i)
+ r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+ pp.displayVelocityPath(i+1)
+ #time.sleep(2)
+"""
+
+"""
+i=0
+
+ps.clearRoadmap()
+ps.solve()
+r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+i = i+1
+pp.displayVelocityPath(i)
+
+pp(i)
+
+
+"""
+
+"""
+r.client.gui.addCurve("c1",qlist,r.color.red)
+r.client.gui.setVisibility("c1","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c1",r.sceneName)
+
+
+r.client.gui.addCurve("c2",qlist2,r.color.blue)
+r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c2",r.sceneName)
+
+
+
+"""
+
+
+
diff --git a/script/dynamic/sideWall_hyq_interpKino.py b/script/dynamic/sideWall_hyq_interpKino.py
index c0393f0..1436bc0 100644
--- a/script/dynamic/sideWall_hyq_interpKino.py
+++ b/script/dynamic/sideWall_hyq_interpKino.py
@@ -81,7 +81,7 @@ fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
r(q_init)
# computing the contact sequence
-configs = fullBody.interpolate(0.05,pathId=1,robustnessTreshold = 2, filterStates = True)
+configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
print "number of configs =", len(configs)
diff --git a/script/tools/displayCOntact.py b/script/tools/displayCOntact.py
index 52df83d..f979f25 100644
--- a/script/tools/displayCOntact.py
+++ b/script/tools/displayCOntact.py
@@ -4,19 +4,18 @@ q2 = [[1.41929 , 0.56273 , 0.3167],[0.633238 , 0.498175 , 0.252145],[1.41049 , -
-"""
-color = r.color.black
+color = r.color.black
for i in range(0,len(q)):
r.client.gui.addSphere("s"+str(i),0.03,color)
r.client.gui.applyConfiguration("s"+str(i),q[i]+[1,0,0,0])
+ r.client.gui.setVisibility("s"+str(i),"ALWAYS_ON_TOP")
r.client.gui.addToGroup("s"+str(i),r.sceneName)
-"""
+r.client.gui.refresh()
color = r.color.red
-
for i in range(0,len(q2)):
r.client.gui.addSphere("sc"+str(i),0.01,color)
r.client.gui.applyConfiguration("sc"+str(i),q2[i]+[1,0,0,0])
@@ -26,6 +25,17 @@ for i in range(0,len(q2)):
r.client.gui.refresh()
+color = r.color.blue
+name = "vec2"
+
+quat = rbprmBuilder.quaternionFromVector(v[3:6])
+v[3:7] = quat[::]
+r.client.gui.addArrow(name,0.02,1,color)
+r.client.gui.addToGroup(name,r.sceneName)
+r.client.gui.setVisibility(name,"ALWAYS_ON_TOP")
+r.client.gui.applyConfiguration(name,v)
+r.client.gui.refresh()
+
""""""""""""""""""""""""""""""""""""""""""""
diff --git a/script/tools/screenCap.py b/script/tools/screenCap.py
index a1f018f..bcf3816 100644
--- a/script/tools/screenCap.py
+++ b/script/tools/screenCap.py
@@ -38,7 +38,7 @@ tp.r.stopCapture ()
"""
## avconv (bash) commands
-avconv -i capture_0_%d.png -r 30 -vcodec mpeg4 -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -y sideWall_hyq_contactsPlan.mp4
+avconv -i capture_0_%d.png -r 30 -vcodec mpeg4 -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -y sideWall_hyq_contactsPlan2.mp4
avconv -i capture_0_%d.png -r 30 -vcodec mpeg4 -qscale 1 -mbd rd -flags +mv4+aic -trellis 2 -cmp 2 -subcmp 2 -g 300 -pass 2 hyq_darpa.mp4
--
GitLab