[Scenarios] changes in talos_plateforme and talos_flatground for PAL demos

src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround_path.py

@@ -2,14 +2,28 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
 
 
 class PathPlanner(TalosPathPlanner):
+
+    def init_problem(self):
+        self.a_max = 0.05
+        super().init_problem()
+        # greatly increase the number of loops of the random shortcut
+        self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
+        # force the base orientation to follow the direction of motion along the Z axis
+        self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
+
+
     def run(self):
         self.init_problem()
 
         self.q_init[:2] = [0, 0]
-        self.q_goal[:2] = [1, 0]
+        self.q_goal[:2] = [1., 0]
+
+        # Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
+        #self.q_init[-6:-3] = [0.1, 0, 0]
+        #self.q_goal[-6:-3] = [0, -0.1, 0]
 
         self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
-        self.init_planner()
+        self.init_planner(True, False)
         self.solve()
         self.display_path()
         # self.play_path()

src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes.py

@@ -11,7 +11,7 @@ class ContactGenerator(TalosContactGenerator):
         # use a model with upscaled collision geometry for the feet
         Robot.urdfSuffix += "_safeFeet"
         self.fullbody = Robot()
-        self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
+        self.q_ref = self.fullbody.referenceConfig_legsApart[::] + [0] * self.path_planner.extra_dof
         self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
 
     def set_joints_bounds(self):

src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes_path.py

@@ -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[:3] = [0.16, 0.25, 1.14]
-        self.q_goal[:3] = [1.09, 0.25, 1.14]
+        self.q_init[:3] = [0.18, 0.25, 1.14]
+        self.q_goal[:3] = [1.07, 0.25, 1.14]
 
         self.init_viewer("multicontact/plateforme_surfaces",
                          reduce_sizes=[0.18, 0, 0],