Unverified Commit ac5acb3d authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #79 from pFernbach/solo

Solo
parents fda5fce8 156d8225
......@@ -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
......
......@@ -83,7 +83,16 @@ class AbstractContactGenerator:
self.fullbody.setPostureWeights(self.weight_postural)
self.fullbody.usePosturalTaskContactCreation(use_postural_task)
def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None):
def set_rom_filters(self):
"""
Define which type of affordance should be considered to create contact with each limb
By default, use "Support" type for all limbs
"""
for limb in self.used_limbs:
self.path_planner.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None,
disableEffectorCollision=False):
"""
Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used,
......@@ -93,6 +102,7 @@ class AbstractContactGenerator:
:param nb_samples: The number of samples for each limb database. Default is set in the Robot python class
:param octree_size: The size of each cell of the octree. Default is set in the Robot python class
"""
self.set_rom_filters()
self.fullbody.limbs_names = self.used_limbs
if nb_samples is None:
nb_samples = self.fullbody.nbSamples
......@@ -100,7 +110,8 @@ class AbstractContactGenerator:
octree_size = self.fullbody.octreeSize
print("Generate limb DB ...")
t_start = time.time()
self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples, octree_size)
self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples, octree_size,
disableEffectorCollision=disableEffectorCollision)
t_generate = time.time() - t_start
print("Databases generated in : " + str(t_generate) + " s")
......
......@@ -113,7 +113,8 @@ class AbstractPathPlanner:
# sample only configuration with null velocity and acceleration :
self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[]):
def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[],
min_area = None):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
......@@ -121,6 +122,7 @@ class AbstractPathPlanner:
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
:param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance
"""
vf = ViewerFactory(self.ps)
if self.context:
......@@ -130,11 +132,13 @@ class AbstractPathPlanner:
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
if min_area is not None:
for (aff_type, min_size) in min_area:
self.afftool.setMinimumArea(aff_type, min_size)
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
......
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()
......@@ -62,18 +62,33 @@ def getRotationMatrixFromConfigs(configs):
#print "R in getRotationMatrixFromConfigs : ",R
return R
def getALlContactsNames(rbprmBuilder, q):
step_contacts = []
for limb in rbprmBuilder.urdfNameRom:
step_contacts += rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, limb)
return step_contacts
# get contacted surface names at configuration
def getContactsNames(rbprmBuilder, i, q):
def getContactsNames(rbprmBuilder, i, q, use_all_limbs=False):
if use_all_limbs:
return getALlContactsNames(rbprmBuilder, q)
if i % 2 == LF: # left leg
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
elif i % 2 == RF: # right leg
step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
return step_contacts
def getALlContactsIntersections(rbprmBuilder, q):
intersections = []
for limb in rbprmBuilder.urdfNameRom:
intersections += rbprmBuilder.getContactSurfacesAtConfig(q, limb)
return intersections
# get intersections with the rom and surface at configuration
def getContactsIntersections(rbprmBuilder, i, q):
def getContactsIntersections(rbprmBuilder, i, q, use_all_limbs=False):
if use_all_limbs:
return getALlContactsIntersections(rbprmBuilder, q)
if i % 2 == LF: # left leg
intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
elif i % 2 == RF: # right leg
......@@ -118,7 +133,8 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
useIntersection=False,
mergeCandidates=False,
max_yaw=0.5,
max_surface_area=MAX_SURFACE):
max_surface_area=MAX_SURFACE,
use_all_limbs=False):
pathLength = ps.pathLength(pId) #length of the path
discretizationStep = 0.01 # step at which we check the colliding surfaces
#print "path length = ",pathLength
......@@ -152,24 +168,42 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
t += 0.0001
q = ps.configAtParam(pId, t)
#print " t in getSurfacesFromGuideContinuous : ",t
step_contacts = getContactsNames(rbprmBuilder, i, q)
step_contacts = getContactsNames(rbprmBuilder, i, q, use_all_limbs)
for contact_name in step_contacts:
if not contact_name in phase_contacts_names:
phase_contacts_names.append(contact_name)
# end current phase
# get all the surfaces from the names and add it to seqs:
if useIntersection:
intersections = getContactsIntersections(rbprmBuilder, i, q)
phase_surfaces = []
if useIntersection:
intersections = getContactsIntersections(rbprmBuilder, i, q, use_all_limbs)
# First add intersection for the surfaces in contact at the last discretization step
for k, name in enumerate(step_contacts):
surface = surfaces_dict[name][0] # [0] because the last vector contain the normal of the surface
if area(surface) > max_surface_area:
intersection = intersections[k]
if len(intersection) > 3:
phase_surfaces.append(intersection)
else:
phase_surfaces.append(surfaces_dict[name][0])
# then add possible small surfaces encountered during the discretization of the path
for name in phase_contacts_names:
if name not in step_contacts:
phase_surfaces.append(surfaces_dict[name][0])
else:
# add all the surfaces encountered during the path
for name in phase_contacts_names:
phase_surfaces.append(surfaces_dict[name][0])
for name in phase_contacts_names:
surface = surfaces_dict[name][0]
surface = surfaces_dict[name][0] # [0] because the last vector contain the normal of the surface
if useIntersection and area(surface) > max_surface_area:
if name in step_contacts:
intersection = intersections[step_contacts.index(name)]
if len(intersection) > 3:
phase_surfaces.append(intersection)
else:
phase_surfaces.append(surface) # [0] because the last vector contain the normal of the surface
phase_surfaces.append(surface)
#print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail
phase_surfaces_array = [] # convert from list to array, we cannot do this before because sorted() require list
......
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