Merge pull request #79 from pFernbach/solo

Solo

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/abstract_contact_generator.py

@@ -83,7 +83,16 @@ class AbstractContactGenerator:
         self.fullbody.setPostureWeights(self.weight_postural)
         self.fullbody.usePosturalTaskContactCreation(use_postural_task)
 
-    def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None):
+    def set_rom_filters(self):
+        """
+        Define which type of affordance should be considered to create contact with each limb
+        By default, use "Support" type for all limbs
+        """
+        for limb in self.used_limbs:
+            self.path_planner.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
+
+    def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None,
+                   disableEffectorCollision=False):
         """
         Generate the samples used for each limbs in 'used_limbs'
         :param heuristic: the name of the heuristic used,
@@ -93,6 +102,7 @@ class AbstractContactGenerator:
         :param nb_samples: The number of samples for each limb database. Default is set in the Robot python class
         :param octree_size: The size of each cell of the octree. Default is set in the Robot python class
         """
+        self.set_rom_filters()
         self.fullbody.limbs_names = self.used_limbs
         if nb_samples is None:
             nb_samples = self.fullbody.nbSamples
@@ -100,7 +110,8 @@ class AbstractContactGenerator:
             octree_size = self.fullbody.octreeSize
         print("Generate limb DB ...")
         t_start = time.time()
-        self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples, octree_size)
+        self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples, octree_size,
+                                   disableEffectorCollision=disableEffectorCollision)
         t_generate = time.time() - t_start
         print("Databases generated in : " + str(t_generate) + " s")
 

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

@@ -113,7 +113,8 @@ class AbstractPathPlanner:
         # sample only configuration with null velocity and acceleration :
         self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
 
-    def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[]):
+    def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[],
+                    min_area = None):
         """
         Build an instance of hpp-gepetto-viewer from the current problemSolver
         :param env_name: name of the urdf describing the environment
@@ -121,6 +122,7 @@ class AbstractPathPlanner:
         :param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
         (in order to avoid putting contacts closes to the edges of the surface)
         :param visualize_affordances: list of affordances type to visualize, default to none
+        :param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance
         """
         vf = ViewerFactory(self.ps)
         if self.context:
@@ -130,11 +132,13 @@ class AbstractPathPlanner:
         else:
             self.afftool = AffordanceTool()
         self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+        if min_area is not None:
+            for (aff_type, min_size) in min_area:
+                self.afftool.setMinimumArea(aff_type, min_size)
         self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
                                        "planning",
                                        vf,
                                        reduceSizes=reduce_sizes)
-
         self.v = vf.createViewer(ghost=True, displayArrows=True)
         self.pp = PathPlayer(self.v)
         for aff_type in visualize_affordances:

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()

src/hpp/corbaserver/rbprm/tools/surfaces_from_path.py

@@ -62,18 +62,33 @@ def getRotationMatrixFromConfigs(configs):
     #print "R in getRotationMatrixFromConfigs : ",R
     return R
 
+def getALlContactsNames(rbprmBuilder, q):
+    step_contacts = []
+    for limb in rbprmBuilder.urdfNameRom:
+        step_contacts += rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, limb)
+    return step_contacts
 
 # get contacted surface names at configuration
-def getContactsNames(rbprmBuilder, i, q):
+def getContactsNames(rbprmBuilder, i, q, use_all_limbs=False):
+    if use_all_limbs:
+        return getALlContactsNames(rbprmBuilder, q)
     if i % 2 == LF:  # left leg
         step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
     elif i % 2 == RF:  # right leg
         step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
     return step_contacts
 
+def getALlContactsIntersections(rbprmBuilder, q):
+    intersections = []
+    for limb in rbprmBuilder.urdfNameRom:
+        intersections += rbprmBuilder.getContactSurfacesAtConfig(q, limb)
+    return intersections
+
 
 # get intersections with the rom and surface at configuration
-def getContactsIntersections(rbprmBuilder, i, q):
+def getContactsIntersections(rbprmBuilder, i, q, use_all_limbs=False):
+    if use_all_limbs:
+        return getALlContactsIntersections(rbprmBuilder, q)
     if i % 2 == LF:  # left leg
         intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
     elif i % 2 == RF:  # right leg
@@ -118,7 +133,8 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
                                    useIntersection=False,
                                    mergeCandidates=False,
                                    max_yaw=0.5,
-                                   max_surface_area=MAX_SURFACE):
+                                   max_surface_area=MAX_SURFACE,
+                                   use_all_limbs=False):
     pathLength = ps.pathLength(pId)  #length of the path
     discretizationStep = 0.01  # step at which we check the colliding surfaces
     #print "path length = ",pathLength
@@ -152,24 +168,42 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
                     t += 0.0001
                     q = ps.configAtParam(pId, t)
             #print " t in getSurfacesFromGuideContinuous : ",t
-            step_contacts = getContactsNames(rbprmBuilder, i, q)
+            step_contacts = getContactsNames(rbprmBuilder, i, q, use_all_limbs)
             for contact_name in step_contacts:
                 if not contact_name in phase_contacts_names:
                     phase_contacts_names.append(contact_name)
         # end current phase
         # get all the surfaces from the names and add it to seqs:
-        if useIntersection:
-            intersections = getContactsIntersections(rbprmBuilder, i, q)
         phase_surfaces = []
+        if useIntersection:
+            intersections = getContactsIntersections(rbprmBuilder, i, q, use_all_limbs)
+            # First add intersection for the surfaces in contact at the last discretization step
+            for k, name in enumerate(step_contacts):
+                surface = surfaces_dict[name][0]  # [0] because the last vector contain the normal of the surface
+                if area(surface) > max_surface_area:
+                    intersection = intersections[k]
+                    if len(intersection) > 3:
+                        phase_surfaces.append(intersection)
+                else:
+                    phase_surfaces.append(surfaces_dict[name][0])
+            # then add possible small surfaces encountered during the discretization of the path
+            for name in phase_contacts_names:
+                if name not in step_contacts:
+                    phase_surfaces.append(surfaces_dict[name][0])
+        else:
+            # add all the surfaces encountered during the path
+            for name in phase_contacts_names:
+                phase_surfaces.append(surfaces_dict[name][0])
+
         for name in phase_contacts_names:
-            surface = surfaces_dict[name][0]
+            surface = surfaces_dict[name][0] # [0] because the last vector contain the normal of the surface
             if useIntersection and area(surface) > max_surface_area:
                 if name in step_contacts:
                     intersection = intersections[step_contacts.index(name)]
                     if len(intersection) > 3:
                         phase_surfaces.append(intersection)
             else:
-                phase_surfaces.append(surface)  # [0] because the last vector contain the normal of the surface
+                phase_surfaces.append(surface)
         #print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
         phase_surfaces = sorted(phase_surfaces)  # why is this step required ? without out the lp can fail
         phase_surfaces_array = []  # convert from list to array, we cannot do this before because sorted() require list