From a846f1d095d2c6348a018c307f9506e86d7e8afb Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Mon, 27 Jun 2016 17:44:43 +0200
Subject: [PATCH] stair and darpa scenes compliant with affordance api
---
affordance-tests/darpa_hyq_interp.py | 15 +++++++++------
script/scenarios/darpa_hyq_interp.py | 3 ++-
script/scenarios/darpa_hyq_path.py | 14 +++++++++-----
script/scenarios/stair_bauzil_hrp2_interp.py | 9 +++++++--
script/scenarios/stair_bauzil_hrp2_path.py | 12 +++++++++++-
src/rbprmbuilder.impl.cc | 1 -
6 files changed, 38 insertions(+), 16 deletions(-)
diff --git a/affordance-tests/darpa_hyq_interp.py b/affordance-tests/darpa_hyq_interp.py
index b279d861..8e1e62b0 100755
--- a/affordance-tests/darpa_hyq_interp.py
+++ b/affordance-tests/darpa_hyq_interp.py
@@ -88,10 +88,13 @@ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
-collisions = 0;
-for config in configs:
- if fullBody.isConfigValid(config)[0] == False:
- collisions += 1
-
-collisions
+#~ collisions = 0;
+#~ for config in configs:
+ #~ if fullBody.isConfigValid(config)[0] == False:
+ #~ collisions += 1
+#~
+#~ collisions
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (ps.robot.client.basic, r)
diff --git a/script/scenarios/darpa_hyq_interp.py b/script/scenarios/darpa_hyq_interp.py
index 5430f7c6..bec7f844 100755
--- a/script/scenarios/darpa_hyq_interp.py
+++ b/script/scenarios/darpa_hyq_interp.py
@@ -88,4 +88,5 @@ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1
-
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (ps.robot.client.basic, r)
diff --git a/script/scenarios/darpa_hyq_path.py b/script/scenarios/darpa_hyq_path.py
index 3f3a30c0..cd5e4420 100755
--- a/script/scenarios/darpa_hyq_path.py
+++ b/script/scenarios/darpa_hyq_path.py
@@ -21,11 +21,10 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# 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'])
-# ... and only if a contact surface includes the gravity
-rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.5)
+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.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
@@ -45,6 +44,11 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
diff --git a/script/scenarios/stair_bauzil_hrp2_interp.py b/script/scenarios/stair_bauzil_hrp2_interp.py
index 2f80a41b..6953a6e6 100755
--- a/script/scenarios/stair_bauzil_hrp2_interp.py
+++ b/script/scenarios/stair_bauzil_hrp2_interp.py
@@ -88,8 +88,8 @@ fullBody.setCurrentConfig (q_init)
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
- 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17
- 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24
+ 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
+ 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
@@ -127,3 +127,8 @@ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ f1.write(str(configs))
#~ f1.close()
+from hpp.gepetto import PathPlayer
+#~ pp = PathPlayer (ps.robot.client.basic, r, 0.001, 0.1)
+pp = PathPlayer (ps.robot.client.basic, r)
+#~ (self, client, publisher, dt=0.01, speed=1)
+
diff --git a/script/scenarios/stair_bauzil_hrp2_path.py b/script/scenarios/stair_bauzil_hrp2_path.py
index d0978c9d..e8e39dc1 100755
--- a/script/scenarios/stair_bauzil_hrp2_path.py
+++ b/script/scenarios/stair_bauzil_hrp2_path.py
@@ -14,6 +14,9 @@ rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
+rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
+rbprmBuilder.setAffordanceFilter('0rLeg', ['Support',])
+rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
@@ -44,9 +47,16 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
+#~ afftool.analyseAll()
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
+
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-r.loadObstacleModel (packageName, "stair_bauzil", "planning")
#~ ps.solve ()
t = ps.solve ()
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index e2bed9f3..01713642 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -675,7 +675,6 @@ namespace hpp {
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
}
//create helper
-// /interpolation::LimbRRTHelper helper(fullBody_, problemSolver_->problem());
core::PathVectorPtr_t path = interpolation::interpolateStates(fullBody_,problemSolver_->problem(),
lastStatesComputed_.begin()+s1,lastStatesComputed_.begin()+s2);
problemSolver_->addPath(path);
--
GitLab