[Scenario] add scenario class for Solo and add demos scripts

src/CMakeLists.txt

@@ -119,6 +119,8 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS
     __init__.py
     talos_contact_generator.py
     talos_path_planner.py
+    solo_contact_generator.py
+    solo_path_planner.py
     )
 
 SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
@@ -143,6 +145,12 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
     talos_plateformes.py
     talos_stairs10_path.py
     talos_stairs10.py
+    talos_stairs15_path.py
+    talos_stairs15.py
+    solo_flatGround_path.py
+    solo_flatGround.py
+    solo_slalom_path.py
+    solo_pallet_path.py
     )
 
 SET(${PROJECT_NAME}_PYTHON_SCENARIOS_MEMMO

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

+from hpp.corbaserver.rbprm.scenarios.demos.solo_flatGround_path import PathPlanner
+from hpp.corbaserver.rbprm.scenarios.solo_contact_generator import SoloContactGenerator
+
+
+class ContactGenerator(SoloContactGenerator):
+    def __init__(self):
+        super().__init__(PathPlanner())
+        self.testReachability=False
+
+
+if __name__ == "__main__":
+    cg = ContactGenerator()
+    cg.run()

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

+from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
+
+
+class PathPlanner(SoloPathPlanner):
+
+    def init_problem(self):
+        self.a_max = 0.1
+        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] = [0.5, 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(True, False)
+        self.solve()
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()

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

+from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
+
+
+class PathPlanner(SoloPathPlanner):
+
+    def init_problem(self):
+        self.a_max = 0.1
+        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.q_init[:2] = [-0.85, 0.]
+       # self.q_goal[:2] = [1, 0.]
+        self.q_goal[:2] = [0., 0.]
+
+
+        self.init_viewer("ori/modular_palet_low", visualize_affordances=["Support"], reduce_sizes=[0.06, 0, 0],
+                         min_area = [['Support', 0.05]])
+        self.init_planner(True, False)
+        self.ps.directPath(self.q_init, self.q_goal, False)
+
+
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()

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

+from hpp.corbaserver.rbprm.scenarios.solo_path_planner import SoloPathPlanner
+
+
+class PathPlanner(SoloPathPlanner):
+
+    def init_problem(self):
+        self.a_max = 0.1
+        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.q_init[:2] = [-0.7, 0.95]
+        #self.q_goal[:2] = [1.05, 0.95]
+        self.q_goal[:2] = [-0.2, 0.95]
+
+        # 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/slalom_debris", visualize_affordances=["Support"], reduce_sizes=[0.05, 0, 0])
+        self.init_planner(True, False)
+        self.solve()
+
+        """
+        ps = self.ps
+        q_init = self.rbprmBuilder.getCurrentConfig();
+        q_init[0:3] = [-1.8, 0., self.rbprmBuilder.ref_height];
+        self.v(q_init)
+        q_init[-6:-3] = [0.1, 0, 0]
+        q_goal = q_init[::]
+        q_goal[0:3] = [-0.9, 0.95, self.rbprmBuilder.ref_height];
+        self.v(q_goal)
+        q_goal[-6:-3] = [0.1, 0, 0]
+        ps.setInitialConfig(q_init)
+        ps.addGoalConfig(q_goal)
+        self.v(q_goal)
+
+        q_init_0 = q_init[::]
+        t = ps.solve()
+        print("done planning, optimize path ...")
+        # v.solveAndDisplay('rm',2,0.005)
+        # for i in range(5):
+        #  ps.optimizePath(ps.numberPaths() -1)
+
+        pId_begin = ps.numberPaths() - 1
+        ### END BEGIN up to the rubbles #####
+        ps.resetGoalConfigs()
+        ### BEGIN rubbles #####
+        ps.setParameter("Kinodynamic/velocityBound", 0.4)
+        ps.setParameter("Kinodynamic/accelerationBound", 0.1)
+        q_init = self.rbprmBuilder.getCurrentConfig();
+        q_init = q_goal[::];
+        self.v(q_init)
+        # q_init[-6:-3] = [0.,0,0]
+        q_goal[0:3] = [1.05, 0.95, self.rbprmBuilder.ref_height];
+        self.v(q_goal)
+        q_goal[-6:-3] = [0.1, 0, 0]
+        ps.setInitialConfig(q_init)
+        ps.addGoalConfig(q_goal)
+        self.v(q_goal)
+
+        t = ps.solve()
+        print("done planning, optimize path ...")
+        # v.solveAndDisplay('rm',2,0.005)
+        for i in range(10):
+          ps.optimizePath(ps.numberPaths() -1)
+
+        pId_rubbles = ps.numberPaths() - 1
+        ### END rubbles #####
+        ps.resetGoalConfigs()
+        ### BEGIN after rubbles #####
+        ps.setParameter("Kinodynamic/velocityBound", 0.15)
+        ps.setParameter("Kinodynamic/accelerationBound", 0.1)
+        q_init = self.rbprmBuilder.getCurrentConfig();
+        q_init = q_goal[::];
+        self.v(q_init)
+        q_goal[0:3] = [2.2, 0, self.rbprmBuilder.ref_height];
+        self.v(q_goal)
+        q_goal[-6:-3] = [0.05, 0, 0]
+        ps.setInitialConfig(q_init)
+        ps.addGoalConfig(q_goal)
+        self.v(q_goal)
+
+        t = ps.solve()
+        print("done planning, optimize path ...")
+        # v.solveAndDisplay('rm',2,0.005)
+        # for i in range(5):
+        #  ps.optimizePath(ps.numberPaths() -1)
+
+        pId_end = ps.numberPaths() - 1
+        ### END after rubbles #####
+        self.pathId = pId_begin
+        ps.concatenatePath(self.pathId, pId_rubbles)
+        ps.concatenatePath(self.pathId, pId_end)
+        """
+
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()

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

+from hpp.corbaserver.rbprm.scenarios.demos.talos_stairs15_path import PathPlanner
+from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
+
+
+class ContactGenerator(TalosContactGenerator):
+    def __init__(self):
+        super().__init__(PathPlanner())
+        self.test_reachability = False
+
+    def load_fullbody(self):
+        super().load_fullbody()
+        self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
+        # path planning use different limbs, reset it to right/left feet
+        self.used_limbs = [self.fullbody.lLegId, self.fullbody.rLegId, self.fullbody.rArmId]
+        self.init_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
+        self.end_contacts = self.used_limbs
+
+    def load_limbs(self):
+        super().load_limbs("static", nb_samples=100000)
+
+    def compute_configs_from_guide(self):
+        super().compute_configs_from_guide()
+        self.q_goal[2] = self.q_ref[2] + 0.75  # set height to the top of the stairs
+
+
+if __name__ == "__main__":
+    cg = ContactGenerator()
+    cg.run()

src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs15_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.urdfNameRom = [Robot.lLegId, Robot.rLegId, Robot.rArmId]
+        self.used_limbs = Robot.urdfNameRom
+        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)
+        self.ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
+        self.ps.setParameter("Kinodynamic/verticalAccelerationBound", 3.)
+
+    def compute_extra_config_bounds(self):
+        # bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
+        self.extra_dof_bounds = [
+            -self.v_max, self.v_max, -self.v_max, self.v_max, -10, 10, -self.a_max, self.a_max, -self.a_max,
+            self.a_max, -10, 10
+        ]
+
+    def run(self):
+        self.root_translation_bounds = [-2., -1.7, 0, 2., 0.95, 2.]
+        self.init_problem()
+
+        self.q_init[:2] = [-1.9, 1.9]
+        self.q_init[3:7] = [0, 0, -0.7071, 0.7071]
+        self.q_init[2] = 0.98
+        self.q_goal = self.q_init[::]
+        self.q_goal[1] = 0.9
+        self.q_goal[2] += 3 * 0.15 # 4 steps of 15cm each
+
+        self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.08, 0., 0.], visualize_affordances=["Support"])
+        self.init_planner()
+        self.solve()
+        self.display_path()
+        # self.play_path()
+        self.hide_rom()
+
+
+if __name__ == "__main__":
+    planner = PathPlanner()
+    planner.run()

