Commit d2aa6f1b authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Format] run yapf in src/hpp/corbaserver/rbprm

parent 1080fdea
......@@ -6,32 +6,35 @@ from importlib import import_module
import sys
import os
def kill_process(proc):
proc.kill()
# init argument parser
parser = argparse.ArgumentParser(description="This script must be called with one argumemnt : "
"the name of the script describing the scenario."
"If a relative path is given, "
"hpp.corbaserver.rbprm.scenarios is prepended")
parser.add_argument('scenario_name', type=str,
help="The name of the scenario script to run. "
"If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended")
"the name of the script describing the scenario."
"If a relative path is given, "
"hpp.corbaserver.rbprm.scenarios is prepended")
parser.add_argument('scenario_name',
type=str,
help="The name of the scenario script to run. "
"If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended")
args = parser.parse_args()
# retrieve argument
scenario_name = args.scenario_name
scenario_name = scenario_name.rstrip(".py") # remove extension if specified
scenario_name.replace("/",".") # if the path to the file is given, transform it to a python path
scenario_name.replace("/", ".") # if the path to the file is given, transform it to a python path
# try to import the given module
try:
module_scenario = import_module(scenario_name)
except ImportError:
print("Cannot import "+scenario_name+" try to prepend path")
print("Cannot import " + scenario_name + " try to prepend path")
scenario_name = "hpp.corbaserver.rbprm.scenarios." + scenario_name
try:
module_scenario = import_module(scenario_name)
except ImportError:
print("Cannot import "+scenario_name+". Check if the path is correct")
print("Cannot import " + scenario_name + ". Check if the path is correct")
sys.exit(1)
# kill already existing instance of the viewer/server
......@@ -40,8 +43,14 @@ subprocess.run(["killall", "hpp-rbprm-server"])
# run the viewer/server in background
# stdout and sterr outputs of the child process are redirected to denull (hidden).
# preexec_fn is used to ignore ctrl-c signal send to the main script (otherwise they are forwarded to the child process)
process_viewer = subprocess.Popen("gepetto-gui", stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, preexec_fn=os.setpgrp)
process_server = subprocess.Popen("hpp-rbprm-server", stdout=subprocess.PIPE, stderr=subprocess.DEVNULL, preexec_fn=os.setpgrp)
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
process_server = subprocess.Popen("hpp-rbprm-server",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
# wait a little for the initialization of the server/viewer
time.sleep(3)
......
......@@ -158,23 +158,23 @@ class FullBody(Robot):
# \param kinematicConstraintsMin : add an additionnal inequalities on the kinematicConstraints, of normal (0,0,1)
# and origin (0,0,kinematicConstraintsMin)
def addLimb( # noqa
self,
limbId,
name,
effectorname,
offset,
normal,
x,
y,
samples,
heuristicName,
resolution,
contactType="_6_DOF",
disableEffectorCollision=False,
grasp=False,
limbOffset=[0, 0, 0],
kinematicConstraintsPath="",
kinematicConstraintsMin=0.):
self,
limbId,
name,
effectorname,
offset,
normal,
x,
y,
samples,
heuristicName,
resolution,
contactType="_6_DOF",
disableEffectorCollision=False,
grasp=False,
limbOffset=[0, 0, 0],
kinematicConstraintsPath="",
kinematicConstraintsMin=0.):
boolValEff = 0.
if (disableEffectorCollision):
boolValEff = 1.
......@@ -256,7 +256,7 @@ class FullBody(Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName, configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False,[0,0,0,0])
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
return sId
# # Retrieves the contact candidates configurations given a configuration and a limb
......@@ -322,7 +322,7 @@ class FullBody(Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName, configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False,[0,0,0,0])
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
return cl.setStartStateId(sId)
# # Initialize the goal configuration of the path interpolation
......@@ -339,7 +339,7 @@ class FullBody(Robot):
num_max_sample = 1
for (limbName, normal) in zip(contacts, normals):
p = cl.getEffectorPosition(limbName, configuration)[0]
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False,[0,0,0,0])
cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
return cl.setEndStateId(sId)
# # Initialize the first state of the path interpolation
......
......@@ -4,15 +4,16 @@ from hpp.corbaserver import ProblemSolver, Client
import time
from abc import abstractmethod
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
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):
......@@ -32,28 +33,28 @@ class AbstractContactGenerator:
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:])
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
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.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
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'
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):
......@@ -67,11 +68,11 @@ class AbstractContactGenerator:
"""
Define the root translation bounds and the extra config bounds
"""
self.fullbody.setJointBounds ("root_joint", self.root_translation_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):
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
......@@ -82,7 +83,7 @@ class AbstractContactGenerator:
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):
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,
......@@ -103,7 +104,6 @@ class AbstractContactGenerator:
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
......@@ -120,7 +120,7 @@ class AbstractContactGenerator:
"""
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):
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
......@@ -138,16 +138,16 @@ class AbstractContactGenerator:
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]
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]
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]
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]
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]
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]
......@@ -171,22 +171,21 @@ class AbstractContactGenerator:
print("Generate contact plan ...")
t_start = time.time()
self.configs = self.fullbody.interpolate(0.01,
pathId=self.pid,
robustnessTreshold=self.robustness,
filterStates=self.filter_states,
testReachability=self.test_reachability,
quasiStatic=self.quasi_static,
erasePreviousStates=self.erase_previous_states)
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):
def display_sequence(self, dt=0.5):
"""
Display the sequence of configuration in contact,
requires that self.configs is not empty
......@@ -214,8 +213,6 @@ class AbstractContactGenerator:
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.
......@@ -228,4 +225,3 @@ class AbstractContactGenerator:
self.init_viewer()
self.compute_configs_from_guide()
self.interpolate()
......@@ -4,93 +4,97 @@ from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver
import time
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):
"""
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
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 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 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):
"""
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)
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):
"""
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'])
self.rbprmBuilder.setFilter(self.used_limbs)
for limb in self.used_limbs:
self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def init_problem(self):
"""
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 = []):
"""
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)
......@@ -98,116 +102,116 @@ class AbstractPathPlanner:
(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, reduceSizes = reduce_sizes)
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()
try:
self.pp = PathPlayer(self.v)
except Exception:
pass
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):
"""
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, reduceSizes=reduce_sizes)
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()
try:
self.pp = PathPlayer(self.v)
except Exception:
pass
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):
"""
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)