Support import of URDF models

[severin-lemaignan]

MORSE should be able to import URDF description of robots, including the full kinematic chains.

The spec is available here: http://wiki.ros.org/urdf/XML

Support for some of the Gazebo extensions could make sense as well.

Design choices for the URDF importer

URDF joints map to bones, URDF links map to objects whose origin is an empty located at the joint's bone's HEAD, and whose orientation match the URDF's joint frame.

Since Blender does not support zero-lenght bones, joints connected by zero-length links are combined into a single Blender bone (named joint1>joint2>...)

Special care must be given to the definition of the URDF joint frame. In Blender, bones always have their own frame with Y pointing from the head of the bone to the tail of the bone. X and Z can rotate around this axis with the 'roll' parameter. This is especially important because the joint rotation axis (for revolute joints) needs
to be one of the X, Y or Z axis of the Blender bone. Hence, to match the URDF joint axis (defined as a vector in the URDF joint frame), the Blender bone must be carefully build.

Here the proposed procedure:

for each joint:

• When existing and unique, let Vj be the vector (joint origin, child
  joint origin)
• Mj is the transformation matrix from the parent joint frame to
  the joint frame
• if Vj exists and if the joint axis is orthogonal to Vj,
  create a Blender bone that match Vj
• if not, place the bone tail such as the bone is orthogonal with
  the rotation axis, and if possible, coplanar with the parent and/or
  the child bones
• roll it so that the bone's X axis is aligned with the joint axis
• creates an empty, name it after the link, place it with Mj,
  parent it to the bone

After following this procedure, the rotation axis is always the X axis of the bone.

Note that one link may be connected to several other (child) links at different places: corresponding Blender bones may not be "visually" connected.

Multi-DOFs

Attention: this procedure does not provide support for multi-DoFs links! For multi-DoFs links, a maximum of 3 DoFs can be supported, as long as they correspond to independant rotations on independant axis (ie, rotations on mutually orthogonal axis). The orientation of the bone must then be computed such as each of these rotations axis match one of the bone main axis.

If more than 3 DoFs are needed, or if the rotations are not independant, then an artifical non-null link must be added. If this can be done automatically has yet to be researched.

If the joints at the end of the kinematic chains (ie, joints whose child links are not connected to any other link) are fixed joints, we consider them as 'static frames' and we do not create a bone for them (only an empty is created, named after the link).

Current state

• parse a URDF file into a set of links and joints
• build out of it a Blender armature for simple cases (in particular, no multi-dofs joint)
• import Collada meshes
• import physics details (mass, intertia...)
• nicely integrate into the Builder API
• build a test-suite with URDF files ranging from simple to more advanced
• handle multi-dofs

[severin-lemaignan]

I've started the work in a private branch.

[adegroote]

Can you summarize what is working and what needs to be done ?

[severin-lemaignan]

Done.

[PierrickKoch]

for collada meshes, you can have a look at
morse.builder.abstractcomponent.AbstractComponent.append_collada

[kavonszadkowski]

