Merge pull request #73 from pFernbach/devel

Update API to load robots

idl/hpp/corbaserver/rbprm/rbprmbuilder.idl

@@ -32,21 +32,14 @@ module hpp
     /// This function can be called several times to include several ROMs (one for each limb)
     /// The device automatically has an anchor joint called "base_joint" as
     /// root joint.
-    /// \param romRobotName the name of the robot range of motion.
-    ///  Load robot model
+    /// \param robotName the name of the robot range of motion,
     /// \param rootJointType type of root joint among "anchor", "freeflyer",
     /// "planar",
-    /// \param packageName Name of the ROS package containing the model,
-    /// \param modelName Name of the package containing the model
-    /// \param urdfSuffix suffix for urdf file,
+    /// \param urdfName name of the urdf file. It may contain
+    ///        "file://", or "package://" prefixes.
     ///
-    /// The ros url are built as follows:
-    /// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
-    /// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
-    ///
-    void loadRobotRomModel (in string romRobotName, in string rootJointType,
-             in string packageName, in string modelName,
-             in string urdfSuffix, in string srdfSuffix)
+    void loadRobotRomModel (in string robotName, in string rootJointType,
+    in string urdfName)
       raises (Error);
 
 
@@ -54,39 +47,34 @@ module hpp
 
     /// The device automatically has an anchor joint called "base_joint" as
     /// root joint.
-    /// \param trunkRobotName the name of the robot trunk used for collision.
+    /// \param robotName the name of the robot trunk used for collision,
     /// \param rootJointType type of root joint among "anchor", "freeflyer",
     /// "planar",
-    /// \param packageName Name of the ROS package containing the model,
-    /// \param modelName Name of the package containing the model
-    /// \param urdfSuffix suffix for urdf file,
-    ///
-    /// The ros url are built as follows:
-    /// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
-    /// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
+    /// \param urdfName name of the urdf file. It may contain
+    ///        "file://", or "package://" prefixes.
+    /// \param srdfName name of the srdf file. It may contain
+    ///        "file://", or "package://" prefixes.
     ///
-    void loadRobotCompleteModel (in string trunkRobotName, in string rootJointType,
-             in string packageName, in string modelName,
-             in string urdfSuffix, in string srdfSuffix)
+    void loadRobotCompleteModel (in string robotName, in string rootJointType,
+    in string urdfName, in string srdfName)
       raises (Error);
 
+    ///  Load fullbody robot model
+    ///
     /// Create a RbprmFullBody object
     /// The device automatically has an anchor joint called "base_joint" as
     /// root joint.
-    /// \param trunkRobotName the name of the robot trunk used for collision.
+    ///
+    /// \param robotName Name of the robot that is constructed,
     /// \param rootJointType type of root joint among "anchor", "freeflyer",
     /// "planar",
-    /// \param packageName Name of the ROS package containing the model,
-    /// \param modelName Name of the package containing the model
-    /// \param urdfSuffix suffix for urdf file,
-    ///
-    /// The ros url are built as follows:
-    /// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
-    /// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
+    /// \param urdfName name of the urdf file. It may contain
+    ///        "file://", or "package://" prefixes.
+    /// \param srdfName name of the srdf file. It may contain
+    ///        "file://", or "package://" prefixes.
     ///
-    void loadFullBodyRobot (in string trunkRobotName, in string rootJointType,
-             in string packageName, in string modelName,
-             in string urdfSuffix, in string srdfSuffix, in string selectedProblem)
+    void loadFullBodyRobot (in string robotName, in string rootJointType,
+       in string urdfName, in string srdfName, in string selectedProblem)
       raises (Error);
 
 		/// Create a RbprmFullBody object

matlab/effectorRomToObj.m

+% Open it on blender with default axis setting and export it with x forward. 
+% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
+
+function [] = effectorRomToObj(filename, outname)
+delimiterIn = ',';
+headerlinesIn = 0;
+points = importdata(filename,delimiterIn,headerlinesIn);
+k = convhull(points);
+vertface2obj(points,k,outname)

matlab/vertface2obj.m

+function vertface2obj(v,f,name)
+% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
+% VERTFACE2OBJ(v,f,fname)
+%     v is a Nx3 matrix of vertex coordinates.
+%     f is a Mx3 matrix of vertex indices. 
+%     fname is the filename to save the obj file.
+
+fid = fopen(name,'w');
+
+for i=1:size(v,1)
+fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
+end
+
+fprintf(fid,'g foo\n');
+
+for i=1:size(f,1);
+fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
+end
+fprintf(fid,'g\n');
+
+fclose(fid);

src/CMakeLists.txt

