Skip to content
GitLab
Commit 44207e1a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts] refactor all HYQ scripts in demos/

parent afd0446c
Hide whitespace changes
Inline Side-by-side
script/scenarios/demos/darpa_hyq.py deleted 100644 → 0
View file @ afd0446c
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.hyq_contact6D import Robot
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
from . import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
# This time we load the full body model of HyQ
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
#~ fullBody.client.robot.setDimensionExtraConfigSpace(6)
#~ fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples = 10000
ps = tp.ProblemSolver(fullBody)
v = tp.Viewer (ps, viewerClient=tp.v.client)
rootName = 'base_joint_xyz'
cType = "_6_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_init=fullBody.referenceConfig[::] ; q_init[0:7] = tp.q_init[0:7];
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7];
q_init = fullBody.generateContacts(q_init, [0,0,1])
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
v(q_init)
configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client, v)
import time
#DEMO METHODS
def initConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_init)
def endConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_goal)
def rootPath():
v.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path")
tp.v.client.gui.setVisibility("toto", "OFF")
v.client.gui.setVisibility("hyq", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0)
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.06):
tp.cl.problem.selectProblem("default")
v.client.gui.setVisibility("hyq", "ON")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print(("Contact plan generated in " + str(end-start) + "seconds"))
def contactPlan(step = 0.5):
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
v(configs[i]);
time.sleep(step)
print(("Root path generated in " + str(tp.t) + " ms."))
genPlan(0.01);contactPlan(0.01)
script/scenarios/demos/darpa_hyq3d.py deleted 100644 → 0
View file @ afd0446c
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.hyq import Robot
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
#calling script darpa_hyq_path to compute root path
from . import darpa_hyq_path as tp
from os import environ
ins_dir = environ['DEVEL_HPP_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
from hpp.corbaserver import Client
# This time we load the full body model of HyQ
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples = 10000
ps = tp.ProblemSolver(fullBody)
v = tp.Viewer (ps, viewerClient=tp.v.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7];
#q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]+ [0]*6; 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_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId], [[0.,0.,1.] for _ in range(4)])
v(q_init)
configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client, v)
import time
#DEMO METHODS
def initConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_init)
def endConfig():
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_goal)
def rootPath():
v.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path")
tp.v.client.gui.setVisibility("toto", "OFF")
v.client.gui.setVisibility("hyq", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0)
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.06):
tp.cl.problem.selectProblem("default")
v.client.gui.setVisibility("hyq", "ON")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print("Contact plan generated in " + str(end-start) + "seconds")
def contactPlan(step = 0.5):
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
v(configs[i]);
time.sleep(step)
print("Root path generated in " + str(tp.t) + " ms.")
genPlan(0.01);contactPlan(0.01)
script/scenarios/demos/darpa_hyq_path.py deleted 100644 → 0
View file @ afd0446c
# Importing helper class for setting up a reachability planning problem
from hpp.corbaserver.rbprm.hyq_abstract import Robot
# Importing Gepetto viewer helper class
from hpp.gepetto import Viewer
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
rbprmBuilder.setJointBounds ("root_joint", [-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'])
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,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
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_environments", "multicontact/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
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.64]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.64]; v (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# 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 ()
#~ t = 0.
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
print(("computation time for root path ", t))
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.6];
v(q_far)
for i in range(1,10):
rbprmBuilder.client.problem.optimizePath(i)
#~ pp(9)
from hpp.corbaserver import Client
#DEMO code to play root path and final contact plan
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
r2 = Viewer (ps2, viewerClient=v.client)
r2(q_far)
script/scenarios/demos/hyq_darpa.py 0 → 100644
View file @ 44207e1a
from scenarios.demos.hyq_darpa_path import PathPlanner
from scenarios.hyq_contact_generator import HyqContactGenerator
class ContactGenerator(HyqContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
def load_limbs(self):
dict_heuristic = {self.fullbody.rLegId: "fixedStep04",
self.fullbody.lLegId: "fixedStep04",
self.fullbody.rArmId: "static",
self.fullbody.lArmId: "static"}
super().load_limbs(dict_heuristic, "ReferenceConfiguration")
def compute_configs_from_guide(self):
super().compute_configs_from_guide()
self.q_init[2] = self.q_ref[2]
self.q_goal[2] = self.q_ref[2]
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
cg.play_guide_path()
cg.display_sequence()
\ No newline at end of file
script/scenarios/demos/hyq_darpa_path.py 0 → 100644
View file @ 44207e1a
from scenarios.hyq_path_planner import HyqPathPlanner
class PathPlanner(HyqPathPlanner):
def init_problem(self):
super().init_problem()
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
def run(self):
self.root_translation_bounds = [-2,5, -1, 1, 0.3, 4]
self.root_rotation_bounds = [-0.4,0.4,-0.3,0.3,-0.3,0.3]
self.init_problem()
self.q_init[0:2] = [-2, 0]
self.q_goal[0:2] = [3, 0]
self.init_viewer("multicontact/darpa", visualize_affordances=["Support"], reduce_sizes=[0.06,0,0])
self.init_planner(kinodynamic=False)
self.solve()
print("Optimize path ...")
for i in range(1, 6):
self.rbprmBuilder.client.problem.optimizePath(i)
print("Optimization pass "+str(i)+"/5 done.")
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
script/scenarios/demos/hyq_slalom_debris.py
View file @ 44207e1a
from hpp.corbaserver.rbprm.hyq_contact6D import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import time
print("Plan guide trajectory ...")
from . import hyq_slalom_debris_path as tp
print("Done.")
import time
from scenarios.demos.hyq_slalom_debris_path import PathPlanner
from scenarios.hyq_contact_generator import HyqContactGenerator
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
class ContactGenerator(HyqContactGenerator):
# Set the bounds for the root
root_bounds = tp.root_bounds
root_bounds[4] -= 0.05
root_bounds[5] += 0.05
fullBody.setJointBounds ("root_joint", root_bounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
def __init__(self):
super().__init__(PathPlanner())
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
def load_limbs(self):
super().load_limbs("fixedStep06", "ReferenceConfiguration")
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
for limbId in fullBody.limbNames :
fullBody.runLimbSampleAnalysis(limbId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; 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 = [0,0,0]
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] = [0,0,0]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
fullBody.setCurrentConfig (q_goal)
v.addLandmark('hyq/base_link',0.3)
q_init[2] = fullBody.referenceConfig[2]
q_goal[2] = fullBody.referenceConfig[2]
v(q_init)
#q_init = fullBody.generateContacts(q_init, [0,0,1])
#q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.rArmId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.rArmId,fullBody.lLegId])
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability = False)
tInterpolateConfigs = time.time() - tStart
print("Done.")
print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
def compute_configs_from_guide(self):
super().compute_configs_from_guide()
self.q_init[2] = self.q_ref[2]
self.q_goal[2] = self.q_ref[2]
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
script/scenarios/demos/hyq_slalom_debris_path.py
View file @ 44207e1a
from hpp.corbaserver.rbprm.hyq_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import time
from scenarios.hyq_path_planner import HyqPathPlanner
class PathPlanner(HyqPathPlanner):
def init_problem(self):
super().init_problem()
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
self.ps.setParameter("Kinodynamic/forceYawOrientation",True)
vMax = 0.2# linear velocity bound for the root
aMax = 0.1# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
root_bounds = [-1.7,2.5, -0.2, 2, 0.63, 0.63]
rbprmBuilder.setJointBounds ("root_joint",root_bounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
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([-4,4,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
def run(self):
self.v_max = 0.2
self.a_max = 0.1
self.root_translation_bounds = [-1.7,2.5, -0.2, 2, 0.64, 0.64]
self.init_problem()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",0.5)
ps.setParameter("Kinodynamic/forceYawOrientation",True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",50)
self.q_init[0:2] = [-1.5, 0]
self.q_init[-6] = 0.05
self.q_goal[0:2] = [2.2, 0]
self.q_goal[-6] = 0.05
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
self.init_viewer("multicontact/slalom_debris")
self.init_planner()
self.solve()
self.ps.optimizePath(1)
self.display_path()