Commit 6fc60fcd authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts] format changes according to reviews

parent 68e950c3
......@@ -128,10 +128,10 @@ class AbstractContactGenerator:
: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]
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[0:7] = self.path_planner.ps.configAtParam(self.pid, self.path_planner.ps.pathLength(self.pid))[0:7]
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()
......@@ -181,9 +181,7 @@ class AbstractContactGenerator:
print("Contact plan generated in : " + str(t_interpolate_configs) + " s")
print("number of configs :", len(self.configs))
"""
Helper methods used to display results
"""
# Helper methods used to display results
def display_sequence(self, dt=0.5):
"""
......
......@@ -2,8 +2,6 @@ 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 AbstractPathPlanner:
......@@ -33,8 +31,8 @@ class AbstractPathPlanner:
@abstractmethod
def load_rbprm(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def set_configurations(self):
......@@ -45,35 +43,41 @@ class AbstractPathPlanner:
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
]
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
"""
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
"""
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
"""
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()
......@@ -95,13 +99,13 @@ class AbstractPathPlanner:
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
"""
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])
......@@ -131,10 +135,10 @@ class AbstractPathPlanner:
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)
"""
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:
......@@ -149,9 +153,9 @@ class AbstractPathPlanner:
def solve(self):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
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():
......@@ -162,18 +166,13 @@ class AbstractPathPlanner:
t = self.ps.solve()
print("Guide planning time : ", t)
def run(self):
self.init_problem()
self.init_viewer()
self.init_planner()
self.solve()
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)
"""
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
......@@ -182,10 +181,10 @@ class AbstractPathPlanner:
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)
"""
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:
......@@ -195,32 +194,32 @@ class AbstractPathPlanner:
def hide_rom(self):
"""
Remove the current robot from the display
"""
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
"""
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.
"""
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[0:2] = [0, 0]
self.q_goal[0:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
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
......@@ -5,7 +5,7 @@ class PathPlanner(HRP2PathPlanner):
def run(self):
self.init_problem()
self.q_init[0:2] = [0, 0]
self.q_init[:2] = [0, 0]
self.q_goal[0] = 1.5
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
......
......@@ -12,8 +12,8 @@ class PathPlanner(HRP2PathPlanner):
self.v_max = 0.3
self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.70, 0.8]
self.init_problem()
self.q_init[0:3] = [0.12, 0.25, 0.75]
self.q_goal[0:3] = [1.08, 0.25, 0.75]
self.q_init[:3] = [0.12, 0.25, 0.75]
self.q_goal[:3] = [1.08, 0.25, 0.75]
self.init_viewer("multicontact/plateforme_surfaces",
reduce_sizes=[0.1, 0, 0],
visualize_affordances=["Support"])
......
......@@ -11,8 +11,8 @@ class PathPlanner(HyqPathPlanner):
self.root_rotation_bounds = [-0.4, 0.4, -0.3, 0.3, -0.3, 0.3]
self.init_problem()
self.q_init[0:2] = [-2, 0]
self.q_goal[0:2] = [3, 0]
self.q_init[:2] = [-2, 0]
self.q_goal[:2] = [3, 0]
self.init_viewer("multicontact/darpa", visualize_affordances=["Support"], reduce_sizes=[0.06, 0, 0])
self.init_planner(kinodynamic=False)
self.solve()
......
......@@ -13,9 +13,9 @@ class PathPlanner(HyqPathPlanner):
self.root_translation_bounds = [-1.7, 2.5, -0.2, 2, 0.64, 0.64]
self.init_problem()
self.q_init[0:2] = [-1.5, 0]
self.q_init[:2] = [-1.5, 0]
self.q_init[-6] = 0.05
self.q_goal[0:2] = [2.2, 0]
self.q_goal[:2] = [2.2, 0]
self.q_goal[-6] = 0.05
self.init_viewer("multicontact/slalom_debris")
......
......@@ -5,14 +5,14 @@ class PathPlanner(TalosPathPlanner):
def run(self):
self.init_problem()
self.q_init[0:2] = [0, 0]
self.q_goal[0:2] = [1, 0]
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()
# self.play_path()
self.hide_rom()
......
......@@ -21,14 +21,14 @@ class PathPlanner(TalosPathPlanner):
]
self.set_joints_bounds()
self.q_init[0:2] = [-0.7, 2]
self.q_goal[0:2] = [0, -1]
self.q_init[:2] = [-0.7, 2]
self.q_goal[:2] = [0, -1]
self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
#self.play_path()
# self.play_path()
self.hide_rom()
......
......@@ -19,17 +19,17 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
self.set_joints_bounds()
self.q_init[0:2] = [-0.9, 1.7]
self.q_init[:2] = [-0.9, 1.7]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
self.q_init[-6:-3] = [0.07, 0, 0]
self.q_goal[0:2] = [3.6, 1.2]
self.q_goal[:2] = [3.6, 1.2]
self.q_goal[-6:-3] = [0, -0.1, 0]
self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
#self.play_path()
# self.play_path()
self.hide_rom()
......
......@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3]
self.init_problem()
self.q_init[0:3] = [0.16, 0.25, 1.14]
self.q_goal[0:3] = [1.09, 0.25, 1.14]
self.q_init[:3] = [0.16, 0.25, 1.14]
self.q_goal[:3] = [1.09, 0.25, 1.14]
self.init_viewer("multicontact/plateforme_surfaces",
reduce_sizes=[0.18, 0, 0],
......@@ -21,7 +21,7 @@ class PathPlanner(TalosPathPlanner):
self.init_planner(kinodynamic=False)
self.solve()
self.display_path()
#self.play_path()
# self.play_path()
self.hide_rom()
......
......@@ -31,14 +31,14 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
self.init_problem()
self.q_init[0:3] = [0.05, 1.2, 0.98]
self.q_goal[0:3] = [1.87, 1.2, 1.58]
self.q_init[:3] = [0.05, 1.2, 0.98]
self.q_goal[:3] = [1.87, 1.2, 1.58]
self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.11, 0., 0.], visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
#self.play_path()
# self.play_path()
self.hide_rom()
......
from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
from pinocchio import Quaternion
import numpy as np
import random
import sys
class PathPlanner(AnymalPathPlanner):
......@@ -23,17 +25,16 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self.q_init[0:3] = [0, 0, 0.465]
self.q_init[:3] = [0, 0, 0.465]
self.q_init[3:7] = [0, 0, 0, 1]
self.q_init[-6] = self.v_init
import random
random.seed()
alpha = random.uniform(0., 2. * np.pi)
#alpha = 4.
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
......@@ -62,7 +63,6 @@ class PathPlanner(AnymalPathPlanner):
success = self.ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
self.ps.client.problem.finishSolveStepByStep()
self.display_path()
......
from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
from pinocchio import Quaternion
import numpy as np
import random
import sys
class PathPlanner(AnymalPathPlanner):
......@@ -17,18 +19,17 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self.q_init[0:3] = [0, 0, 0.465]
self.q_init[:3] = [0, 0, 0.465]
self.q_init[3:7] = [0, 0, 0, 1]
import random
random.seed()
alpha = random.uniform(0., 2. * np.pi)
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
self.v(self.q_init)
print("initial root position : ", self.q_init[0:3])
print("final root position : ", self.q_goal[0:3])
print("initial root position : ", self.q_init[:3])
print("final root position : ", self.q_goal[:3])
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
......@@ -46,7 +47,6 @@ class PathPlanner(AnymalPathPlanner):
success = self.ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
self.ps.client.problem.finishSolveStepByStep()
self.display_path()
......
from hpp.corbaserver.rbprm.scenarios.anymal_contact_generator import AnymalContactGenerator as Parent
import sys
class AnymalContactGenerator(Parent):
......@@ -49,8 +50,6 @@ class AnymalContactGenerator(Parent):
f.write("cg_too_many_states: " + str(cg_too_many_states) + "\n")
f.close()
if (not cg_success) or cg_too_many_states:
import sys
sys.exit(1)
if throw_if_goal_not_reached and not cg_reach_goal:
import sys
sys.exit(1)
from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
from pinocchio import Quaternion
import numpy as np
import random
import sys
class PathPlanner(AnymalPathPlanner):
......@@ -32,16 +34,15 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
import random
random.seed()
self.q_init[0:3] = [
self.q_init[:3] = [
random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
]
self.q_goal = self.q_init[::]
for i in range(random.randint(0, 1000)):
random.uniform(0., 1.)
self.q_goal[0:3] = [
self.q_goal[:3] = [
random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
]
......@@ -54,8 +55,8 @@ class PathPlanner(AnymalPathPlanner):
self.q_init[3:7] = quat.coeffs().tolist()
self.q_goal[3:7] = self.q_init[3:7]
self.v(self.q_init)
print("initial root position : ", self.q_init[0:3])
print("final root position : ", self.q_goal[0:3])
print("initial root position : ", self.q_init[:3])
print("final root position : ", self.q_goal[:3])
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
......@@ -73,7 +74,6 @@ class PathPlanner(AnymalPathPlanner):
success = self.ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
self.ps.client.problem.finishSolveStepByStep()
self.display_path()
......
from hpp.corbaserver.rbprm.tools.sample_root_config import generate_random_conf_without_orientation
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
from talos_rbprm.talos_abstract import Robot
import random
class PathPlanner(TalosPathPlanner):
......@@ -50,7 +51,6 @@ class PathPlanner(TalosPathPlanner):
"""
randomly sample initial and goal configuration :
"""
import random
random.seed()
# sample an initial and goal zone : either floor, right platform or left platform. With more weight on the floor
# 0,1,2 are floor, 3 is platform10 4 is platform15
......@@ -123,7 +123,7 @@ class PathPlanner(TalosPathPlanner):
self.q_init = self.ps.configAtParam(pathId, 0)
self.q_goal = self.ps.configAtParam(pathId, self.ps.pathLength(pathId))
self.display_path()
#self.play_path()
# self.play_path()
self.hide_rom()
......
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
from pinocchio import Quaternion
import numpy as np
import random
import sys
class PathPlanner(TalosPathPlanner):
......@@ -22,17 +24,16 @@ class PathPlanner(TalosPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self.q_init[0:3] = [0, 0, 1.]
self.q_init[:3] = [0, 0, 1.]
self.q_init[3:7] = [0, 0, 0, 1]
self.q_init[-6] = self.v_init
import random
random.seed()
alpha = random.uniform(0., 2. * np.pi)
#alpha = 4.
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
......@@ -61,12 +62,11 @@ class PathPlanner(TalosPathPlanner):
success = self.ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
self.ps.client.problem.finishSolveStepByStep()