Severin pointed out this issue to me on the ros-users mailing list, as I am working on a Blender add-on for robot modelling called Phobos (https://github.com/rock-simulation/phobos).
As I mentioned on the mailing list, we may face slightly different issues when importing URDF, as we are currently using armatures differently in Phobos: we create one armature per link with one bone each. Thus I haven't been working with proper multi-bone Blender armatures a lot and don't know in which creative ways Blender tries to ruin one's day with them.

As for multiple DoF, I'm not sure if there really is a problem. There are only two types of joints in URDF with more than one degree of freedom: planar and floating. Floating is not a real mechanical link as far as I understand it as it allows 6 DoF, and planar only allows movement on a plane or 2 DoF. Thus for instance two rotational DoF at the same position would have to be modelled with two links and two joints anyway in URDF.

Which parenting method are you using? This was a bit of an issue for us until we figured out that Blender recently introduced the BONE_RELATIVE parenting method, that solved our problems, as it decoupled moving the edit bone from moving children of the pose bone.

[severin-lemaignan]

@kavonszadkowski In its current state, the URDF importer does not yet parent meshes to bones. I'm still struggling with a correct way to represent the armature.

The issue I call "multi DoF" issue is not really a multi-DoF joint issue in URDF: as you said, only planar and floating are real multi-DoF joints, and those are special anyway.
But URDF allow for 0-lenght joints, ie, you can get (and you actually often have) two 1-DoF joints that have the exact same origin. Blender does not allow for 0-lenght bones, which make it really hard to properly model such overlapping joints.

[kavonszadkowski]

@severin-lemaignan
As I said I'm not overly familiar with editing multi-bone armatures, but I think you can solve your problem by changing your parenting type within the armature. In edit mode, there is two different types of bone-bone parenting: Connected and Keep Offset. If you extrude one bone from another, the head of every subsequent bone is connected to the tail of the preceding one and Blender implicitly assigns the 'Connected' parenting type. However if you do not extrude a new bone, but Add it, you can parent them with the 'Keep Offset' option. That allows two heads to be placed in the same location, although the bones are connected. I'm assuming the default parenting option is Connected and the documentation is elusive.

I haven't quite figured out yet how these parenting options relate to the BONE and BONE_RELATIVE parenting types one can assign between bones in pose mode.

Anyway, this should allow you to put multiple joints with arbitrary axes at the same origin in theory, in practice I haven't really tested it thoroughly.

[severin-lemaignan]

Hum, yes, interesting idea. I'll check that.

[kavonszadkowski]

I'm wondering if you guys have made any progress on this as the idea was floating around in the Phobos project to maybe switch to one-armature-one-URDF at some point.

[severin-lemaignan]

As far as I'm concerned, I've not worked on URDF in MORSE since last year.

[warp1337]

Bump need this for Pepper :D

[severin-lemaignan]

I've reviving this work a little, to add initial import of Collada meshes: 28dde04

[severin-lemaignan]

Just a short update. @jbaudisch has been doing a lot of work lately on the URDF import, and complex models like the Meka robot can now be imported directly from their URDF:

Still not complete (in particular, @jbaudisch has introduced several changes in the way the armature is built which might require some changes to the armature actuator + some other models like Pepper still have scaling issues), but making good progress.

To test:

• clone my urdf branch
• re-install MORSE
• go to testing/urdf and type morse edit meka_testing.py (you can try the other *_testing.py files as well)

[warp1337]

This is sooo awesome. Thanks a lot

[severin-lemaignan]

And here is Pepper:

We are certainly getting closer to the URDF import being merged into MORSE!

[gabinsane]

Hi, when trying to import the Pepper urdf I get the following error:

Read blend: /usr/local/share/morse/data/morse_default.blend
Error: Failed to read blend file '/morse-urdf/testing/urdf/res/pepper.urdf', not a blend file

I am using Blender 2.79. Could you please tell me whether the problem is on my side or is it the current version?

[severin-lemaignan]

@gabinsane: to import the URDF model, your need a Builder script that reference your URDF file. See https://github.com/severin-lemaignan/morse/blob/urdf/doc/morse/user/urdf.rst

[maktub1884]

@severin-lemaignan I followed the link you gave https://github.com/severin-lemaignan/morse/blob/urdf/doc/morse/user/urdf.rst to add my robot urdf file but it still showed error same as Gabinsane.
Error: Failed to read blend file '/home/noh/build/morse/data/urdf/mr18_1.urdf.xacro', not a blend file
I used Blender 2.78 and lasted Morse version with Python 3.5. Could you suggest any idea to fix this. Thank you.

[dgerod]

Hi @severin-lemaignan and others,

I have been able to load the URDF file of my OAT Viper s60 robot using @severin-lemaignan's branch. I did some minor modifications related to ROS_SHARE_ROOT because the URDF loader was failing to find the meshes of my robot (see https://github.com/drm0hr/adept_experimental/blob/master/adept_s650_support/urdf/s650.urdf), for my understanding this part of your code is a temporary solution.

The script that loads my URDF:

from morse.builder import Robot, Environment
 
urdf_file = "/home/dieesrod/Workspaces/ROS/anchoring/src/adept_viper_robot/adept_s650_support/urdf/s650.urdf"

def load_simulation():
    viper_robot = Robot(_urdf_file, name="oat_viper_s650")
    env = Environment("empty", fastmode = False)

if __name__ == "__main__":
    load_simulation()

However, I see the robot correctly using 'morse edit' but when I use 'morse run' the links are unmounted. Do you know why this happens?

Now I would like to control it using an armature actuator but I don't know how to do it. I am following the armature actuator examples in the documentation and reading the source code to check how this could be done.

What I am trying:

from morse.builder import Robot, Environment
from morse.builder.actuators import Armature
from morse.builder.sensors import Pose, ArmaturePose

_urdf_file = "/home/dieesrod/Workspaces/ROS/anchoring/src/adept_viper_robot/adept_s650_support/urdf/s650.urdf"

viper_robot = Robot(_urdf_file, name="oat_viper_s650")

pose_sensor = ArmaturePose()
viper_robot.append(pose_sensor)

env = Environment("empty", fastmode = False)

Other way:

from morse.builder import Robot, Environment
from morse.builder.actuators import Armature
from morse.builder.sensors import Pose, ArmaturePose

_urdf_file = "/home/dieesrod/Workspaces/ROS/anchoring/src/adept_viper_robot/adept_s650_support/urdf/s650.urdf"

armature = Armature(_urdf_file)

pose_sensor = ArmaturePose()
armature.append(pose_sensor)

env = Environment("empty", fastmode = False)

As neither approach is working, I have saved the simulation that loads the robot using the URDF file as a .blend file. Now I can create a new armature as KukaLWR but using new .blend file.

class AdeptViperS650(Armature):
    _name = "oat_viper_s650"
    _short_desc = "Omron Adept - 6DoF Robotic Arm"

    def __init__(self, name=None):
        Armature.__init__(self, self._name, model_name = "oat_viper_s650")
        self.create_ik_targets(["joint_6"])

def load_simulation():
    robot = FakeRobot()

    arm = AdeptViperS650()
    pose_sensor = ArmaturePose()
    arm.append(pose_sensor)
    robot.append(arm)

    env = Environment(_file_storage.find('empty_apartment.blend'))
    env.set_camera_location([2.0, -2.0, 4.0])
    env.show_framerate(True)

if __name__ == "__main__":
    load_simulation()

The simulation works when I don't use ArmaturePose() sensor, if I add this sensor the simulator crashes due to an "AttributeError: 'Armature' object has no attribute 'gettime'". In any case the links of the robot are unmounted when I start the simulation using 'morse run'.

Could someone provide me some clues to solve my problem?

Thanks in advance.

Kind regards,
-- Diego

[severin-lemaignan]

@dgerod thanks for testing the branch! Quite clearly, many bugs remain :-)
I'll see if I can dedicate a bit of time in the coming days to investigate it further, but I can not promise (ie, please feel free to dig the issue on your side as well!)

