From ea8516c5227a76f9c43f40a4af209cc0a7f0d7c9 Mon Sep 17 00:00:00 2001
From: Akseppal <seppala@laas.fr>
Date: Tue, 26 Apr 2016 11:39:26 +0200
Subject: [PATCH] add test file that uses affordance.py interface of
 hpp-affordance-corba

---
 test3.py | 53 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)
 create mode 100644 test3.py

diff --git a/test3.py b/test3.py
new file mode 100644
index 00000000..806e2b5a
--- /dev/null
+++ b/test3.py
@@ -0,0 +1,53 @@
+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk_large'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+rbprmBuilder.setFilter(['hyq_rhleg_rom']) 
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+import random
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, 'planning', [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")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp (0)
-- 
GitLab