@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
 SET(${PROJECT_NAME}_PYTHON_TOOLS
     affordance_centroids.py
     com_constraints.py
+    constants_and_tools.py
     constraint_to_dae.py
     cwc_trajectory_helper.py
     cwc_trajectory.py

src/hpp/corbaserver/rbprm/rbprmbuilder.py

@@ -26,42 +26,55 @@ from hpp.corbaserver.robot import Robot
 #  trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
 class Builder(Robot):
     # # Constructor
-    def __init__(self, load=True, clientRbprm=None):
+    def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
+        Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
         self.tf_root = "base_link"
         if clientRbprm is None:
             self.clientRbprm = RbprmClient()
         else:
             self.clientRbprm = clientRbprm
         self.load = load
+        if load:
+          self.loadModel(robotName, rootJointType)
+
+    ## Return urdf and srdf filenames of the roms
+    #
+    def urdfROMFilenames (self):
+        urdfNameRoms = []
+        nameRoms = []
+        if hasattr (self, 'packageName') and hasattr (self,  'urdfNameRom') and \
+           hasattr (self, 'urdfSuffix') :
+            if not isinstance(self.urdfNameRom, list):
+                self.urdfNameRom = [self.urdfNameRom]
+            nameRoms = self.urdfNameRom
+            for urdfName in nameRoms:
+                urdfNameRoms += ["package://" + self.packageName + '/urdf/' + urdfName + self.urdfSuffix + '.urdf']
+        elif hasattr (self, 'urdfFilenameRom'):
+            urdfNameRoms = self.urdfFilenameRom
+            for urdfNameRom in urdfNameRoms:
+              nameRoms += [urdfNameRom.split("/")[-1].rstrip(".urdf")]
+        else :
+            raise RuntimeError (\
+            """instance should have one of the following sets of members
+            - (packageName, urdfNameRom, urdfSuffix),
+            - (urdfFilenameRom)""")
+        return nameRoms, urdfNameRoms
 
     # # Virtual function to load the robot model.
     #
-    # \param urdfName urdf description of the robot trunk,
-    # \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
+    # This method looks for the following class attribute in order to find the files to load:
+    # First it looks for urdfFilename and srdfFilename and use it if available
+    # Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
+    # \param robotNamethe name of the robot
     # \param rootJointType type of root joint among ("freeflyer", "planar",
-    #        "anchor"),
-    # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
-    # \param packageName name of the package from where the robot will be loaded
-    # \param urdfSuffix optional suffix for the urdf of the robot package
-    # \param srdfSuffix optional suffix for the srdf of the robot package
-    def loadModel(self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None):
-        Robot.__init__(self, urdfName, rootJointType, False, client=client)
-        if (isinstance(urdfNameroms, list)):
-            for urdfNamerom in urdfNameroms:
-                self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom,
-                                                         urdfSuffix, srdfSuffix)
-        else:
-            self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms,
-                                                     urdfSuffix, srdfSuffix)
+    #          "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
+    def loadModel(self, robotName, rootJointType):
+        nameRoms, urdfROMFilenames = self.urdfROMFilenames()
+        for i, urdfNameRom in enumerate(urdfROMFilenames):
+            self.clientRbprm.rbprm.loadRobotRomModel(nameRoms[i], rootJointType, urdfNameRom)
         self.clientRbprm.rbprm.initNewProblemSolver()
-        self.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
-                                                      srdfSuffix)
-        self.client.robot.meshPackageName = meshPackageName
-        self.meshPackageName = meshPackageName
-        self.packageName = packageName
-        self.urdfName = urdfName
-        self.urdfSuffix = urdfSuffix
-        self.srdfSuffix = srdfSuffix
+        urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
+        self.clientRbprm.rbprm.loadRobotCompleteModel(robotName, rootJointType, urdfFilename, srdfFilename)
 
     # # Init RbprmShooter
     #

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -28,7 +28,8 @@ from numpy import array, matrix
 #  trunk of the robot, one for the range of motion
 class FullBody(Robot):
     # # Constructor
-    def __init__(self, load=True, clientRbprm=None):
+    def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
+        Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
         self.tf_root = "base_link"
         if clientRbprm == None:
             self.clientRbprm = RbprmClient()
@@ -36,27 +37,22 @@ class FullBody(Robot):
             self.clientRbprm = clientRbprm
         self.load = load
         self.limbNames = []
+        if load:
+          self.loadFullBodyModel(robotName, rootJointType)
 
     # # Virtual function to load the fullBody robot model.
-    #
-    # \param urdfName urdf description of the fullBody robot
+    # This method looks for the following class attribute in order to find the files to load:
+    # First it looks for urdfFilename and srdfFilename and use it if available
+    # Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
+    # \param robotNamethe name of the robot
     # \param rootJointType type of root joint among ("freeflyer", "planar",
     #          "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
-    # \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
-    # \param packageName name of the package from where the robot will be loaded
-    # \param urdfSuffix optional suffix for the urdf of the robot package
-    # \param srdfSuffix optional suffix for the srdf of the robot package
-    def loadFullBodyModel(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None):
-        Robot.__init__(self, urdfName, rootJointType, False, client)
-        self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
-                                                 srdfSuffix,
+    def loadFullBodyModel(self, robotName, rootJointType):
+        urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
+        print("selected problem: ",self.client.problem.getSelected("problem")[0])
+        self.clientRbprm.rbprm.loadFullBodyRobot(robotName, rootJointType,
+                                                 urdfFilename, srdfFilename,
                                                  self.client.problem.getSelected("problem")[0])
