Commit 156d8225 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scenario] add scenario class for Solo and add demos scripts

parent 993c64c2
......@@ -119,6 +119,8 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS
__init__.py
talos_contact_generator.py
talos_path_planner.py
solo_contact_generator.py
solo_path_planner.py
)
SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
......@@ -143,6 +145,12 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_plateformes.py
talos_stairs10_path.py
talos_stairs10.py
talos_stairs15_path.py
talos_stairs15.py
solo_flatGround_path.py
solo_flatGround.py
solo_slalom_path.py
solo_pallet_path.py
)
SET(${PROJECT_NAME}_PYTHON_SCENARIOS_MEMMO
......
from hpp.corbaserver.rbprm.scenarios.demos.solo_flatGround_path import PathPlanner
from hpp.corbaserver.rbprm.scenarios.solo_contact_generator import SoloContactGenerator
class ContactGenerator(SoloContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.testReachability=False
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
class PathPlanner(SoloPathPlanner):
def init_problem(self):
self.a_max = 0.1
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self):
self.init_problem()
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [0.5, 0]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner(True, False)
self.solve()
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
class PathPlanner(SoloPathPlanner):
def init_problem(self):
self.a_max = 0.1
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self):
self.init_problem()
self.q_init[:2] = [-0.85, 0.]
# self.q_goal[:2] = [1, 0.]
self.q_goal[:2] = [0., 0.]
self.init_viewer("ori/modular_palet_low", visualize_affordances=["Support"], reduce_sizes=[0.06, 0, 0],
min_area = [['Support', 0.05]])
self.init_planner(True, False)
self.ps.directPath(self.q_init, self.q_goal, False)
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
class PathPlanner(SoloPathPlanner):
def init_problem(self):
self.a_max = 0.1
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self):
self.init_problem()
self.q_init[:2] = [-0.7, 0.95]
#self.q_goal[:2] = [1.05, 0.95]
self.q_goal[:2] = [-0.2, 0.95]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self.init_viewer("multicontact/slalom_debris", visualize_affordances=["Support"], reduce_sizes=[0.05, 0, 0])
self.init_planner(True, False)
self.solve()
"""
ps = self.ps
q_init = self.rbprmBuilder.getCurrentConfig();
q_init[0:3] = [-1.8, 0., self.rbprmBuilder.ref_height];
self.v(q_init)
q_init[-6:-3] = [0.1, 0, 0]
q_goal = q_init[::]
q_goal[0:3] = [-0.9, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
q_init_0 = q_init[::]
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_begin = ps.numberPaths() - 1
### END BEGIN up to the rubbles #####
ps.resetGoalConfigs()
### BEGIN rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.4)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
# q_init[-6:-3] = [0.,0,0]
q_goal[0:3] = [1.05, 0.95, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.1, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
for i in range(10):
ps.optimizePath(ps.numberPaths() -1)
pId_rubbles = ps.numberPaths() - 1
### END rubbles #####
ps.resetGoalConfigs()
### BEGIN after rubbles #####
ps.setParameter("Kinodynamic/velocityBound", 0.15)
ps.setParameter("Kinodynamic/accelerationBound", 0.1)
q_init = self.rbprmBuilder.getCurrentConfig();
q_init = q_goal[::];
self.v(q_init)
q_goal[0:3] = [2.2, 0, self.rbprmBuilder.ref_height];
self.v(q_goal)
q_goal[-6:-3] = [0.05, 0, 0]
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
self.v(q_goal)
t = ps.solve()
print("done planning, optimize path ...")
# v.solveAndDisplay('rm',2,0.005)
# for i in range(5):
# ps.optimizePath(ps.numberPaths() -1)
pId_end = ps.numberPaths() - 1
### END after rubbles #####
self.pathId = pId_begin
ps.concatenatePath(self.pathId, pId_rubbles)
ps.concatenatePath(self.pathId, pId_end)
"""
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
from hpp.corbaserver.rbprm.scenarios.demos.talos_stairs15_path import PathPlanner
from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.test_reachability = False
def load_fullbody(self):
super().load_fullbody()
self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
# path planning use different limbs, reset it to right/left feet
self.used_limbs = [self.fullbody.lLegId, self.fullbody.rLegId, self.fullbody.rArmId]
self.init_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
self.end_contacts = self.used_limbs
def load_limbs(self):
super().load_limbs("static", nb_samples=100000)
def compute_configs_from_guide(self):
super().compute_configs_from_guide()
self.q_goal[2] = self.q_ref[2] + 0.75 # set height to the top of the stairs
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId, Robot.rArmId]
self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot()
def init_problem(self):
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
self.ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
self.ps.setParameter("Kinodynamic/verticalAccelerationBound", 3.)
def compute_extra_config_bounds(self):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
self.extra_dof_bounds = [
-self.v_max, self.v_max, -self.v_max, self.v_max, -10, 10, -self.a_max, self.a_max, -self.a_max,
self.a_max, -10, 10
]
def run(self):
self.root_translation_bounds = [-2., -1.7, 0, 2., 0.95, 2.]
self.init_problem()
self.q_init[:2] = [-1.9, 1.9]
self.q_init[3:7] = [0, 0, -0.7071, 0.7071]
self.q_init[2] = 0.98
self.q_goal = self.q_init[::]
self.q_goal[1] = 0.9
self.q_goal[2] += 3 * 0.15 # 4 steps of 15cm each
self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.08, 0., 0.], visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
from .abstract_contact_generator import AbstractContactGenerator
class SoloContactGenerator(AbstractContactGenerator):
def __init__(self, path_planner):
super().__init__(path_planner)
self.robustness = 5
self.used_limbs = ['FRleg', 'HLleg', 'FLleg', 'HRleg']
self.init_contacts = self.used_limbs[::]
self.end_contacts = self.used_limbs[::]
self.robot_node_name = "solo"
def set_start_end_states(self):
# for 3D contacts, we must specify the normals manually
normals_init = []
normals_end = []
for limb_id in self.init_contacts:
normals_init += [self.fullbody.dict_normal[self.fullbody.dict_limb_joint[limb_id]]]
for limb_id in self.init_contacts:
normals_end += [self.fullbody.dict_normal[self.fullbody.dict_limb_joint[limb_id]]]
self.fullbody.setStartState(self.q_init, self.init_contacts, normals_init)
self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
def load_fullbody(self):
from solo_rbprm.solo import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
self.fullbody.setConstrainedJointsBounds()
def init_viewer(self):
super().init_viewer()
self.v.addLandmark('solo/base_link_0', 0.3)
def load_limbs(self, heuristic="fixedStep04", analysis=None, nb_samples=None, octree_size=None):
super().load_limbs(heuristic, analysis, nb_samples, octree_size, disableEffectorCollision=True)
from .abstract_path_planner import AbstractPathPlanner
class SoloPathPlanner(AbstractPathPlanner):
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.241, 0.241]
# set default used limbs to be both feet
self.used_limbs = ['solo_RFleg_rom','solo_LHleg_rom','solo_LFleg_rom','solo_RHleg_rom']
self.size_foot_x = 0.01 # size of the feet along the x axis
self.size_foot_y = 0.01 # size of the feet along the y axis
self.v_max = 0.3
self.a_max = 1.
self.extra_dof_bounds = [
-self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0, -self.a_max, self.a_max, -self.a_max, self.a_max,
0, 0
]
self.robot_node_name = "solo_trunk"
def load_rbprm(self):
from solo_rbprm.solo_abstract import Robot
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
def set_configurations(self):
super().set_configurations()
Markdown is supported
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