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

[Scripts] save 'guide' problem to be able to display path after

parent 7a71de96
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import displayContactSequence
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import ProblemSolver, Client
import time
from abc import abstractmethod
......@@ -13,6 +13,7 @@ class AbstractContactGenerator:
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):
"""
......@@ -21,8 +22,18 @@ class AbstractContactGenerator:
"""
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
......@@ -143,15 +154,19 @@ class AbstractContactGenerator:
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
"""
# specify the full body configurations as start and goal state of the problem
self.set_start_end_states()
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()
......@@ -166,12 +181,40 @@ class AbstractContactGenerator:
print("Contact plan generated in : " + str(t_interpolate_configs) + " s")
print("number of configs :", len(self.configs))
def display_sequence(self):
"""
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)
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):
"""
......
......@@ -12,6 +12,7 @@ class AbstractPathPlanner:
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
......@@ -180,6 +181,7 @@ class AbstractPathPlanner:
: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
......@@ -188,11 +190,16 @@ class AbstractPathPlanner:
def hide_rom(self):
"""
Move visual ROM far above the meshs, as we cannot just delete it.
Remove the current robot from the display
"""
q_far = self.q_init[::]
q_far[2] = -3
self.v(q_far)
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):
......
......@@ -6,6 +6,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
def __init__(self, path_planner):
super().__init__(path_planner)
self.robustness = 1
self.robot_node_name = "hrp2_14"
def load_fullbody(self):
......
......@@ -16,13 +16,10 @@ class HRP2PathPlanner(AbstractPathPlanner):
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]
self.robot_node_name = "hrp2_trunk_flexible"
def load_rbprm(self):
from hpp.corbaserver.rbprm.hrp2_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
......@@ -6,7 +6,7 @@ class TalosContactGenerator(AbstractContactGenerator):
def __init__(self, path_planner):
super().__init__(path_planner)
self.robustness = 2
self.robot_node_name = "talos"
def load_fullbody(self):
from talos_rbprm.talos import Robot
......
......@@ -15,6 +15,7 @@ class TalosPathPlanner(AbstractPathPlanner):
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]
self.robot_node_name = "talos_trunk"
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
......
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