[dgerod]

Sure, I am going to start digging the issue now. And I will let you know my conclusions.

However, as this is my first time analyzing this type of defect, I would like to know what approach you use: debugging, adding traces, etc. In fact, I have added a Q&A issues asking people how they develop code in MORSE, see #801.

Thanks.

[dgerod]

First, I have created a branch with my modifications: https://github.com/dgerod/morse/tree/my-work/urdf. And I am doing some tests now.

[1] Difference between a Kuka LWR load using blender file provided by MORSE (left) and using URDF loader (right). The direction of the bones in the one using URDF are following joint rotational axis but in the original one no, and the bones are smaller in the second one.

Do you think this is OK? For my understanding this is OK but I don't know why bones in original Kuka LWR are defined in a different way.

After checking I understand that this is the bones generated by URDF class are OK, my problem is around which axis of the bone it must rotate. In my robot this is ZYYZYZ but what is happening is that all the rotations are around X axis.

I have been debugging the code and I have realized that a channel in armature is returning incorrect about around which axis a bone could rotate,

[channel.ik_dof_x, 
 channel.ik_dof_y,
 channel.ik_dof_z]

Above code returns True for all DoFs of all my robot joints. However, it should only return true for the Z axis in the case of the first joint. Due to that all rotations are apply always to the X coordinate of the bone. I have added a screenshot of the variables when I am debugging the code.