src/hpp/corbaserver/rbprm/scenarios/solo_contact_generator.py

+from .abstract_contact_generator import AbstractContactGenerator
+
+
+class SoloContactGenerator(AbstractContactGenerator):
+    def __init__(self, path_planner):
+        super().__init__(path_planner)
+        self.robustness = 5
+        self.used_limbs = ['FRleg', 'HLleg', 'FLleg', 'HRleg']
+        self.init_contacts = self.used_limbs[::]
+        self.end_contacts = self.used_limbs[::]
+        self.robot_node_name = "solo"
+
+    def set_start_end_states(self):
+        # for 3D contacts, we must specify the normals manually
+        normals_init = []
+        normals_end = []
+        for limb_id in self.init_contacts:
+            normals_init += [self.fullbody.dict_normal[self.fullbody.dict_limb_joint[limb_id]]]
+        for limb_id in self.init_contacts:
+            normals_end += [self.fullbody.dict_normal[self.fullbody.dict_limb_joint[limb_id]]]
+        self.fullbody.setStartState(self.q_init, self.init_contacts, normals_init)
+        self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
+
+    def load_fullbody(self):
+        from solo_rbprm.solo import Robot
+        self.fullbody = Robot()
+        self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
+        self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
+        self.fullbody.setConstrainedJointsBounds()
+
+    def init_viewer(self):
+        super().init_viewer()
+        self.v.addLandmark('solo/base_link_0', 0.3)
+
+    def load_limbs(self, heuristic="fixedStep04", analysis=None, nb_samples=None, octree_size=None):
+        super().load_limbs(heuristic, analysis, nb_samples, octree_size, disableEffectorCollision=True)

src/hpp/corbaserver/rbprm/scenarios/solo_path_planner.py

+from .abstract_path_planner import AbstractPathPlanner
+
+
+class SoloPathPlanner(AbstractPathPlanner):
+    def __init__(self, context = None):
+        super().__init__(context)
+        # set default bounds to a large workspace on x,y with small interval around reference z value
+        self.root_translation_bounds = [-5., 5., -5., 5., 0.241, 0.241]
+        # set default used limbs to be both feet
+        self.used_limbs = ['solo_RFleg_rom','solo_LHleg_rom','solo_LFleg_rom','solo_RHleg_rom']
+        self.size_foot_x = 0.01  # size of the feet along the x axis
+        self.size_foot_y = 0.01  # size of the feet along the y axis
+        self.v_max = 0.3
+        self.a_max = 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 = "solo_trunk"
+
+    def load_rbprm(self):
+        from solo_rbprm.solo_abstract import Robot
+        self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
+
+    def set_joints_bounds(self):
+        super().set_joints_bounds()
+
+    def set_configurations(self):
+        super().set_configurations()