[Scripts] add demos for bauzil with small obstacles on the floor

src/CMakeLists.txt

@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
     talos_navBauzil_hard_path.py
     talos_navBauzil_hard.py
     talos_navBauzil_path.py
+    talos_navBauzil_obstacles_path.py
     talos_navBauzil.py
+    talos_navBauzil_obstacles.py
     talos_plateformes_path.py
     talos_plateformes.py
     talos_stairs10_path.py 

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

+from hpp.corbaserver.rbprm.scenarios.demos.talos_navBauzil_obstacles_path import PathPlanner
+from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
+
+
+class ContactGenerator(TalosContactGenerator):
+    def __init__(self):
+        super().__init__(PathPlanner())
+        self.robustness = 2.
+
+    def load_fullbody(self):
+        super().load_fullbody()
+        self.fullbody.nbSamples = 100000
+        self.fullbody.limbs_names = [self.fullbody.lLegId, self.fullbody.rLegId]  # left feet first on this scenario
+
+if __name__ == "__main__":
+    cg = ContactGenerator()
+    cg.run()

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

+from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
+
+
+class PathPlanner(TalosPathPlanner):
+    def load_rbprm(self):
+        from talos_rbprm.talos_abstract import Robot
+        Robot.urdfName = "talos_trunk_large"  # load the model with conservative bounding boxes for trunk
+        self.robot_node_name = "talos_trunk_large"
+        self.rbprmBuilder = Robot()
+
+    def init_problem(self):
+        super().init_problem()
+        # greatly increase the number of loops of the random shortcut
+        self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
+        # 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.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
+        self.set_joints_bounds()
+
+        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[:2] = [2, 2.6]
+
+        self.init_viewer("multicontact/floor_bauzil_obstacles", visualize_affordances=["Support"],
+                         reduce_sizes=[0.02, 0., 0.])
+        self.init_planner()
+        self.solve()
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()