-
Notifications
You must be signed in to change notification settings - Fork 156
Description
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
Vjbe the vector (joint origin, child
joint origin) Mjis the transformation matrix from the parent joint frame to
the joint frame- if
Vjexists and if the joint axis is orthogonal toVj,
create a Blender bone that matchVj - 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
Xaxis 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