From 46f31fc8558e1e3412e0529533e103ffc1e41732 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 1 Mar 2017 22:11:22 +0100
Subject: [PATCH] add scene slope 20 degree
---
CMakeLists.txt | 3 +
data/meshes/downSlope_easy20.stl | Bin 0 -> 2484 bytes
data/srdf/downSlope_easy20.srdf | 19 ++
data/urdf/downSlope_easy20.urdf | 19 ++
profile/profile.py | 2 +-
.../scenarios/downSlope_easy20_hrp2_interp.py | 128 ++++++++++++
.../downSlope_easy20_hrp2_pathKino.py | 193 ++++++++++++++++++
.../scenarios/downSlope_easy_hrp2_interp.py | 2 +-
8 files changed, 364 insertions(+), 2 deletions(-)
create mode 100644 data/meshes/downSlope_easy20.stl
create mode 100755 data/srdf/downSlope_easy20.srdf
create mode 100644 data/urdf/downSlope_easy20.urdf
create mode 100644 profile/scenarios/downSlope_easy20_hrp2_interp.py
create mode 100644 profile/scenarios/downSlope_easy20_hrp2_pathKino.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 23b9f58..1b0e568 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -109,6 +109,7 @@ install(FILES
data/urdf/sideWall_long.urdf
data/urdf/downSlope.urdf
data/urdf/downSlope_easy.urdf
+ data/urdf/downSlope_easy20.urdf
data/urdf/slalom.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
@@ -156,6 +157,7 @@ install(FILES
data/srdf/sideWall_long.srdf
data/srdf/downSlope.srdf
data/srdf/downSlope_easy.srdf
+ data/srdf/downSlope_easy20.srdf
data/srdf/slalom.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
@@ -207,6 +209,7 @@ install(FILES
data/meshes/sideWall_long.stl
data/meshes/downSlope.stl
data/meshes/downSlope_easy.stl
+ data/meshes/downSlope_easy20.stl
data/meshes/downSlope2.stl
data/meshes/slalom.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
diff --git a/data/meshes/downSlope_easy20.stl b/data/meshes/downSlope_easy20.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e2eac074e61d1c54ed5d2dce24e2420a7dbeed70
GIT binary patch
literal 2484
zcmb7FF>4f27<`S8KM^>NBx<1?HYp_cj);|=QN->DxnMw(<1|Di;QoWt%38EYp~l#n
zyA5cX*vVCalon}31cg`_cfOa|H*eL#Eph`h^Ue2t@9pOD{q9QdUT2}U*ju?%o4wsx
zUg-4BT{u5IT|0Akb-va(>;CI$m^#a8YOZbVv_5?ue!p_v6Zbza8}5F5?}4FB9c17D
zRyQ!KXNIaOy4Szd&CLwUF^4k5%wS$r1F4D_b%8M-GeSpnkf910bSHD<a8&ohy{Q{I
zJYiqT5Tg^V6!hV5mQ%%PnV~yO9m>GX{b;4cQBKHCcpm@kD(RPXxm~)E5pPMhe|4}?
z|2Dk!bTlA#a}VADj}G3nfd6!mp-SDroFklKRYiAW>BtiY$MNwayI0B(GXoB0#9N{&
zV)PD-`N}!0KGKvSMqRp@;at&`-K(mKb#-s$<mOO@m>JwU9r(?1-IaEt*{MSr&Cp7T
z!?SMnoayGS=*qk@%;R?B3BK-kZR)x4&F13I)Al~^{<z`C_9hKaoa!3JO$2uC<%y$R
zIG`c}H!$2<DRu;;Ds^Yir)J=8KYX#BIufQmx(L<dCx-*A<lL)1dUNjP_~y`wO5MPC
z=R<}%5~grNg$%7!c4hklLzTL9Z>d9>WIs~Lxl8E3c-|IUP9~I@G&^-DqZvshWHe7Q
zyzWQOL%X$%(xHq_u9Z+dey=uWkNk&UhxWZ9#!UpC+NuBLd!-C9-a)$KdtT(&xmQ)J
zd+phJix^$u2@Yk5nE?kg;`d5b#FzsZ^YI-_9b~9NUAmcJUD>^=s_5qA%!m$Uh?&8y
rLmeJ`7j;+KiDpxe-z--~Gqh54;6DQER?nGkox?J`GR%WYD>?TYw73Ag
literal 0
HcmV?d00001
diff --git a/data/srdf/downSlope_easy20.srdf b/data/srdf/downSlope_easy20.srdf
new file mode 100755
index 0000000..6e8c6fb
--- /dev/null
+++ b/data/srdf/downSlope_easy20.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_easy20.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_easy20.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/data/urdf/downSlope_easy20.urdf b/data/urdf/downSlope_easy20.urdf
new file mode 100644
index 0000000..6e8c6fb
--- /dev/null
+++ b/data/urdf/downSlope_easy20.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/downSlope_easy20.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_easy20.stl"/>
+ </geometry>
+ </collision>
+ </link>
+</robot>
diff --git a/profile/profile.py b/profile/profile.py
index 4045289..5ae32e0 100755
--- a/profile/profile.py
+++ b/profile/profile.py
@@ -9,7 +9,7 @@ import time
#~ scenarios = ['standing_hrp2']
scenarios = ['downSlope_easy_hrp2']
#~ scenarios = ['stair_bauzil_hrp2']
-n_trials = 20
+n_trials = 14
stats = ['balance','collision','ik']
stats_optim = ['time_cwc','com_traj']
diff --git a/profile/scenarios/downSlope_easy20_hrp2_interp.py b/profile/scenarios/downSlope_easy20_hrp2_interp.py
new file mode 100644
index 0000000..016f855
--- /dev/null
+++ b/profile/scenarios/downSlope_easy20_hrp2_interp.py
@@ -0,0 +1,128 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import downSlope_easy20_hrp2_pathKino as tp
+import time
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+# Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-2,5, 0, 2, 0.45, 1.8])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+
+ps = tp.ProblemSolver( fullBody )
+ps.client.problem.setParameter("aMax",tp.aMax)
+ps.client.problem.setParameter("vMax",tp.vMax)
+r = tp.Viewer (ps,viewerClient=tp.r.client)
+
+#~ AFTER loading obstacles
+rLegId = 'hrp2_rleg_rom'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.1)
+
+lLegId = 'hrp2_lleg_rom'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.1)
+
+
+
+
+q_0 = fullBody.getCurrentConfig();
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+
+
+q_init =[0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,0.0, 0.0, 0.0, 0.0,0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,0,0,0,0,0,0]; r (q_init)
+fullBody.setCurrentConfig (q_init)
+
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
+dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
+acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
+acc_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS+3:tp.indexECS+6]
+
+robTreshold = 3
+# copy extraconfig for start and init configurations
+q_init[configSize:configSize+3] = dir_init[::]
+q_init[configSize+3:configSize+6] = acc_init[::]
+q_goal[configSize:configSize+3] = dir_goal[::]
+q_goal[configSize+3:configSize+6] = acc_goal[::]
+
+
+# FIXME : test
+q_init[2] = q_init[2]+0.15
+q_goal[2] = q_goal[2]+0.15
+
+fullBody.setStaticStability(False)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
+r(q_goal)
+
+# specifying the full body configurations as start and goal state of the problem
+
+r(q_init)
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+
+
+
+configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = True)
+print "number of configs :", len(configs)
+r(configs[-1])
+time.sleep(3)
+
+"""
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayerHrp2
+
+player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=False,use_velocity=True,pathId = pId)
+
+#player.displayContactPlan()
+
+player.interpolate(4,5)
+
+"""
+
+"""
+import hpp.corbaserver.rbprm.tools.cwc_trajectory
+import hpp.corbaserver.rbprm.tools.path_to_trajectory
+import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
+
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory)
+reload(hpp.corbaserver.rbprm.tools.path_to_trajectory)
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory_helper)
+reload(fullBodyPlayerHrp2)
+
+
+"""
diff --git a/profile/scenarios/downSlope_easy20_hrp2_pathKino.py b/profile/scenarios/downSlope_easy20_hrp2_pathKino.py
new file mode 100644
index 0000000..0033b25
--- /dev/null
+++ b/profile/scenarios/downSlope_easy20_hrp2_pathKino.py
@@ -0,0 +1,193 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+class Robot (Parent):
+ rootJointType = 'freeflyer'
+ packageName = 'hpp-rbprm-corba'
+ meshPackageName = 'hpp-rbprm-corba'
+ # URDF file describing the trunk of the robot HyQ
+ urdfName = 'hrp2_trunk_flexible'
+ urdfSuffix = ""
+ srdfSuffix = ""
+ def __init__ (self, robotName, load = True):
+ Parent.__init__ (self, robotName, self.rootJointType, load)
+ self.tf_root = "base_footprint"
+ self.client.basic = Client ()
+ self.load = load
+
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk_flexible'
+urdfNameRom = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+vMax = 4;
+aMax = 6;
+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", [-2,4, 0, 2, 0.2, 1.4])
+rbprmBuilder.setJointBounds('CHEST_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.35,0.1])
+rbprmBuilder.setJointBounds('HEAD_JOINT0',[0,0])
+rbprmBuilder.setJointBounds('HEAD_JOINT1',[0,0])
+
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_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([-4,4,-1,1,-2,2,0,0,0,0,0,0])
+indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
+
+# Creating an instance of HPP problem solver and the viewer
+
+ps = ProblemSolver( rbprmBuilder )
+ps.client.problem.setParameter("aMax",aMax)
+ps.client.problem.setParameter("vMax",vMax)
+ps.client.problem.setParameter("sizeFootX",0.24)
+ps.client.problem.setParameter("sizeFootY",0.14)
+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_easy20", "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] = [1,0,0,0]
+q_init[8] = -0.2
+q_init [0:3] = [-1.6, 1, 1.25]; 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] = [1,0,0,0]
+q_goal[8] = 0
+q_goal [0:3] = [3, 1, 0.55]; 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)
+
+
+#r.solveAndDisplay("rm",1,0.01)
+
+t = ps.solve ()
+if isinstance(t, list):
+ t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+
+
+"""
+for i in range(0,9):
+ t = ps.solve()
+ if isinstance(t, list):
+ ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.
+ f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
+ f.write("t = "+str(ts) + "\n")
+ f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "\n")
+ f.close()
+ print "problem "+str(i)+" solved \n"
+ ps.clearRoadmap()
+"""
+#ps.client.problem.prepareSolveStepByStep()
+
+#ps.client.problem.finishSolveStepByStep()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+
+
+
+
+"""
+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)
+
+"""
+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")
+
+
+"""
+# for seed 1486657707
+ps.client.problem.extractPath(0,0,2.15)
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=0.3
+#pp (0)
+"""
+#display path with post-optimisation
+
+
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+
+
diff --git a/profile/scenarios/downSlope_easy_hrp2_interp.py b/profile/scenarios/downSlope_easy_hrp2_interp.py
index efc9070..658b928 100644
--- a/profile/scenarios/downSlope_easy_hrp2_interp.py
+++ b/profile/scenarios/downSlope_easy_hrp2_interp.py
@@ -95,7 +95,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId])
-configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 3, filterStates = True)
+configs = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 0, filterStates = True)
print "number of configs :", len(configs)
r(configs[-1])
time.sleep(3)
--
GitLab