-        self.client.robot.meshPackageName = meshPackageName
-        self.meshPackageName = meshPackageName
-        self.packageName = packageName
-        self.urdfName = urdfName
-        self.urdfSuffix = urdfSuffix
-        self.srdfSuffix = srdfSuffix
         self.octrees = {}
 
     # Virtual function to load the fullBody robot model.

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

 from abc import abstractmethod
 from hpp.gepetto import ViewerFactory, PathPlayer
 from hpp.corbaserver.affordance.affordance import AffordanceTool
-from hpp.corbaserver import ProblemSolver
+from hpp.corbaserver import ProblemSolver, Client
+from hpp.corbaserver import createContext, loadServerPlugin
+from hpp.corbaserver.rbprm import Client as RbprmClient
 
 class AbstractPathPlanner:
 
@@ -13,7 +15,11 @@ class AbstractPathPlanner:
     extra_dof_bounds = None
     robot_node_name = None  # name of the robot in the node list of the viewer
 
-    def __init__(self):
+    def __init__(self, context = None):
+        """
+        Constructor
+        :param context: An optional string that give a name to a corba context instance
+        """
         self.v_max = -1  # bounds on the linear velocity for the root, negative values mean unused
         self.a_max = -1  # bounds on the linear acceleration for the root, negative values mean unused
         self.root_translation_bounds = [0] * 6  # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
@@ -27,6 +33,17 @@ class AbstractPathPlanner:
         self.size_foot_y = 0  # size of the feet along the y axis
         self.q_init = []
         self.q_goal = []
+        self.context = context
+        if context:
+            createContext(context)
+            loadServerPlugin(context, 'rbprm-corba.so')
+            loadServerPlugin(context, 'affordance-corba.so')
+            self.hpp_client = Client(context=context)
+            self.hpp_client.problem.selectProblem(context)
+            self.rbprm_client = RbprmClient(context=context)
+        else:
+            self.hpp_client = None
+            self.rbprm_client = None
 
     @abstractmethod
     def load_rbprm(self):
@@ -107,7 +124,11 @@ class AbstractPathPlanner:
         :param visualize_affordances: list of affordances type to visualize, default to none
         """
         vf = ViewerFactory(self.ps)
-        self.afftool = AffordanceTool()
+        if self.context:
+            self.afftool = AffordanceTool(context=self.context)
+            self.afftool.client.affordance.affordance.resetAffordanceConfig()  # FIXME: this should be called in afftool constructor
+        else:
+            self.afftool = AffordanceTool()
         self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
         self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
                                        "planning", vf, reduceSizes=reduce_sizes)
@@ -135,7 +156,7 @@ class AbstractPathPlanner:
             else:
                 self.ps.addPathOptimizer("RandomShortcut")
 
-    def solve(self):
+    def solve(self, display_roadmap = False):
         """
         Solve the path planning problem.
         q_init and q_goal must have been defined before calling this method
@@ -147,7 +168,10 @@ class AbstractPathPlanner:
         self.ps.setInitialConfig(self.q_init)
         self.ps.addGoalConfig(self.q_goal)
         self.v(self.q_init)
-        t = self.ps.solve()
+        if display_roadmap and self.v.client.gui.getNodeList() is not None:
+            t = self.v.solveAndDisplay("roadmap", 5, 0.001)
+        else:
+            t = self.ps.solve()
         print("Guide planning time : ", t)
 
 

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

@@ -23,7 +23,7 @@ class AnymalContactGenerator(AbstractContactGenerator):
         self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
 
     def load_fullbody(self):
-        from hpp.corbaserver.rbprm.anymal import Robot
+        from anymal_rbprm.anymal 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

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

@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
 
 
 class AnymalPathPlanner(AbstractPathPlanner):
-    def __init__(self):
-        super().__init__()
+    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.4, 0.5]
         # set default used limbs to be both feet
@@ -19,8 +19,8 @@ class AnymalPathPlanner(AbstractPathPlanner):
         self.robot_node_name = "anymal_trunk"
 
     def load_rbprm(self):
-        from hpp.corbaserver.rbprm.anymal_abstract import Robot
-        self.rbprmBuilder = Robot()
+        from anymal_rbprm.anymal_abstract import Robot
+        self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
 
     def set_joints_bounds(self):
         super().set_joints_bounds()

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.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] = [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_navBauzil_hard_path.py

@@ -6,7 +6,7 @@ class PathPlanner(TalosPathPlanner):
         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()
+        self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
 
     def init_problem(self):
         super().init_problem()

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],

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

@@ -9,7 +9,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
         self.robot_node_name = "hrp2_14"
 
     def load_fullbody(self):
-        from hpp.corbaserver.rbprm.hrp2 import Robot
+        from hrp2_rbprm.hrp2 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

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

@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
 
 
 class HRP2PathPlanner(AbstractPathPlanner):
-    def __init__(self):
-        super().__init__()
+    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