Merge pull request #65 from pFernbach/topic/surface_extraction

fix surface extraction from guide

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

@@ -7,12 +7,15 @@ class PathPlanner(TalosPathPlanner):
         # select ROM model with really conservative ROM shapes
         Robot.urdfName = "talos_trunk_large_reducedROM"
         self.robot_node_name = "talos_trunk_large_reducedROM"
-        Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
+        Robot.rLegId = 'talos_rleg_rom_reduced'
+        Robot.lLegId = 'talos_lleg_rom_reduced'
+        Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
         self.used_limbs = Robot.urdfNameRom
         self.rbprmBuilder = Robot()
+
         # As the ROM names have changed, we need to set this values again (otherwise it's automatically done)
-        self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg)
-        self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
 
     def init_problem(self):
         super().init_problem()
@@ -32,10 +35,11 @@ class PathPlanner(TalosPathPlanner):
         self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
         self.init_problem()
 
-        self.q_init[:3] = [0.05, 1.2, 0.98]
-        self.q_goal[:3] = [1.87, 1.2, 1.58]
+        self.q_init[:2] = [0., 1.2]
+        self.q_goal[:2] = [1.87, 1.2]
+        self.q_goal[2] += 0.6
 
-        self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.11, 0., 0.], visualize_affordances=["Support"])
+        self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.08, 0., 0.], visualize_affordances=["Support"])
         self.init_planner()
         self.solve()
         self.display_path()

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_bauzil_with_stairs_path.py

@@ -26,10 +26,12 @@ class PathPlanner(TalosPathPlanner):
         Robot.urdfName = "talos_trunk_large_reducedROM"
         self.robot_node_name = "talos_trunk_large_reducedROM"
         # select conservative ROM for feet
-        Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
+        Robot.rLegId = 'talos_rleg_rom_reduced'
+        Robot.lLegId = 'talos_lleg_rom_reduced'
+        Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
         self.rbprmBuilder = Robot()
-        self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg)
-        self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
 
     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

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_stairs10_random_path.py

@@ -37,12 +37,14 @@ class PathPlanner(TalosPathPlanner):
         # select ROM model with really conservative ROM shapes
         Robot.urdfName = "talos_trunk_large_reducedROM"
         self.robot_node_name = "talos_trunk_large_reducedROM"
-        Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
+        Robot.rLegId = 'talos_rleg_rom_reduced'
+        Robot.lLegId = 'talos_lleg_rom_reduced'
+        Robot.urdfNameRom = [Robot.lLegId, Robot.rLegId]
         self.used_limbs = Robot.urdfNameRom
         self.rbprmBuilder = Robot()
         # As the ROM names have changed, we need to set this values again (otherwise it's automatically done)
-        self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg)
-        self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.lLegId, self.rbprmBuilder.ref_EE_lLeg)
+        self.rbprmBuilder.setReferenceEndEffector(Robot.rLegId, self.rbprmBuilder.ref_EE_rLeg)
 
     def set_random_configs(self):
         from hpp.corbaserver.rbprm.tools.sampleRotation import sampleRotationForConfig

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

@@ -5,7 +5,7 @@ from hpp.corbaserver.rbprm.tools.display_tools import displaySurfaceFromPoints
 import numpy as np
 from pinocchio import Quaternion, log3
 import eigenpy
-eigenpy.switchToNumpyMatrix()
+eigenpy.switchToNumpyArray()
 
 ROBOT_NAME = 'talos'
 MAX_SURFACE = 1.  # if a contact surface is greater than this value, the intersection is used instead of the whole surface
@@ -50,7 +50,6 @@ def getAllSurfacesDict(afftool):
 
 # get rotation matrices form configs
 def getRotationMatrixFromConfigs(configs):
-    eigenpy.switchToNumpyMatrix()
     R = []
     for config in configs:
         q = [0, 0, 0] + config[3:7]
@@ -67,18 +66,18 @@ def getRotationMatrixFromConfigs(configs):
 # get contacted surface names at configuration
 def getContactsNames(rbprmBuilder, i, q):
     if i % 2 == LF:  # left leg
-        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_lleg_rom')
+        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.lLegId)
     elif i % 2 == RF:  # right leg
-        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, ROBOT_NAME + '_rleg_rom')
+        step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q, rbprmBuilder.rLegId)
     return step_contacts
 
 
 # get intersections with the rom and surface at configuration
 def getContactsIntersections(rbprmBuilder, i, q):
     if i % 2 == LF:  # left leg
-        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_lleg_rom')
+        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.lLegId)
     elif i % 2 == RF:  # right leg
-        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, ROBOT_NAME + '_rleg_rom')
+        intersections = rbprmBuilder.getContactSurfacesAtConfig(q, rbprmBuilder.rLegId)
     return intersections
 
 
@@ -97,11 +96,11 @@ def getMergedPhases(seqs):
 def computeRootYawAngleBetwwenConfigs(q0, q1):
     quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
     quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
-    v_angular = np.matrix(log3(quat0.matrix().T * quat1.matrix())).T
-    #print "q_prev : ",q0
-    #print "q      : ",q1
-    #print "v_angular = ",v_angular
-    return v_angular[2, 0]
+    v_angular = np.array(log3(quat0.matrix() @ quat1.matrix()))
+    #print ("q_prev : ",q0)
+    #print ("q      : ",q1)
+    #print ("v_angular = ",v_angular)
+    return v_angular[2]
 
 
 def isYawVariationsInsideBounds(q0, q1, max_yaw=0.5):
@@ -164,7 +163,7 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
         phase_surfaces = []
         for name in phase_contacts_names:
             surface = surfaces_dict[name][0]
-            if useIntersection and area(surface) > MAX_SURFACE:
+            if useIntersection and area(surface) > max_surface_area:
                 if name in step_contacts:
                     intersection = intersections[step_contacts.index(name)]
                     if len(intersection) > 3: