  # Create a Robot Model A model of an industrial manipulator with six degrees of freedom. The coordinate systems of its links are shown with arrows in red, green, and blue for the corresponding x, y, and z axis.

This tutorial shows how to create a kinematics and geometry description for a new robot model, based on the manufacturer's CAD data. The CAD geometry is converted to the VRML file format used by the scene graph abstraction RL::SG. The final model is used in combination with the `rlCoachMdl` demo program included with the Robotics Library to test forward and inverse position kinematics.

## Acquiring Kinematics Data

The information required for creating a model for a new robot includes the CAD data from the manufacturer. This is typically available from the company website, including 2D drawings and 3D geometry in CAD data exchange formats.

The 2D drawing shown here is a common example of such data. The side view of the industrial manipulator used throughout this tutorial provides measurements of the distances between its six joints. Using this information, we can start with the creation of a matching kinematics description. Exact measurements of manipulator from CAD data provided by manufacturer. The image shows the distances between the joint axes required for a kinematic description of the robot.

## Denavit-Hartenberg Parameters

A common way to describe the kinematic chain of a robot model are the Denavit-Hartenberg parameters introduced by Jacques Denavit and Richard S. Hartenberg. It is a very efficient notation, as only four parameters are required to define a transformation between links. A number of variations with different numbering of joints, links, and order of parameters exist, including the often used modified DH parameters.

The (right-handed) coordinate systems are chosen, so that the axis of each joint is aligned with a corresponding z axis. The z axis of the first joint is always aligned with the world frame, as the notation only describes the kinematic chain itself. Translations or rotations around the y axis cannot be defined.

In the original notation,

• d specifies the translation along the previous z axis,
• θ is the rotation around the previous z axis to align with the new x axis,
• a defines the translation along the new x axis,
• α gives the rotation around the new x axis to align with the new z axis.

With these parameters, corresponding translation and rotation matrices can be used to transform a coordinate system from one link to the next. A revolute joint is modeled by adding the joint's rotation to its θ value. For a prismatic joint, the offset is added to the value of its d parameter.

For the robot used in our tutorial, the DH parameters can be gathered from the CAD drawing shown above. The values for each link are shown in the table. The next section demonstrates how to use this information to create a kinematics description.

## Kinematics and Dynamics Description File

The kinematics description of RL::MDL describes a tree-like structure with vertices and edges. The nodes replicate the class structure used by the library. Due to its underlying spatial vector algebra, it can be used to model Denavit-Hartenberg parameters and other conventions. A transformation between two frames can use an arbitrary matrix to define the spatial relationship. Several joint types besides prismatic and revolute joints are available. Rigid body dynamics are supported via recursive Newton-Euler inverse dynamics and Articulated-Body forward dynamics. General structure of the XML file format used by RL::MDL for the description of kinematics and dynamics for robots with tree-like structures.

The available nodes in a model description tree include one world node `world`, the nodes associated with the robot's links `body`, and intermediate nodes simply called `frame`. The world frame can be assigned a transformation matrix to define the robot's position and orientation with respect to this coordinate system. There are seven `body` nodes matching the links of the Puma kinematics used in this example. These nodes can be given dynamics properties such as center of mass, an inertia tensor, and a mass. In order to create a flexible tree structure, additional `frame` nodes without special properties can be used as required by the model. Each of these nodes is identified by a unique identifier via a matching `id` attribute.

Self collisions between bodies during path planning queries can be handled via `ignore` elements inside a `body`. An `idref` attribute can be used to disable collision checking with the referenced robot body, whereas an `ignore` entry without an `idref` attribute will disable collision checking with bodies of other scene models, e.g., environment obstacles. Dynamics properties (center of mass, inertia, mass) for a body frame.

The edges in the tree are specified using a combination of static transform nodes `fixed` and the various joint types, e.g., `revolute` or `prismatic`. A static transform defines a `rotation` and `translation` between two frames `a` and `b`. These reference a `frame`, `body`, or `world` node via an `idref` attribute. A joint node by default uses the z axis, but can specify a different one via the `axis` node if desired. Maximum and minimum joint position values, as well as maximum velocity or an offset can be defined as well. These are automatically transformed from degrees to radians for relevant joint types such as `revolute`.

The Denavit-Hartenberg convention used here defines an order of d, θ, a, and α. In case of a revolute joint, the effect of the rotation is added to the θ value and for a prismatic value to the d value. Due to this, three transforms are required to create one DH revolute joint: a `fixed` node for translation d, the `revolute` joint itself, and a `fixed` node for translation a and rotation α.

For the first values d = 0.43, θ = 0°, a = 0.15, and α = -90° in the above table with DH parameters, this translates to the following XML syntax for the two `fixed` nodes before and after the `revolute` node of the first joint.

For joints, where the translation d and θ are equal to zero, the corresponding `fixed` node can be omitted. Likewise, the second `fixed` node is not needed if both translation a and rotation α in are equal to zero. The corresponding tree for the robot in this example is shown in the next image. RL::MDL tree structure of the robot described in this tutorial, consisting of six revolute joints and seven links.

A visualization of this kind can be generated by applying the XSLT stylesheet `rlmdl2dot.xsl` included in the examples to the corresponding XML kinematics file (e.g., via `xsltproc` included in libxslt) and then using the Graphviz tools to convert it to an image or PDF.

``````xsltproc rlmdl2dot.xsl robot.rlmdl.xml > robot.dot
dot -T png robot.dot -o robot.png
dot -T pdf robot.dot -o robot.pdf``````

The complete description file robot.rlmdl.xml for this tutorial's industrial manipulator is shown here.

``````<?xml version="1.0" encoding="UTF-8"?>
<rlmdl xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="rlmdl.xsd">
<model>
<manufacturer>Comau</manufacturer>
<name>Racer 7-1.4</name>
<world id="world">
<rotation>
<x>0</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0</z>
</translation>
<g>
<x>0</x>
<y>0</y>
<z>9.86055</z>
</g>
</world>
<body id="body0">
<ignore/>
<ignore idref="body1"/>
</body>
<frame id="frame0"/>
<frame id="frame1"/>
<body id="body1">
<ignore idref="body0"/>
<ignore idref="body2"/>
<cm>
<x>-0.118105</x>
<y>0.069009</y>
<z>0.049382</z>
</cm>
<i>
<xx>1.40593</xx>
<xy>0.16236</xy>
<xz>0.337009</xz>
<yy>1.82497</yy>
<yz>0.035148</yz>
<zz>1.80314</zz>
</i>
<m>75.831</m>
</body>
<frame id="frame2"/>
<body id="body2">
<ignore idref="body1"/>
<ignore idref="body3"/>
<ignore idref="body4"/>
<cm>
<x>-0.316854</x>
<y>0.000596</y>
<z>-0.13391</z>
</cm>
<i>
<xx>0.0931587</xx>
<xy>-1.88851e-005</xy>
<xz>0.0159436</xz>
<yy>0.870264</yy>
<yz>0.000660646</yz>
<zz>0.862324</zz>
</i>
<m>22.4953</m>
</body>
<frame id="frame3"/>
<body id="body3">
<ignore idref="body2"/>
<ignore idref="body4"/>
<cm>
<x>-0.067066</x>
<y>-0.012527</y>
<z>-0.01986</z>
</cm>
<i>
<xx>0.210305</xx>
<xy>0.0149452</xy>
<xz>0.0351761</xz>
<yy>0.224497</yy>
<yz>-0.0152144</yz>
<zz>0.290373</zz>
</i>
<m>23.396</m>
</body>
<frame id="frame4"/>
<frame id="frame5"/>
<body id="body4">
<ignore idref="body2"/>
<ignore idref="body3"/>
<ignore idref="body5"/>
<ignore idref="body6"/>
<cm>
<x>-9.6e-005</x>
<y>-0.389991</y>
<z>-0.003164</z>
</cm>
<i>
<xx>0.894888</xx>
<xy>-0.00116986</xy>
<xz>-4.80413e-005</xz>
<yy>0.0348884</yy>
<yz>0.011889</yz>
<zz>0.879107</zz>
</i>
<m>13.0332</m>
</body>
<frame id="frame6"/>
<body id="body5">
<ignore idref="body4"/>
<ignore idref="body6"/>
<cm>
<x>0.00456</x>
<y>0.000462</y>
<z>0.002269</z>
</cm>
<i>
<xx>0.0132424</xx>
<xy>4.34903e-005</xy>
<xz>0.000308698</xz>
<yy>0.0103445</yy>
<yz>-3.57753e-005</yz>
<zz>0.00787334</zz>
</i>
<m>4.03009</m>
</body>
<frame id="frame7"/>
<body id="body6">
<ignore idref="body4"/>
<ignore idref="body5"/>
<cm>
<x>0.00096</x>
<y>-8e-006</y>
<z>-0.007888</z>
</cm>
<i>
<xx>6.94948e-005</xx>
<xy>-2.05157e-009</xy>
<xz>-3.78268e-007</xz>
<yy>6.72486e-005</yy>
<yz>-1.96848e-007</yz>
<zz>0.000126754</zz>
</i>
<m>0.267131</m>
</body>
<frame id="frame8"/>
<fixed id="fixed0">
<frame>
<a idref="world"/>
<b idref="body0"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0</z>
</translation>
</fixed>
<fixed id="fixed1">
<frame>
<a idref="body0"/>
<b idref="frame0"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0.43</z>
</translation>
</fixed>
<revolute id="joint0">
<frame>
<a idref="frame0"/>
<b idref="frame1"/>
</frame>
<axis>
<x>0</x>
<y>0</y>
<z>-1</z>
</axis>
<max>165</max>
<min>-165</min>
<speed>140</speed>
</revolute>
<fixed id="fixed2">
<frame>
<a idref="frame1"/>
<b idref="body1"/>
</frame>
<rotation>
<x>-90</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0.15</x>
<y>0</y>
<z>0</z>
</translation>
</fixed>
<revolute id="joint1">
<frame>
<a idref="body1"/>
<b idref="frame2"/>
</frame>
<max>155</max>
<min>-85</min>
<speed>160</speed>
</revolute>
<fixed id="fixed3">
<frame>
<a idref="frame2"/>
<b idref="body2"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>-90</z>
</rotation>
<translation>
<x>0</x>
<y>-0.59</y>
<z>0</z>
</translation>
</fixed>
<revolute id="joint2">
<frame>
<a idref="body2"/>
<b idref="frame3"/>
</frame>
<axis>
<x>0</x>
<y>0</y>
<z>-1</z>
</axis>
<max>0</max>
<min>-170</min>
<speed>170</speed>
</revolute>
<fixed id="fixed4">
<frame>
<a idref="frame3"/>
<b idref="body3"/>
</frame>
<rotation>
<x>-90</x>
<y>0</y>
<z>-90</z>
</rotation>
<translation>
<x>0</x>
<y>-0.13</y>
<z>0</z>
</translation>
</fixed>
<fixed id="fixed5">
<frame>
<a idref="body3"/>
<b idref="frame4"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0.684</z>
</translation>
</fixed>
<revolute id="joint3">
<frame>
<a idref="frame4"/>
<b idref="frame5"/>
</frame>
<axis>
<x>0</x>
<y>0</y>
<z>-1</z>
</axis>
<max>210</max>
<min>-210</min>
<speed>450</speed>
</revolute>
<fixed id="fixed6">
<frame>
<a idref="frame5"/>
<b idref="body4"/>
</frame>
<rotation>
<x>90</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0</z>
</translation>
</fixed>
<revolute id="joint4">
<frame>
<a idref="body4"/>
<b idref="frame6"/>
</frame>
<max>130</max>
<min>-120</min>
<speed>375</speed>
</revolute>
<fixed id="fixed7">
<frame>
<a idref="frame6"/>
<b idref="body5"/>
</frame>
<rotation>
<x>-90</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0</z>
</translation>
</fixed>
<fixed id="fixed8">
<frame>
<a idref="body5"/>
<b idref="frame7"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>180</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0.1</z>
</translation>
</fixed>
<revolute id="joint5">
<frame>
<a idref="frame7"/>
<b idref="body6"/>
</frame>
<axis>
<x>0</x>
<y>0</y>
<z>-1</z>
</axis>
<max>2700</max>
<min>-2700</min>
<speed>550</speed>
</revolute>
<fixed id="fixed9">
<frame>
<a idref="body6"/>
<b idref="frame8"/>
</frame>
<rotation>
<x>0</x>
<y>0</y>
<z>0</z>
</rotation>
<translation>
<x>0</x>
<y>0</y>
<z>0</z>
</translation>
</fixed>
<home>
<q unit="deg">0</q>
<q unit="deg">0</q>
<q unit="deg">-90</q>
<q unit="deg">0</q>
<q unit="deg">0</q>
<q unit="deg">0</q>
</home>
</model>
</rlmdl>``````

## Converting CAD Data to VRML

Geometry models of robots are usually available on the manufacturer's support pages in various formats. Typically these include CAD data exchange formats such as IGES or STEP that can be read by many CAD programs and define a boundary representation of the 3D model. For visualization in the Robotics Library, a triangulation via a CAD kernel and an export to the VRML file format is required.

In order to convert the model to this format, either a CAD or a 3D converter program can be used. FreeCAD presents an open source alternative that can be used for this procedure. After importing a CAD model, the individual links of the robot have to be separated. The left pane of FreeCAD's main window shows all available objects of the model. For the second link, three parts have to be selected.

As FreeCAD's VRML export uses all parts of a file irrelevant of selection, the correct parts have to be selected, copied, and pasted into a new file before export. After selecting the parts in this new file and setting the draw style to shaded in order to remove CAD lines in the output, the link can be exported to a VRML file. This procedure has to be repeated for all remaining links.

## Correcting Coordinate Systems in VRML Files

The coordinate systems of the exported links now have to be adjusted to match the identified Denavit-Hartenberg parameters. The imported STEP file in the previous step used the y axis as up vector, the front of the robot faced the negative x axis, and the model used millimeters as base unit. We typically aim for the z axis as up vector, with the robot's front facing the positive x axis and meters as the base unit. The individual links all currently share the same scene origin instead of aligning it with their respective pivot point.

Please note, that the example here uses the classical Denavit-Hartenberg notation with the link's frame and z axis aligned with the following joint.

In order to correct the link's origin, the main Group or Transform node in its corresponding VRML file has to be adjusted. The `rotation`, `scale`, and `translation` fields of the Transform node can be used to modify the origin. `rotation` first defines the rotation axis with three values from `[-1,1]` followed by an angle specified in radians, here 180° around the z axis. `scale` defines three scaling factors, in this case `0.001` for conversion from millimeters to meters. `translation` specifies three values that define the offset between the desired pivot point and the scene origin, in this case in meters. These values can be extracted from the CAD drawings and Denavit-Hartenberg parameters mentioned above.

In order to better visualize the origin, a helper frame can be included with an Inline node and its `url` field. frame.wrl is an example file for such a coordinate frame. A visible origin can also be displayed when using the `wrlview` program included in the Robotics Library.

## Combining Individual Links to Full Geometry Model

In order to create a complete model, a new file is created that combines the files of individual links via Inline nodes. The following example highlights how to include the first two of seven links of a model. The values used in the Transform nodes align the links with the robot's base frame and are the inverse of the values entered in the corresponding files. These values are overwritten during runtime when setting the frames calculated by forward position kinematics and can thus be omitted if desired. As mentioned above, a helper frame can optionally be included for better visualization.

The Transform nodes are given names via the `DEF` syntax in order to be referred to later in a scene description file. The following robot.wrl file combines the individual link files exported in the previous sections.

``````#VRML V2.0 utf8
DEF robot Transform {
children [
children [
# Inline { url "frame.wrl" }
]
}
rotation 1 0 0 -1.570796
translation 0.15 0 0.43
children [
# Inline { url "frame.wrl" }
]
}
rotation -0.577350 -0.577350 -0.577350 2.094395
translation 0.15 0 1.02
children [
# Inline { url "frame.wrl" }
]
}
rotation 0 0 1 3.141593
translation 0.02 0 1.02
children [
# Inline { url "frame.wrl" }
]
}
rotation 0 0.70710677 0.70710677 3.1415927
translation 0.02 0 1.704
children [
# Inline { url "frame.wrl" }
]
}
rotation 0 0 1 3.141593
translation 0.02 0 1.704
children [
# Inline { url "frame.wrl" }
]
}
translation 0.02 0 1.804
children [
# Inline { url "frame.wrl" }
]
}
]
}``````

If the model is correctly combined, the new file should now provide a visualization of the manipulator in its chosen configuration, similar to the first image on this page. The following images show an exploded view of the model with the coordinate systems of each link and a view of the model when the fields of the Transform nodes have been omitted. In the latter case, the matrices of all links use the identity matrix.

## Writing a Scene Description File

The VRML format allows defining geometry models in a hierarchy, but cannot provide structural information (models, bodies, and shapes) to `rlsg`. This extra information is defined by a scene description file in XML format as shown below. This file references the VRML file via the `href` attribute of it's `scene` tag and the respective models and bodies of that file based on their names as defined by the `DEF` syntax next to the Transform nodes. Models and bodies in the VRML scene are referenced by name.

In combination with the Inline nodes of VRML, a new scene file can reuse existing robot and environment models in other VRML files. This also includes multiple robot models in a scene file. The following robot.rlsg.xml XML file references the robot.wrl VRML file of the previous section.

``````<?xml version="1.0" encoding="UTF-8"?>
<rlsg xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="rlsg.xsd">
<scene href="robot.wrl">
<model name="robot">
</model>
</scene>
</rlsg>``````

## Using the Kinematics and Geometry Descriptions with rlCoachMdl

Now that both kinematics and geometry descriptions of the manipulator have been created, the demo program `rlCoachMdl` can be used to visualize the full model, including forward and inverse position kinematics. rlCoachMdl with the modeled industrial manipulator with six degrees of freedom as defined by this tutorial.

`rlCoachMdl` requires a scene description file as first parameter and a matching kinematics description as second parameter. Assuming the name `robot.rlsg.xml` for the scene file and `robot.rlmdl.xml` for the kinematics file, the model can be opened with the following command on Windows

``"C:\Program Files\Robotics Library\0.7.0\MSVC\14.1\x64\bin\rlCoachMdl.exe" robot.rlsg.xml robot.rlmdl.xml``

and this one on Linux

``/usr/bin/rlCoachMdl robot.rlsg.xml robot.rlmdl.xml``

Within the program, the values of the individual joints can be modified to update the forward kinematics. Alternatively, the desired operational position can be entered and the matching joint configuration is generated via inverse kinematics.