The[channel.ik_dof_x, channel.ik_dof_y, channel.ik_dof_z] are coming from Inverse Kinematics parameters of the .blend file. So, I have modified modified them using the Inverse Kinematics panel.

After locking two axis of each joint according to ZYYZYZ configuration of my robot, the rotation of the joints are correct. So, I can move the robot now !!!

However, what it still needs to be fixed is that links of the robot are unmounted when I start the simulation using 'morse run'. When the .blend file is load using 'morse edit', the robot is correctly mounted, there is no space between the links.

[2] Now I am trying to load the blend file that is generated by the URDF loader so I have created two scenes to compare MORSE behavior. In the first one I am using the .blend file that I have generated using URDF:

class AdeptViperS650(Armature):
    _name = "OAT Viper s650"
    _short_desc = "Omron Adept - 6DoF Robotic Arm"

    def __init__(self, name=None):
        Armature.__init__(self, name, model_name="oat_viper_s650")
        self.create_ik_targets(["joint_6"])

def load_simulation_3_1():

    robot = FakeRobot('robot')
    arm = AdeptViperS650()

    pose_sensor = ArmaturePose()
    arm.append(pose_sensor)
    robot.append(arm)

    env = Environment("apartment")
    env.set_camera_location([2.0, -2.0, 4.0])

While second one is using KUKA LWR default actuator.

def load_simulation_3_2():

    robot = FakeRobot('robot')
    arm = KukaLWR()

    pose_sensor = ArmaturePose()
    arm.append(pose_sensor)
    robot.append(arm)

    env = Environment("apartment")
    env.set_camera_location([2.0, -2.0, 4.0])

Second one works while first one raises the error that is what I said in one of my previous comment: AttributeError: 'Armature' object has no attribute 'gettime'. I have checked the reason for that and this method is only available in Robot class, so it is normal that Sensor class raises this error when it tries to call this method of self.robot_parent attribute (line 70) when I am using AdeptViperS650 actuator. However, what I don't understand is why this is not happening with KukaLWR() actuator, both actuators are children of Armature actuator.

I have fixed the problem [2] after debugging MORSE, it was that the .blend file of OAT Viper armature had the 'RobotTag' property. After removing it and another property in main object of the .blend file, the 'load_simulation_3_1' approach is working. This problem was there because I imported the URDF file using viper_robot = Robot(_urdf_file, name="oat_viper_s650") but I was using it as a .blend file to create an armature.

Finally, what is your expectation of using URDF class? Do you expect to load always the URDF file when a simulation is started or a blend file is created so it is used for loading the simulation? At this moment I am working on the second option, let me know if I am using a wrong approach.

[dgerod]

Let me summarize my current situation of importing an URDF model.

The approach I am using is to import a URDF file and store it as .blend file. For doing that I have to modify some properties of the .blend once I stored it.

1. Using URDF importer I import the URDF file using morse edit import_urdf_model.py command and the following script:

def load_urdf_file():
    _resource_filename = "/home/dieesrod/Workspaces/ROS/anchoring/src/adept_viper_robot/adept_s650_support/urdf/s650.urdf"

    from morse.builder.urdf import URDF
    urdf = URDF(_resource_filename)
    urdf.build()

    env = Environment("empty", fastmode = False)

2. Once the file is load in Blender I have to remove all objects in the scene (e.g. CameraFP, Ground, MORE.Properties, etc.) except "adept_s650" object.
3. Rename "adept_s650" object to "adept_s650_armature", and the armature inside this object to "Armature".
4. In the Inverse Kinematics panel of each joint of Pose I have to block the axes of the joint that cannot be moved according to the URDF file. The free ones in my robot are ZYYZYZ, there is only one DOF for each joint.
5. After this modification I stored the file as .blend in "data/actuators" as "oat_viper_s650.blend".
6. I have created a specific actuator for my Viper robot, similar to KukaLWR actutor.

