Skip to content
Snippets Groups Projects
Commit b617fef4 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] use hyq_contact6D in darpa (required for whole body motion)

parent 79a2dcfb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
script/scenarios/demos/darpa_hyq.py
+ 28
− 28
View file @ b617fef4
#Importing helper class for RBPRM #Importing helper class for RBPRM
from hpp.corbaserver.rbprm.hyq import Robot from hpp.corbaserver.rbprm.hyq_contact6D import Robot
from hpp.corbaserver.problem_solver import ProblemSolver from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
...@@ -23,10 +23,10 @@ fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4]) ...@@ -23,10 +23,10 @@ fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
nbSamples = 10000 nbSamples = 10000
ps = tp.ProblemSolver(fullBody) ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps, viewerClient=tp.r.client) v = tp.Viewer (ps, viewerClient=tp.v.client)
rootName = 'base_joint_xyz' rootName = 'base_joint_xyz'
cType = "_3_DOF" cType = "_6_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False): def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision) fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
...@@ -61,8 +61,8 @@ q_init = [-2.0, ...@@ -61,8 +61,8 @@ q_init = [-2.0,
-0.9577096766517215, -0.9577096766517215,
0.9384602821326071] 0.9384602821326071]
""" """
q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02 q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02 q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]
q_init = fullBody.generateContacts(q_init, [0,0,1]) q_init = fullBody.generateContacts(q_init, [0,0,1])
...@@ -73,12 +73,12 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f ...@@ -73,12 +73,12 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId]) fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
r(q_init) v(q_init)
configs = [] configs = []
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client, r) pp = PathPlayer (fullBody.client, v)
import time import time
...@@ -86,36 +86,36 @@ import time ...@@ -86,36 +86,36 @@ import time
#DEMO METHODS #DEMO METHODS
def initConfig(): def initConfig():
r.client.gui.setVisibility("hyq", "ON") v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default") tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF") tp.v.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_init) v(q_init)
def endConfig(): def endConfig():
r.client.gui.setVisibility("hyq", "ON") v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default") tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF") tp.v.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_goal) v(q_goal)
def rootPath(): def rootPath():
r.client.gui.setVisibility("hyq", "OFF") v.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path") tp.cl.problem.selectProblem("rbprm_path")
tp.r.client.gui.setVisibility("toto", "OFF") tp.v.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF") v.client.gui.setVisibility("hyq", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "ON") tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0) tp.pp(0)
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
r.client.gui.setVisibility("hyq", "ON") v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default") tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.06): def genPlan(stepsize=0.06):
tp.cl.problem.selectProblem("default") tp.cl.problem.selectProblem("default")
r.client.gui.setVisibility("hyq", "ON") v.client.gui.setVisibility("hyq", "ON")
tp.r.client.gui.setVisibility("toto", "OFF") tp.v.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs global configs
start = time.clock() start = time.clock()
configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True) configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True)
...@@ -123,12 +123,12 @@ def genPlan(stepsize=0.06): ...@@ -123,12 +123,12 @@ def genPlan(stepsize=0.06):
print "Contact plan generated in " + str(end-start) + "seconds" print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5): def contactPlan(step = 0.5):
r.client.gui.setVisibility("hyq", "ON") v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default") tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF") tp.v.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF") tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)): for i in range(0,len(configs)):
r(configs[i]); v(configs[i]);
time.sleep(step) time.sleep(step)
......
script/scenarios/demos/darpa_hyq_path.py
+ 17
− 12
View file @ b617fef4
...@@ -21,13 +21,24 @@ rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3]) ...@@ -21,13 +21,24 @@ rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer # Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.problem_solver import ProblemSolver from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder ) ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps) from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", vf,reduceSizes=[0.1,0,0])
v = vf.createViewer()
#afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
# Setting initial and goal configurations # Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig (); q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
q_goal = q_init [::] q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal) q_goal [0:3] = [3, 0, 0.63]; v (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal) #~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer # Choosing a path optimizer
...@@ -35,12 +46,6 @@ ps.addPathOptimizer("RandomShortcut") ...@@ -35,12 +46,6 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods. # Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used. # Note that the standard RRT algorithm is used.
ps.client.problem.selectConfigurationShooter("RbprmShooter") ps.client.problem.selectConfigurationShooter("RbprmShooter")
...@@ -56,11 +61,11 @@ print "computation time for root path ", t ...@@ -56,11 +61,11 @@ print "computation time for root path ", t
# Playing the computed path # Playing the computed path
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (r) pp = PathPlayer (v)
q_far = q_init [::] q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63]; q_far [0:3] = [-2, -3, 0.63];
r(q_far) v(q_far)
for i in range(1,10): for i in range(1,10):
rbprmBuilder.client.problem.optimizePath(i) rbprmBuilder.client.problem.optimizePath(i)
...@@ -76,6 +81,6 @@ rbprmBuilder2 = Robot ("toto") ...@@ -76,6 +81,6 @@ rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 ) ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default") cl.problem.selectProblem("default")
cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:]) cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
r2 = Viewer (ps2, viewerClient=r.client) r2 = Viewer (ps2, viewerClient=v.client)
r2(q_far) r2(q_far)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment