])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7, 1.7, -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()
# 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.2)
ps.setParameter("DynamicPlanner/sizeFootY", 0.12)
ps.setParameter("DynamicPlanner/friction", 0.5)
ps.setParameter("Kinodynamic/forceYawOrientation", True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
rbprmBuilder.setFilter(Robot.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.01,0.01,-0.01,0.01,-0.01,0.01])
# 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,-2.,2.,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# 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("Kinodynamic/verticalAccelerationBound",aMaxZ)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
#ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7, 1.7, -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()
# 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", mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
])
rbprmBuilder.setAffordanceFilter('talos_rleg_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()
# 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)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceYawOrientation", True)
ps.setParameter("DynamicPlanner/sizeFootX", 0.2)
ps.setParameter("DynamicPlanner/sizeFootY", 0.12)
ps.setParameter("DynamicPlanner/friction", mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
# load the module to analyse the environnement and compute the possible contact surfaces
joint1R_bounds_prev = fullBody.getJointBounds('leg_right_1_joint')
joint3R_bounds_prev = fullBody.getJointBounds('leg_right_3_joint')
fullBody.setJointBounds('leg_left_1_joint', [-0.2, 0.7])
fullBody.setJointBounds('leg_left_3_joint', [-1.3, 0.4])
fullBody.setJointBounds('leg_right_1_joint', [-0.7, 0.2])
fullBody.setJointBounds('leg_right_3_joint', [-1.3, 0.4])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(6)
vMax = 0.5 # linear velocity bound for the root
aMax = 0.5 # linear acceleration bound for the root
fullBody.client.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0])
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setRandomSeed(random.SystemRandom().randint(0, 999999))
#load the viewer
try:
v = vf.createViewer(displayCoM=True)
except Exception:
print("No viewer started !")
class FakeViewer():
sceneName = ""
def __init__(self):
return
class AbstractPathPlanner:
rbprmBuilder = None
ps = None
v = None
afftool = None
pp = None
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
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
self.q_init = []
self.q_goal = []
@abstractmethod
def load_rbprm(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
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):
"""
Compute extra dof bounds from the current values of v_max and a_max
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# 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
"""
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_rbprm()
self.set_configurations()
self.compute_extra_config_bounds()
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",
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("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:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
def init_planner(self, kinodynamic=True, optimize=True):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
self.ps.selectConfigurationShooter("RbprmShooter")
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
if kinodynamic:
self.ps.selectSteeringMethod("RBPRMKinodynamic")
self.ps.selectDistance("Kinodynamic")
self.ps.selectPathPlanner("DynamicPlanner")
if optimize:
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
else:
self.ps.addPathOptimizer("RandomShortcut")
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 self.pp is not None:
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)
"""
self.show_rom()
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths() - 1
self.pp.dt = dt
self.pp(path_id)
def hide_rom(self):
"""
Remove the current robot from the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
def show_rom(self):
"""
Add the current robot to the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "ON")
@abstractmethod
def run(self):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.init_problem()
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
q_init = q1;
# q_goal = q2
q_goal = q3
from hpp.gepetto import ViewerFactory, PathPlayerGui
vf = ViewerFactory (ps)
# vf.loadObstacleModel ("ur_description","obstacles","obstacles")
vf.loadObstacleModel ("ur_description","table","table")
# vf.loadObstacleModel ("ur_description","wall","wall")
vf(q1)
ps.lockJoint("elbow_joint", [q1[2]])
ps.selectPathValidation("Progressive", .05)
ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3))
ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True))
ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
print "Optimizer parameters are:"
print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
print "alwaysStopAtFirst:", ps.getParameter("SplineGradientBased/alwaysStopAtFirst").value()
print "linearizeAtEachStep:", ps.getParameter("SplineGradientBased/linearizeAtEachStep").value()
print ""
import datetime as dt
totalSolveTime = dt.timedelta (0)
totalOptimTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
robot.setJointBounds("root_joint", [-7, 6.5, -7, 7, 0.4, 0.4])
# Kinodynamic methods need at least 6 extraConfigs, to store the velocity (first 3) and
# acceleration (last 3) of the translation of the root
robot.client.robot.setDimensionExtraConfigSpace(6)
# set the bounds for velocity and acceleration :
aMax = 1.0
vMax = 2.0
robot.client.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0])
ps = ProblemSolver(robot)
# define the velocity and acceleration bounds used by the steering method. This bounds
# will be stastified along the whole trajectory.
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# Uncomment the following line if you want to constraint the orientation of the base of
# the robot to follow the direction of the motion. Note that in this case the initial
# and final orientation are not considered.
# ps.setParameter("Kinodynamic/forceOrientation",True)
# The following line constraint the random sampling method to fix all the extraDOF at 0
# during sampling. Comment it if you want to sample states with non-null velocity and
# acceleration. Note that it increase the complexity of the problem and greatly increase
# the computation time.
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
vf = ViewerFactory(ps)
class AbstractContactGenerator:
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
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self, path_planner):
"""
Constructor, run the guide path and save the results
:param path_planner: an instance of a child class of AbstractPathPlanner
"""
path_planner.run()
self.path_planner = path_planner
self.path_planner.hide_rom()
# ID of the guide path used in problemSolver, default to the last one
self.pid = self.path_planner.ps.numberPaths() - 1
# Save the guide planning problem in a specific instance,
# such that we can use it again even after creating the "fullbody" problem:
self.cl = Client()
self.cl.problem.selectProblem("guide_planning")
path_planner.load_rbprm()
ProblemSolver(path_planner.rbprmBuilder)
self.cl.problem.selectProblem("default")
self.cl.problem.movePathToProblem(
self.pid, "guide_planning",
self.path_planner.rbprmBuilder.getAllJointNames()[1:])
# copy bounds and limbs used from path planning :
self.used_limbs = path_planner.used_limbs
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):
"""
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="ReferenceConfiguration",
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
if octree_size is None:
octree_size = self.fullbody.octreeSize
print("Generate limb DB ...")
t_start = time.time()
self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples,
octree_size)
t_generate = time.time() - t_start
print("Databases generated in : " + str(t_generate) + " s")
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)
if self.path_planner.a_max >= 0:
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,
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[: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
self.q_goal = self.q_init[::]
self.q_goal[:7] = self.path_planner.ps.configAtParam(
self.pid, self.path_planner.ps.pathLength(self.pid))[0:7]
# copy extraconfig for start and init configurations
configSize = self.fullbody.getConfigSize(
) - self.fullbody.client.robot.getDimensionExtraConfigSpace()
q_init_guide = self.path_planner.ps.configAtParam(self.pid, 0)
q_goal_guide = self.path_planner.ps.configAtParam(
self.pid, self.path_planner.ps.pathLength(self.pid))
index_ecs = len(q_init_guide) - self.path_planner.extra_dof
self.q_init[configSize:configSize +
3] = q_init_guide[index_ecs:index_ecs + 3]
if use_acc_init:
self.q_init[configSize + 3:configSize +
6] = q_init_guide[index_ecs + 3:index_ecs + 6]
else:
self.q_init[configSize + 3:configSize + 6] = [0, 0, 0]
self.q_goal[configSize:configSize +
3] = q_goal_guide[index_ecs:index_ecs + 3]
if use_acc_end:
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 set_start_end_states(self):
"""
Set the current q_init and q_goal as initial/goal state for the contact planning
"""
self.fullbody.setStartState(self.q_init, self.init_contacts)
self.fullbody.setEndState(self.q_goal, self.end_contacts)
def interpolate(self):
"""
Compute the sequence of configuration in contact between q_init and q_goal
"""
self.set_start_end_states()
self.fullbody.setStaticStability(self.static_stability)
self.v(self.q_init)
print("Generate contact plan ...")
t_start = time.time()
self.configs = self.fullbody.interpolate(
self.dt,
pathId=self.pid,
robustnessTreshold=self.robustness,
filterStates=self.filter_states,
testReachability=self.test_reachability,
quasiStatic=self.quasi_static,
erasePreviousStates=self.erase_previous_states)
t_interpolate_configs = time.time() - t_start
print("Contact plan generated in : " + str(t_interpolate_configs) +
" s")
print("number of configs :", len(self.configs))
# Helper methods used to display results
def display_sequence(self, dt=0.5):
"""
Display the sequence of configuration in contact,
requires that self.configs is not empty
:param dt: the pause (in second) between each configuration
"""
displayContactSequence(self.v, self.configs, dt)
def display_init_config(self):
self.v(self.q_init)
def display_end_config(self):
self.v(self.q_goal)
def play_guide_path(self):
"""
display the guide path planned
"""
self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
self.path_planner.show_rom()
self.cl.problem.selectProblem("guide_planning")
# this is not the same problem as in the guide_planner. We only added the final path in this problem,
# thus there is only 1 path in this problem
self.path_planner.pp(0)
self.cl.problem.selectProblem("default")
self.path_planner.hide_rom()
self.v.client.gui.setVisibility(self.robot_node_name, "ON")
def run(self):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
self.load_fullbody()
self.set_joints_bounds()
self.set_reference()
self.load_limbs()
self.init_problem()
self.init_viewer()
self.compute_configs_from_guide()
self.interpolate()
robot = RobotRod("rod")
robot.setJointBounds ("root_joint", [-7, 6.5, -7, 7,0.4,0.4])
# Kinodynamic methods need at least 6 extraConfigs, to store the velocity (first 3) and acceleration (last 3) of the translation of the root
robot.client.robot.setDimensionExtraConfigSpace(6)
# set the bounds for velocity and acceleration :
aMax=1.
vMax=2.
robot.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
# define the velocity and acceleration bounds used by the steering method. This bounds will be stastified along the whole trajectory.
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# Uncomment the following line if you want to constraint the orientation of the base of the robot to follow the direction of the motion. Note that in this case the initial and final orientation are not considered.
#ps.setParameter("Kinodynamic/forceOrientation",True)
# The following line constraint the random sampling method to fix all the extraDOF at 0 during sampling. Comment it if you want to sample states with non-null velocity and acceleration. Note that it increase the complexity of the problem and greatly increase the computation time.
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [6.5,-4,0.4] #root position
