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

script/scenarios/abstract_contact_generator.py

 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):
         """

script/scenarios/abstract_path_planner.py

@@ -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):

script/scenarios/hrp2_contact_generator.py

@@ -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):

script/scenarios/hrp2_path_planner.py

@@ -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
+

script/scenarios/talos_contact_generator.py

@@ -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

script/scenarios/talos_path_planner.py

@@ -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