class AdeptViperS650(Armature):
    _name = "OAT Viper s650"
    _short_desc = "Omron Adept - 6DoF Robotic Arm"

    def __init__(self, name=None):
        Armature.__init__(self, name, model_name="oat_viper_s650")
        self.create_ik_targets(["joint_6"])

6. And now I can load my robot in the simulation by using this new actuator class, calling morse edit my_simulation or ```morse run my_simulation`` start my simulation without crashing. And I am able to move the joints which rotate around the correct coordinate but not around the correct axis, it seems the axis is not in the center of the joint.

[Q1]

However, the robot links are incorrectly mounted when I am in run mode but correctly mounted in edit mode, in run mode there are some space between the robot links. This situation also affects to the how a joint is rotated, this is done around an axis that is not in the center of the joint as I have said previously.

Could someone help me to solve this problem? This situation of unmounted links of robots is happening with all URDF models that I have tested. I think that some properties in the .blend file should be removed (e.g. bone transformation) as it seems that some translation is applied two times to the links and/or joints.

[Q2]

Finally, to be able to load the URDF file I have done modifications related to look for file paths in the URDF module, you could see the modifications in my branch: https://github.com/dgerod/morse/tree/my-work/urdf. And I should update the importer so modifications I have done by hand in the .blend file (step 3) are done there. But, I don't know how I to access these properties through Blender Python API.

[dgerod]

Finally I was able to load the model correctly mounted and the rotation of the joints are OK when I use it in Morse. However, to be able to do that I have to do several modifications in the .blend file after importing it. Please, find attached the URDF file I am loading and the .blend file as a zip file.

My .blend model has the following parameters:

Armature in Edit mode:
---------------------------------------------------------------
              #         Head        |        Tail         
---------------------------------------------------------------
    Bone      # X,         Y,     Z |      X,   Y,      Z 
---------------------------------------------------------------
    joint_1   #    0cm, 0cm, 20.3cm |    0cm, 5cm, 20.3cm  
    joint_2   #  7.5cm, 0cm, 33.5cm |  7.5cm, 5cm, 33.5cm
    joint_3   # 34.5cm, 0cm, 33.5cm | 34.5cm, 5cm, 33.5cm
    joint_4   # 25.5cm, 0cm, 63.0cm | 25.5cm, 5cm, 63.0cm
    joint_5   # 25.5cm, 0cm, 63.0cm | 25.5cm, 5cm, 63.0cm
    joint_6   # 25.5cm, 0cm, 71.0cm | 25.5cm, 5cm, 71.0cm
---------------------------------------------------------------


Armature in Pose mode (Transform)
---------------------------------------------------
              # Location | Rotation (XYZ Euler)
---------------------------------------------------
    Bone      # X, Y, Z  | X, Y, Z
---------------------------------------------------
    joint_1   # 0, 0, 0  | 0, 0, 0
    joint_2   # 0, 0, 0  | 0, 0, 0
    joint_3   # 0, 0, 0  | 0, 0, 0
    joint_4   # 0, 0, 0  | 0, 0, 0
    joint_5   # 0, 0, 0  | 0, 0, 0
    joint_6   # 0, 0, 0  | 0, 0, 0
---------------------------------------------------
* Relative Parenting is set to FALSE in all bones.
* While Inherit Rotation and Inherit Scale are set to TRUE all bones except for joint_1.
* Inverse Kinematics of each bone are set to match ZYYZYZ rotation of my model.


Links in Object model (Transform)
---------------------------------------------------
              #  Location  | Rotation (XYZ Euler)
---------------------------------------------------
    Link      # X,    Y, Z |  X, Y, Z 
---------------------------------------------------
    Base Link # 0, -5cm, 0 |  0,     0, 0 
    Link1     # 0, -5cm, 0 |  0,     0, 0 
    Link2     # 0, -5cm, 0 |  0,  pi/2, 0 
    Link3     # 0, -5cm, 0 |  0, -pi/2, 0 
    Link4     # 0, -5cm, 0 |  0, -pi/2, 0 
    Link5     # 0, -5cm, 0 |  0, -pi/2, 0 
    Link6     # 0, -5cm, 0 |  0, -pi/2, 0 
---------------------------------------------------

While the parameters generated directly by the URDF importer are:

Armature in Edit mode (Transform)
---------------------------------------------------------------
              #         Head        |        Tail         
---------------------------------------------------------------
    Bone      # X,         Y,     Z |      X,   Y,      Z 
---------------------------------------------------------------
    joint_1   #    0cm, 0cm, 20.3cm |    0cm, 5cm, 20.3cm  
    joint_2   #  7.5cm, 0cm, 33.5cm |  7.5cm, 5cm, 33.5cm
    joint_3   # 34.5cm, 0cm, 33.5cm | 34.5cm, 5cm, 33.5cm
    joint_4   # 25.5cm, 0cm, 63.0cm | 25.5cm, 5cm, 63.0cm
    joint_5   # 25.5cm, 0cm, 63.0cm | 25.5cm, 5cm, 63.0cm
    joint_6   # 25.5cm, 0cm, 71.0cm | 25.5cm, 5cm, 71.0cm
---------------------------------------------------------------


Armature in Pose mode (Transform)
---------------------------------------------------
              # Location | Rotation (XYZ Euler)
---------------------------------------------------
    Bone      # X, Y, Z  | X, Y, Z
---------------------------------------------------
    joint_1   # 0, 0, 0  | 0, 0, 0
    joint_2   # 0, 0, 0  | 0, 0, 0
    joint_3   # 0, 0, 0  | 0, 0, 0
    joint_4   # 0, 0, 0  | 0, 0, 0
    joint_5   # 0, 0, 0  | 0, 0, 0
    joint_6   # 0, 0, 0  | 0, 0, 0
---------------------------------------------------
* Relative Parenting is set to TRUE in all bones.
* While Inherit Rotation and Inherit Scale are set to TRUE all bones except for joint_1.
* Inverse Kinematics of each bone are not locked.


Links in Object model (Transform)
-----------------------------------------------------------
              #     Location      | Rotation (XYZ Euler)
-----------------------------------------------------------
    Link      #      X, Y,      Z |  X,     Y, Z 
-----------------------------------------------------------
    Base Link #      0, 0,      0 |  0,     0, 0 
    Link1     #      0, 0, 20.3cm |  0,     0, 0 
    Link2     #  7.5cm, 0, 33.5cm |  0,  pi/2, 0 
    Link3     # 34.5cm, 0, 33.5cm |  0, -pi/2, 0 
    Link4     # 25.5cm, 0, 63.0cm |  0, -pi/2, 0 
    Link5     # 25.5cm, 0, 63.0cm |  0, -pi/2, 0 
    Link6     # 25.5cm, 0, 71.0cm |  0, -pi/2, 0 
-----------------------------------------------------------

Difference between my working .blend model and one importer by URDF:

• In my .blend model the kinematics parameters (joints) are only in definition of the armature bones, links values is equal to links in URDF file. While in the imported model the kinematics parameters are used for the bones and the links.
• Imported does not consider size of bone in Y (5cm) to correct links position.
• Relative Parenting is set to TRUE by the imported but it is FALSE in my .blend file.
• In the Inverse Kinematics panel of each joint of Pose I have to block the axes of the joint that cannot be moved according to the URDF file. The importer sets all axis of each Pose to not locked.

After checking the code, I am able to fix somethings as correct Inverse Kinematics parameters in the bones by using "self.posebone.lock_ik_x" and remove relative parenting using "armature.data.bones[joint.name].use_relative_parent".

However, the big issue is how the position of the bone is calculated in "build_editmode()" method of the URDF importer and how is calculated the position of the links in "add_link_frame()". So, before modifying these two methods I would like to know if my approach is correct or not. Could you, @severin-lemaignan, or other people let me know your opinion? Thanks.

[machinekoder]

Just wondering if this could be of use: https://github.com/dfki-ric/phobos