Skip to content

GitLab

Commit 2d0a5781 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Script] update abstract template scripts

parent 74bc5725
Hide whitespace changes
Inline Side-by-side
script/scenarios/abstract_contact_generator.py
View file @ 2d0a5781
......@@ -6,13 +6,13 @@ from abc import abstractmethod
class AbstractContactGenerator:
fullbody = None
ps = None
v = None
q_ref = None
weight_postural = None
q_init = None
q_goal = None
fullbody = None # rborpm.FullBody instance
ps = None # ProblemSolver instance
v = None # gepetto.Viewer instance
q_ref = None # reference configuration used (depending on the settings)
weight_postural = None # weight used for the postural task (depending on the setting)
q_init = None # Initial whole body configuration
q_goal = None # Desired final whole body configuration
def __init__(self, path_planner):
"""
......@@ -21,34 +21,65 @@ class AbstractContactGenerator:
"""
path_planner.run()
self.path_planner = path_planner
# ID of the guide path used in problemSolver, default to the last one
self.pid = self.path_planner.ps.numberPaths() - 1
self.used_limbs = path_planner.used_limbs
self.dt = 0.01
self.robustness = 0
self.filter_states = True
self.test_reachability = True
self.quasi_static = True
self.erase_previous_states = True
self.init_contacts = self.used_limbs
self.end_contacts = self.used_limbs
self.configs = []
self.root_translation_bounds = self.path_planner.root_translation_bounds
# increase bounds from path planning, to leave room for the root motion during the steps
for i in range(3):
self.root_translation_bounds[2*i] -= 0.1
self.root_translation_bounds[2*i + 1] += 0.1
# settings related to the 'interpolate' method:
self.dt = 0.01 # discretisation step used
self.robustness = 0 # robustness treshold
# (see https://github.com/humanoid-path-planner/hpp-centroidal-dynamics/blob/master/include/hpp/centroidal-dynamics/centroidal_dynamics.hh#L215)
self.filter_states = True # if True, after contact generation try to remove all the redundant steps
self.test_reachability = True # if True, check feasibility of the contact transition during contact generation
self.quasi_static = True # if True, use the 2-PAC method to check feasibility, if False use CROC
self.erase_previous_states = True # if False, keep^the previously computed states if 'interpolate' is called a second time
self.static_stability = True # if True, the acceleration computed by the guide is ignored during contact planning
self.init_contacts = self.used_limbs # limbs in contact in the initial configuration
# the order of the limbs in the list define the initial order in which the contacts are removed when then cannot be maintained anymore
self.end_contacts = self.used_limbs # limbs in contact in the final configuration
self.configs = [] # will contains the whole body configuration computed after calling 'interpolate'
@abstractmethod
def load_fullbody(self):
"""
Load the robot from urdf and instanciate a fullBody object
Also initialize the q_ref and weight_postural vectors
"""
pass
def set_joints_bounds(self):
self.fullbody.setJointBounds ("root_joint", self.path_planner.root_translation_bounds)
"""
Define the root translation bounds and the extra config bounds
"""
self.fullbody.setJointBounds ("root_joint", self.root_translation_bounds)
self.fullbody.client.robot.setDimensionExtraConfigSpace(self.path_planner.extra_dof)
self.fullbody.client.robot.setExtraConfigSpaceBounds(self.path_planner.extra_dof_bounds)
def set_reference(self, use_postural_task = True):
"""
Set the reference configuration used and the weight for the postural task
:param use_postural_task: if True, when projecting configurations to contact a postural task is added to the cost function
Disabling this setting will greatly reduce the computation time, but may result in weird configurations in contact
"""
self.fullbody.setReferenceConfig(self.q_ref)
self.fullbody.setCurrentConfig(self.q_ref)
self.fullbody.setPostureWeights(self.weight_postural)
self.fullbody.usePosturalTaskContactCreation(use_postural_task)
def load_limbs(self, heuristic = "fixedStep06", analysis=None, nb_samples=None, octree_size=None):
"""
Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used,
see https://github.com/humanoid-path-planner/hpp-rbprm/blob/master/src/sampling/heuristic.cc#L272-L285
:param analysis: The name of the analysis used,
see https://github.com/humanoid-path-planner/hpp-rbprm/blob/master/src/sampling/analysis.cc#L318-L335
: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.fullbody.limbs_names = self.used_limbs
if nb_samples is None:
nb_samples = self.fullbody.nbSamples
......@@ -62,6 +93,9 @@ class AbstractContactGenerator:
def init_problem(self):
"""
Create a ProblemSolver instance, and set the velocity and acceleration bounds
"""
self.ps = ProblemSolver(self.fullbody)
if self.path_planner.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.path_planner.v_max)
......@@ -69,9 +103,18 @@ class AbstractContactGenerator:
self.ps.setParameter("Kinodynamic/accelerationBound", self.path_planner.a_max)
def init_viewer(self):
"""
Create a Viewer instance
"""
self.v = Viewer(self.ps, viewerClient=self.path_planner.v.client, displayCoM=True)
def compute_configs_from_guide(self, use_acc_init = True, use_acc_end = False):
def compute_configs_from_guide(self, use_acc_init = True, use_acc_end = False, set_ref_height = True):
"""
Compute the wholebody configurations from the reference one and the guide init/goal config
:param use_acc_init: if True, use the initial acceleration from the guide path
:param use_acc_end: if True, use the final acceleration from the guide path
:param set_ref_height: if True, set the root Z position of q_init and q_goal to be equal to q_ref
"""
self.q_init = self.q_ref[::]
self.q_init[0:7] = self.path_planner.ps.configAtParam(self.pid, 0.001)[0:7]
# do not use 0 but an epsilon in order to avoid the orientation discontinuity that may happen at t=0
......@@ -93,12 +136,22 @@ class AbstractContactGenerator:
self.q_goal[configSize + 3 : configSize + 6] = q_goal_guide[index_ecs + 3 : index_ecs + 6]
else:
self.q_goal[configSize + 3 : configSize + 6] = [0, 0, 0]
if set_ref_height:
# force root height to be at the reference position:
self.q_init[2] = self.q_ref[2]
self.q_goal[2] = self.q_ref[2]
self.v(self.q_init)
def interpolate(self):
"""
Compute the sequence of configuration in contact between q_init and q_goal
"""
# specify the full body configurations as start and goal state of the problem
self.fullbody.setStaticStability(self.static_stability)
self.fullbody.setStartState(self.q_init, self.init_contacts)
self.fullbody.setEndState(self.q_goal, self.end_contacts)
self.v(self.q_init)
print("Generate contact plan ...")
t_start = time.time()
self.configs = self.fullbody.interpolate(0.01,
......@@ -113,6 +166,10 @@ class AbstractContactGenerator:
print("number of configs :", len(self.configs))
def display_sequence(self):
"""
Display the sequence of configuration in contact,
requires that self.configs is not empty
"""
displayContactSequence(self.v, self.configs)
@abstractmethod
......@@ -129,9 +186,6 @@ class AbstractContactGenerator:
self.init_problem()
self.init_viewer()
self.compute_configs_from_guide()
# force root height to be at the reference position:
self.q_init[2] = self.q_ref[2]
self.q_goal[2] = self.q_ref[2]
self.interpolate()
"""
pass
script/scenarios/abstract_path_planner.py
View file @ 2d0a5781
......@@ -11,6 +11,7 @@ class AbstractPathPlanner:
v = None
afftool = None
pp = None
extra_dof_bounds = None
def __init__(self):
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
......@@ -23,13 +24,9 @@ class AbstractPathPlanner:
self.used_limbs = [] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
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.q_init = []
self.q_goal = []
@abstractmethod
def load_rbprm(self):
"""
......@@ -37,6 +34,18 @@ class AbstractPathPlanner:
"""
pass
def set_configurations(self):
self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(self.extra_dof)
self.q_init = self.rbprmBuilder.getCurrentConfig()
self.q_goal = self.rbprmBuilder.getCurrentConfig()
self.q_init[2] = self.rbprmBuilder.ref_height
self.q_goal[2] = self.rbprmBuilder.ref_height
def compute_extra_config_bounds(self):
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
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]
def set_joints_bounds(self):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
......@@ -61,6 +70,8 @@ class AbstractPathPlanner:
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self.load_rbprm()
self.set_configurations()
self.compute_extra_config_bounds()
self.set_joints_bounds()
self.set_rom_filters()
self.ps = ProblemSolver( self.rbprmBuilder)
......@@ -77,17 +88,19 @@ 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", visualize_affordances = []):
def init_viewer(self, env_name, env_package = "hpp_environments",reduce_sizes = [0, 0, 0], visualize_affordances = []):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
: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
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool ()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel (env_package, env_name, "planning", vf)
self.afftool.loadObstacleModel (env_package, env_name, "planning", vf, reduceSizes = reduce_sizes)
try :
self.v = vf.createViewer(displayArrows = True)
except Exception:
......
script/scenarios/template_path.py deleted 100644 → 0
View file @ 74bc5725
from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver
import time
class AbstractPathPlanning:
rbprmBuilder = None
ps = None
v = None
afftool = None
pp = None
q_init = []
q_goal = []
def __init__(self):
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self.root_rotation_bounds = [-3.14, 3.14, -0.01, 0.01, -0.01, 0.01] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self.mu = 0.5 # friction coefficient between the robot and the environment
self.used_limbs = [] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
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]
@abstractmethod
def load_robot(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def set_joints_bounds(self):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
"""
self.rbprmBuilder.setJointBounds ("root_joint", self.root_translation_bounds)
self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(self.extra_dof_bounds)
def set_rom_filters(self):
"""
Define which ROM must be in collision at all time and with which kind of affordances
By default it set all the roms in used_limbs to be in contact with 'support' affordances
"""
self.rbprmBuilder.setFilter(self.used_limbs)
for limb in self.used_limbs:
self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def init_problem(self):
"""
Load the robot, set the bounds and the ROM filters and then
Create a ProblemSolver instance and set the default parameters.
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self.load_robot()
self.set_joints_bounds()
self.set_rom_filters()
self.ps = ProblemSolver( self.rbprmBuilder)
# define parameters used by various methods :
if self.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
if self.a_max >= 0:
self.ps.setParameter( "Kinodynamic/accelerationBound", self.a_max)
if self.size_foot_x > 0:
self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
if self.size_foot_y > 0:
self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
self.ps.setParameter("DynamicPlanner/friction", 0.5)
# sample only configuration with null velocity and acceleration :
self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
def init_viewer(self, env_name, env_package = "hpp_environments", visualize_affordances = []):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool ()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel (env_package, env_name, "planning", vf)
try :
self.v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
self.v = FakeViewer()
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
def init_planner(self, kinodynamic = True):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
"""
self.ps.selectConfigurationShooter("RbprmShooter")
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
self.ps.selectSteeringMethod("RBPRMKinodynamic")
self.ps.selectDistance("Kinodynamic")
self.ps.selectPathPlanner("DynamicPlanner")
def solve(self):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
if len(self.q_init) != self.rbprmBuilder.getConfigSize():
raise ValueError("Initial configuration vector do not have the right size")
if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
raise ValueError("Goal configuration vector do not have the right size")
self.ps.setInitialConfig (self.q_init)
self.ps.addGoalConfig (self.q_goal)
self.v(self.q_init)
t = self.ps.solve ()
print("Guide planning time : ",t)
def display_path(self, path_id = -1, dt = 0.1):
"""
Display the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.1)
"""
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp.displayVelocityPath(path_id)
def play_path(self, path_id = -1, dt = 0.01):
"""
play the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.01)
"""
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp(path_id)
def hide_rom(self):
"""
Move visual ROM far above the meshs, as we cannot just delete it.
"""
q_far = self.q_init[::]
q_far[2] = -3
self.v(q_far)
\ No newline at end of file
script/scenarios/template_talos_path.py deleted 100644 → 0
View file @ 74bc5725
from .template_path import AbstractPathPlanning
class TalosPathPlanning(AbstractPathPlanning):
def __init__(self):
super().__init__()
# 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.95, 1.05]
# set default used limbs to be both feet
self.used_limbs = ['talos_lleg_rom','talos_rleg_rom']
self.size_foot_x = 0.2 # size of the feet along the x axis
self.size_foot_y = 0.12 # size of the feet along the y axis
self.v_max = 0.3
self.a_max = 0.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]
def load_robot(self):
from talos_rbprm.talos_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(self.extra_dof)
self.q_init = self.rbprmBuilder.getCurrentConfig()
self.q_goal = self.rbprmBuilder.getCurrentConfig()
self.q_init[2] = self.rbprmBuilder.ref_height
self.q_goal[2] = self.rbprmBuilder.ref_height
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