6. Mapping & Navigation Course

6.1 Mapping Instruction

6.1.1 Introduction and Getting Started with URDF Model

  • URDF Model Introduction

URDF is a format based on the XML specification, designed for describing the structure of robots. Its purpose is to provide a robot description standard that is as general and widely applicable as possible.

Robots are typically composed of multiple links and joints. A link is defined as a rigid object with certain physical properties, while a joint connects two links and constrains their relative motion.

By connecting links with joints and imposing motion restrictions, a kinematic model is formed. The URDF file specifies the relationships between joints and links, their inertial properties, geometric characteristics, and collision models.

  • Comparison between Xacro and URDF Model

The URDF model serves as a description file for simple robot models, offering a clear and easily understandable structure. However, when it comes to describing complex robot structures, using URDF alone can result in lengthy and unclear descriptions.

To address this limitation, the xacro model extends the capabilities of URDF while maintaining its core features. The Xacro format provides a more advanced approach to describe robot structures. It greatly improves code reusability and helps avoid excessive description length.

For instance, when describing the two legs of a humanoid robot, the URDF model would require separate descriptions for each leg. On the other hand, the Xacro model allows for describing a single leg and reusing that description for the other leg, resulting in a more concise and efficient representation.

  • URDF Model Basic Syntax

  1. XML Basic Syntax

Since URDF models are written based on the XML specification, it is necessary to understand the basic structure of the XML format.

Elements:

An element can be defined as desired using the following formula:

<element>

</element>

Properties:

Properties are included within elements to define characteristics and parameters. Please refer to the following formula to define an element with properties:

<element
property_1="property value1"
property_2="property value2">

</element>

Comments:

Comments have no impact on the definition of other properties and elements. Please use the following formula to define a comment:

<!-- comment content -->

  1. Link

The Link element describes the visual and physical properties of the robot’s rigid component. The following tags are commonly used to define the motion of a link:

<visua>: Describe the appearance of the link, such as size, color and shape.

<inertial>: Describe the inertia parameters of the link, which will used in dynamics calculation.

<collision>: Describe the collision inertia property of the link.

Each tag contains the corresponding child tag. The functions of the tags are listed below.

Tag Function
origin Describe the pose of the link. It contains two parameters, including xyz and rpy. xyz describes the pose of the link in the simulated map, while rpy describes the pose of the link in the simulated map.
mess Describe the mass of the link.
inertia Describe the inertia of the link. As the inertia matrix is symmetrical, these six parameters need to be input, ixx, ixy, ixz, iyy, iyz and izz, as properties. These parameters can be calculated.
geometry Describe the shape of the link. It uses mesh parameter to load texture file, and employs filename parameters to load the path for texture file. It has three child tags, namely box, cylinder and sphere, representing rectangles, cylinders and spheres.
material Describe the material of the link. The parameter name is the required filed. The tag color can be used to change the color and transparency of the link.

  1. Joint

In a URDF model, joints are defined using the <joint> tag. They describe the kinematic and dynamic properties of the robot’s joints, as well as constraints on motion such as position and velocity limits. According to the type of motion, joints in a URDF model can be categorized into six types:

Type and Explanation Tag
Rotational joint: can rotate infinitely around a single axis continuous
Rotational joint: similar to continuous, but with rotation angle limits revolute
Prismatic joint: allows translation along an axis, with position limits prismatic
Planar joint: allows translation or rotation in orthogonal directions within a plane planar
Floating joint: allows both translation and rotation floating
Fixed joint: a special joint that does not allow any motion fixed

When defining joint behavior, the following tags are commonly used:

<parent_link>: Specifies the parent link.

<child_link>: Specifies the child link.

<calibration>: Used to calibrate the joint angle.

<dynamics>: Describes certain physical properties of the motion.

<limit>: Defines motion constraints.

Each tag contains the corresponding child tag. The functions of the tags are listed below.

Tag Function
origin Describes the pose of the parent link. It contains two parameters, including xyz and rpy. xyz describes the pose of the link in the simulated map. rpy describes the pose of the link in the simulated map.
axis Specifies the axis of rotation or translation along the parent link's X, Y, or Z axis.
limit Defines motion constraints for the child link. lower and upper attributes specify the range of motion in radians for revolute joints, while the effort attribute specifies the force/torque limit during motion, with both positive and negative values measured in newtons (N). The velocity attribute limits the rotational speed, measured in meters per second (m/s).
mimic Indicates that this joint's motion is dependent on another joint.
safety_controller Defines safety parameters that protect the joint from exceeding safe motion limits.

  1. robot Tag

The complete top tags of a robot, including the <link> and <joint> tags, must be enclosed within the <robot> tag. The format is as follows:

  1. gazebo Tag

This tag is used in conjunction with the Gazebo simulator. Within this tag, you can define simulation parameters and import Gazebo plugins, as well as specify Gazebo’s physical properties, and more.

  1. Write Simple URDF Model

(1) Name the model of the robot

To start writing the URDF model, we need to set the name of the robot following this format: <robot name=“robot model name”>. Lastly, input </robot> at the end to represent that the model is written successfully.

(2) Set links

① To write the first link and use indentation to indicate that it is part of the currently set model. Set the name of the link using the following format: <link name="link name">. Finally, conclude with </link> to indicate the successful completion of the link definition.

② When writing the link description, use indentation to indicate that the description belongs to the current link. Start the description with <visual> and end it with </visual>.

③ The <geometry> tag is employed to define the shape of a link. Once the description is complete, include </geometry>. Within the <geometry> tag, indentation is used to specify the detailed description of the link’s shape. The following example demonstrates a link with a cylindrical shape: <cylinder length="0.01" radius="0.2"/>. In this instance, length="0.01 signifies a length of 0.01 meters for the link, while radius="0.2 denotes a radius of 0.2 meters, resulting in a cylindrical shape.

④ The <origin> tag is utilized to specify the position of a link, with indentation used to indicate the detailed description of the link’s position. The following example demonstrates the position of a link: <origin rpy="0 0 0" xyz="0 0 0" />. In this example, rpy represents the angles of the link, while xyz represents the coordinates of the link’s position. This particular example indicates that the link is positioned at the origin of the coordinate system.

⑤ The <material> tag is used to define the visual appearance of a link, with indentation used to specify the detailed description of the link’s color. To start describing the color, include <material>, and end with </material> when the description is complete. The following example demonstrates setting a link color to yellow: <color rgba="1 1 0 1" />. In this example, rgba="1 1 0 1" represents the color threshold for achieving a yellow color.

(3) Set Joint

① To write the first joint, use indentation to indicate that the joint belongs to the current model being set. Then, specify the name and type of the joint as follows: <joint name="joint name" type="joint type">. Finally, include </joint> to indicate the completion of the joint definition.

Note

To learn about the type of the joint, please refer to the section Joint in this document.

② Write the description section for the connection between the link and the joint. Use indentation to indicate that it is part of the currently defined joint. The parent parameter and child parameter should be set using the following format: <parent link="parent link"/>, and <child link="child link" />. With the parent link serving as the pivot, the joint rotates the child link.

③ <origin> describes the position of the joint, with indentation used to specify the detailed coordinates of the joint. The image below describes the position of the joint: <origin xyz=“0 0 0.1” />. xyz is the coordinate of the joint, indicating that the joint is located at x=0, y=0, z=0.1 in the coordinate system.

④ <axis> describes the orientation of the joint, with indentation used to specify its precise posture. The figure below shows the posture of a joint <axis xyz="0 0 1" />, where xyz defines the orientation of the joint.

⑤ <limit> is used to restrict joint motion, with indentation applied to specify detailed angle constraints. The figure below shows a joint whose maximum torque does not exceed 300 N, with an upper rotation limit of 3.14 radians and a lower limit of -3.14 radians. These limits are defined according to the following formula: effort = joint torque (N), velocity = joint speed, lower = lower bound of the rotation angle (radians), upper = upper bound of the rotation angle (radians).

⑥ <dynamics> describes the dynamic properties of the joint, with indentation used to specify detailed motion parameters. The figure below shows an example of a joint’s dynamics parameters: <dynamics damping="50" friction="1" />, where damping specifies the damping value, and friction specifies the friction coefficient.

The complete code is shown as follows:

6.1.2 Robot URDF Model Instructions

  • Preparation

To understand the URDF model, you can refer to the section URDF Model Basic Syntax. This section provides a brief analysis of the robot model code and component models.

  • Viewing the Robot Model Code

  1. Power on the robot and connect it to the remote control software VNC. For detail informations, please refer to 1.4 Development Environment Setup and Configuration.

  2. Click the terminal icon in the system desktop to open a command-line window.

  3. Enter the following command and press Enter to stop the app auto-start service.

~/.stop_ros.sh

  1. Enter the following command to open the robot simulation model folder.

cd /home/ubuntu/ros2_ws/src/simulations/landerpi_description/urdf/

  1. Enter the following command to open the robot simulation model folder.

vim landerpi.xacro

  1. Find the code section shown in the image below:

Multiple URDF models are called to form the complete robot.

File Name Device
inertial_matrix Inertial matrix
mecanum Mecanum chassis
tank Tank chassis
ack Ackerman chassis
arm Robot arm
gripper Gripper

  • Brief Analysis of the Robot’s Main Model

  1. Robot Base Model

The LanderPi robot supports three types of chassis configurations: Mecanum chassis, Tank chassis, and Ackerman chassis. In this section, we’ll use the Mecanum chassis as an example. The Ackerman and Tank chassis share most of the same model files, with their differences explained later in this document.

Open a new command line, and enter the command to load the robot model file, which contains descriptions of the various components of the robot model.

vim mecanum.xacro

<?xml version="1.0" encoding="utf-8"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mecanum">

This is the beginning of the URDF file. It specifies the XML version and encoding, and defines a robot model named mecanum. The xmlns:xacro namespace is used here to enable the use of Xacro macros for generating the URDF.

Within the file, you’ll find a link definition named base_footprint, which represents the robot’s chassis.

   <link name="base_footprint"/>

   <joint name="base_footprint_to_base_link" type="fixed">

      <parent link="base_footprint"/>

      <child link="base_link"/>

      <origin xyz="0 0 0.054" rpy="0 0 0"/>

   </joint>

Joint Name base_footprint_to_base_link – This joint connects the robot’s footprint reference point to the main body.

Joint Types fixed – A fixed joint, meaning there is no relative motion between the two links.

Parent Link base_footprint – The robot’s footprint projection point on the ground.

Child Link base_link – The main body link of the robot.

Position Offset: xyz="0 0 0.054" – The child link is offset by 0.054 m (54 mm) along the Z-axis relative to the parent link.

Orientation Offset: rpy="0 0 0" – No rotational offset.

   <link name="base_link">

      <inertial>

         <origin xyz="0.00504187685192947 1.09185161661918E-07 -0.00805033236351077" rpy="0 0 0"/>

         <mass value="0.0549114964726037"/>

         <inertia ixx="8.19348226076021E-05" ixy="5.66571121584358E-10" ixz="2.76933476090848E-06"

                       iyy="0.000261801130920988" iyz="-1.79231384469249E-11" izz="0.000329791457319821"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/base_link.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/base_link.STL"/>

         </geometry>

      </collision>

   </link>

This section defines the robot’s main body link: base_link.

Inertial Properties (inertial)

Center of Mass: origin xyz="..." – Offset of the center of mass relative to the link’s coordinate frame. X: 5.04mm, slightly forward.

Y: ~0 mm, near the center line.

Z: -8.05mm, slightly downward.

Mass: 0.055 kg, approximately 55 g.

Inertia Matrix: Defines the link’s rotational inertia around each axis.

ixx, iyy, izz: Principal moments of inertia about the X, Y, and Z axes.

ixy, ixz, iyz: Products of inertia, cross terms.

Visual Properties (visual)

Geometry: Uses the same STL file for rendering as for collision detection.

Position: No offset, aligned with the visual model.

Material Color: RGBA value is defined to display as a light bluish-gray.

R: 0.792 (79.2%)

G: 0.820 (82.0%)

B: 0.933 (93.3%)

A: 1.0 (fully opaque)

Collision Properties (collision)

Geometry: Uses the same STL file for rendering as for collision detection.

Position: No offset, aligned with the visual model.

Summary:

The base_link represents the core body link of the robot, serving multiple purposes:

Physics Simulation: Inertial parameters allow simulators to accurately compute dynamics.

Visualization: Defines how the robot appears in tools such as RViz.

Collision Detection: Used in simulation environments for physics-based collision handling.

Reference Frame: Provides the mounting base for other components such as wheels and sensors.

These detailed physical parameters ensure that the robot’s behavior in simulators like Gazebo closely matches that of the real robot.

   <!-- lidar_link -->

   <link name="lidar_frame">

      <inertial>

         <origin xyz="8.95525878428366E-05 6.449498190296E-05 -0.00525031924280694" rpy="0 0 0"/>

         <mass value="0.0385601724394132"/>

         <inertia ixx="4.66264242687519E-06" ixy="-1.6072319384352E-07" ixz="2.0526387548096E-09"

                       iyy="4.71597786598089E-06" iyz="1.10063344618588E-08" izz="7.61241693698137E-06"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/lidar_link.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/lidar_link.STL"/>

         </geometry>

      </collision>

   </link>

   <!-- lidar_joint -->

   <joint name="lidar_joint" type="fixed">

      <origin xyz="0.077447689677331 0 0.0206999999999998" rpy="0 0 0"/>

      <parent link="back_shell_middle_link"/>

      <child link="lidar_frame"/>

      <axis xyz="0 0 0"/>

   </joint>

Lidar Link (lidar_frame)

Inertial Properties

Center of Mass: Almost at the center of the link with very small offset.

X: 0.09 mm, slightly forward.

Y: 0.06 mm, slightly to the right.

Z: -5.25 mm, downward offset.

Mass: 0.0386 kg, about 38.6 g, a relatively lightweight sensor.

Inertia Matrix: Very small values, consistent with the characteristics of a compact Lidar unit.

Visual and Collision Models

Appearance: Uses the dedicated lidar_link.STL file for visualization.

Color: Same light bluish-gray as the robot’s main body.

Collision Model: Uses the same STL file as the visual model.

Lidar Mount Joint (lidar_joint)

Joint Types fixed – Rigidly mounted, no rotation allowed.

Parent Link back_shell_middle_link – Mounted to the middle section of the robot’s rear shell.

Installation Position:

X: 77.4mm, slightly forward.

Y: 0 mm, aligned with the center line.

Z: 20.7 mm, raised upward.

Orientation: No rotational offset, remains level.

   <!-- wheel_left_front_link -->

   <link name="wheel_left_front_link">

      <inertial>

         <origin xyz="3.88969803044453E-05 -0.00231155374534464 3.49803495641177E-05" rpy="0 0 0"/>

         <mass value="0.0379481315569691"/>

         <inertia ixx="3.67997665923715E-06" ixy="-1.4322868285409E-09" ixz="-1.20125267428777E-08"

                       iyy="5.74062073311133E-06" iyz="1.72620588597218E-09" izz="3.68307436222746E-06"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/wheel_left_front_link.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/wheel_left_front_link.STL"/>

         </geometry>

      </collision>

   </link>

   <!-- wheel_left_front_joint -->

   <joint name="wheel_left_front_joint" type="continuous">

      <origin xyz="0.066774 0.073754 -0.021099" rpy="0 0 0"/>

      <parent link="base_link"/>

      <child link="wheel_left_front_link"/>

      <axis xyz="0 1 0"/>

      <limit lower="0" upper="0" effort="0" velocity="0"/>

   </joint>

This section defines the robot’s left front wheel, including the wheel link and its drive joint.

Left Front Wheel (wheel_left_front_link)

Inertial Properties (inertial)

Center of Mass: Located near the center of the wheel.

X: 0.04 mm, almost no offset.

Y: -2.31 mm, slight inward offset, consistent with tire mass distribution.

Z: 0.03 mm, almost no offset.

Mass: 0.038 kg, about 38 g, a lightweight design for a mecanum wheel.

Inertia Characteristics:

iyy > ixx ≈ izz: Consistent with the inertia profile of a disk-shaped object.

iyy represents the moment of inertia around the wheel’s rotation axis (Y-axis), which is the largest value, as expected.

Wheel’s Visual and Collision Model (visual)

Appearance: Uses a dedicated STL file for the left front wheel. Color: Same light bluish-gray color as the robot’s main body.

Left Front Wheel Joint (wheel_left_front_joint)

Joint Properties

Joint Types continuous – A continuous rotation joint, allowing unlimited rotation.

Mounting Position, relative to base_link:

X: 66.77 mm, forward, front wheel position.

Y: 73.75 mm, to the left, left-side wheel.

Z: -21.10 mm, downward, at ground contact.

Rotation Axis: axis xyz="0 1 0" – Rotates around the Y-axis, matching the standard wheel rotation for forward and backward motion.

Limit Parameters: limit lower="0" upper="0" effort="0" velocity="0"

All set to 0, meaning no limits, which is normal for a continuous joint.

Actual torque and velocity limits are defined in the controller.

Characteristics of the Mecanum Wheels

From the code definitions, you can observe several important features:

Track Width and Wheelbase:

Track width, distance between left and right wheels: 73.75 mm × 2 = 147.5 mm

Wheelbase, distance between front and rear wheels: 66.77 mm × 2 = 133.54 mm

Ground Clearance: 21.10 mm – Determines the robot’s clearance from the ground.

Symmetrical Design: All four wheels have nearly identical mass about 38 g each, ensuring balanced motion.

Independent Drive: Each wheel is controlled by its own continuous joint, enabling omnidirectional movement.

2. Robot arm

Open a new terminal and enter the command to load the robot arm model file.

vim arm.urdf.xacro

   <!-- link1 -->

   <link name="link1">

      <inertial>

         <origin xyz="-5.67434079306178E-05 -0.00183922864739318 0.0128899047255676" rpy="0 0 0"/>

         <mass value="0.0118309583811137"/>

         <inertia ixx="1.4948064066526E-06" ixy="-4.84499294246996E-09" ixz="-7.07755635870065E-09"

                       iyy="1.83446645758585E-06" iyz="2.91602544513314E-08" izz="1.90385026531022E-06"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/link1.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/link1.STL"/>

         </geometry>

      </collision>

   </link>

Inertial Properties (inertial)

Center of Mass: Slightly offset relative to the link’s coordinate frame, about +12.9 mm along the Z-axis.

Mass: 0.0118 kg, approximately 11.8 g.

Inertia Tensor: Defines the rotational inertia about the three axes. The small values indicate this is a lightweight component.

Visual Properties (visual)

Position: The visual model is aligned with the link’s coordinate frame.

Geometry: Uses the STL mesh file link1.STL.

Color: Light bluish-gray, RGB value: 202, 209, 238.

Collision Properties (collision)

Uses the same STL file as the visual model.

Fully aligned with the visual model.

   <!-- joint1 -->

   <joint name="joint1" type="revolute">

      <origin xyz="0.0798305157765817 0.00796999989406598 -0.00881105926199899" rpy="0 0 0"/>

      <parent link="back_shell_black_link"/>

      <child link="link1"/>

      <axis xyz="0 -0.00872653549837298 0.999961923064171"/>

      <limit lower="-2.09" upper="2.09" effort="1000" velocity="10"/>

   </joint>

**Joint Types **

revolute: A rotational joint.

Position:

Relative position: Offset from parent link back_shell_black_link to child link link1.

X: 79.8 mm, main offset direction.

Y: 8.0mm

Z: -8.8 mm, downward offset.

Rotation axis:

<axis xyz="0 -0.00872653549837298 0.999961923064171"/>

Y component is very small (-0.0087), while Z component is nearly 1 (0.9999).

It indicates the joint primarily rotates around the Z-axis.

Motion Limits

Angular Range: -2.09 to 2.09 radians, ≈ -119.7° to +119.7°.

Maximum Torque: 1000 units.

Maximum Velocity: 10 rad/s.

   <!-- link2 -->

   <link name="link2">

      <inertial>

         <origin xyz="-4.90949608693142E-05 -0.0172946567203177 0.0348468784939513" rpy="0 0 0"/>

         <mass value="0.0134442645715788"/>

         <inertia ixx="2.06831776804487E-06" ixy="-4.83298815351655E-09" ixz="-7.08265998322103E-09"

                       iyy="1.86607200913389E-06" iyz="2.9217965782543E-08" izz="1.9164758927137E-06"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/link2.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/link2.STL"/>

         </geometry>

      </collision>

   </link>

   *<!-- joint2 -->*

   <joint name="joint2" type="revolute">

      <origin xyz="0 0.0157173842856447 0.029407276270124" rpy="0 0 0"/>

      <parent link="link1"/>

      <child link="link2"/>

      <axis xyz="-0.000112352847229615 0.999999993633059 1.05223831512449E-05"/>

      <limit lower="-2.09" upper="2.09" effort="1000" velocity="10"/>

   </joint>

Comparison of link2/joint2 and link1/joint1:

Similarities Basic structure: Both are revolute joints.

  • Shared properties: Each joint defines inertia, visual, and collision attributes.

  • Same material color applied.

Identical joint constraints: Rotation range from -2.09 to 2.09 rad, torque of 1000, and velocity of 10 rad/s.

Differences Joint1 Axis of Rotation: Primarily rotates around the Z-axis, which is the vertical axis. Joint2 Axis of Rotation: Primarily rotates around the Y-axis, which is the horizontal axis.

3. Robot Gripper

Open a new terminal and enter the command to load the robot arm file.

vim gripper.urdf.xacro

<!-- r_link -->

   <link name="r_link">

      <inertial>

         <origin xyz="0.00149994724977164 -0.00346401105593584 0.00482321845240047" rpy="0 0 0"/>

         <mass value="0.00222253527753655"/>

         <inertia ixx="4.21526138116034E-07" ixy="5.19501864465232E-12" ixz="4.17679673916264E-12"

                       iyy="2.26587637141938E-07" iyz="1.34876000686462E-07" izz="1.98272335819046E-07"/>

      </inertial>

      <visual>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/r_link.STL"/>

         </geometry>

         <material name="">

            <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>

         </material>

      </visual>

      <collision>

         <origin xyz="0 0 0" rpy="0 0 0"/>

         <geometry>

            <mesh filename="\${mesh_dir}/r_link.STL"/>

         </geometry>

      </collision>

   </link>

   <!-- r_joint -->

   <joint name="r_joint" type="revolute">

      <origin xyz="-0.00309786796953734 -0.0180033588479546 0.026950170065797" rpy="0 0 0"/>

      <parent link="gripper_link"/>

      <child link="r_link"/>

      <axis xyz="-0.999999784828379 -0.000360574512731351 0.000548023007084987"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

   </joint>

r_link

This section defines the physical and visual properties of the right-side component of the gripper.

Inertial Properties (inertial):

origin: Center of mass located at 0.0015, -0.0035, 0.0048 meters.

mass: 0.0022 kg, approximately 2.2 grams.

inertia: Inertia tensor matrix describing the rotational inertia around each axis.

Visual Properties (visual)

Uses the STL mesh file r_link.STL as the 3D model.

Material color is light blue with RGBA values 0.79, 0.82, 0.93, 1.

Collision Properties (collision)

Uses the same STL file r_link.STL for collision detection.

r_joint

Defines the rotational joint connecting the gripper body to the right-side component.

Joint Types revolute: A rotational joint.

Position and Orientation

origin: Offset of the joint relative to the parent link.

parent: gripper_link, the gripper body.

child: r_link, the right-side component.

Axis of Rotation:

axis: Axis of rotation vector: -1.0, -0.00036, 0.00055

Primarily rotates in the opposite direction around the X-axis.

Motion Limits

lower/upper: Angular Range: -1.57 to 1.57 radians, approximately ±90 degrees.

effort: Maximum Torque: 1000 N·m

velocity: Maximum Angular Velocity: 10 rad/s

<!-- r_in_joint -->

   <joint name="r_in_joint" type="revolute">

      <origin xyz="-0.0033016538756948 -0.00934973740913499 0.045358231082756" rpy="0 0 0"/>

      <parent link="gripper_link"/>

      <child link="r_in_link"/>

      <axis xyz="-0.999999999996305 0 0"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

      <mimic

         joint="r_joint"

         multiplier="1"

         offset="0" />

   </joint>

r_in_joint

Basic Joint Properties

Joint Types revolute: A rotational joint.

Position Configuration:

origin: Position relative to parent link: -0.0033, -0.0093, 0.0454 meters.

parent: gripper_link, the gripper body.

child: r_in_link, the right-side internal link.

Axis of Rotation:

axis: -1.0, 0, 0, along the negative X-axis.

Motion Limits

lower/upper: Angular Range: -1.57 to 1.57 radians, ±90 degrees.

effort: Maximum Torque: 1000 N·m

velocity: Maximum Angular Velocity: 10 rad/s

Key Feature: Mimic Mechanism

This is the most important characteristic of this joint:

<mimic

         joint="r_joint"

         multiplier="1"

         offset="0" />

Function Explanation:

Follower Joint: r_in_joint automatically follows the motion of r_joint.

multiplier="1": Follows at a 1:1 ratio, meaning the angles are fully synchronized.

offset="0": No angular offset.

<!-- r_out_joint -->

   <joint name="r_out_joint" type="revolute">

      <origin xyz="0.000243406210538505 -0.0189415012666699 0.0235398453193471" rpy="0 0 0"/>

      <parent link="r_in_link"/>

      <child link="r_out_link"/>

      <axis xyz="-0.999999999996305 0 0"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

      <mimic

         joint="r_joint"

         multiplier="-1"

         offset="0" />

   </joint>

r_out_joint

Basic Joint Properties

Joint Types revolute: A rotational joint.

Hierarchy:

parent: Parent Link: r_in_link, the right-side internal link.

child: Child Link: r_out_link, the right-side external link.

origin: Relative position: 0.0002, -0.0189, 0.0235 meters.

Axis of Rotation:

axis: -1.0, 0, 0, along the negative X-axis.

This is opposite in direction to the axes of r_joint and r_in_joint.

mimic Mechanism Analysis

      <mimic

         joint="r_joint"

         multiplier="-1"

         offset="0" />

Kinematic Chain Analysis

r_out_joint moves in the opposite direction to r_joint.

Mechanical Structure

gripper_link

r_link – outer fixed claw

r_in_link – intermediate link

r_out_link – end contact claw

 <joint name="l_joint" type="revolute">

      <origin xyz="-0.0015514745324403 0.00972619580013072 0.026264953269009" rpy="0 0 0"/>

      <parent link="gripper_link"/>

      <child link="l_link"/>

      <axis xyz="-0.999999999996305 0 0"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

      <mimic

         joint="r_joint"

         multiplier="-1"

         offset="0" />

   </joint>

l_joint

Key Feature:

axis: -1.0, 0, 0 – along the negative X-axis, same as r_joint.

multiplier="-1" follows the right main joint in the opposite direction.

parent: gripper_link → child: l_link

 <joint name="l_in_joint" type="revolute">

      <origin xyz="-0.00307929229401988 0.000993411611255865 0.045929296528685" rpy="0 0 0"/>

      <parent link="gripper_link"/>

      <child link="l_in_link"/>

      <axis xyz="-0.999999999996305 0 0"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

      <mimic

         joint="r_joint"

         multiplier="-1"

         offset="0" />

   </joint>

l_in_joint

Feature:

axis: -1.0, 0, 0, along the negative X-axis.

multiplier="-1" also follows in the opposite direction

parent: gripper_link → child: l_in_link

   <joint name="l_out_joint" type="revolute">

      <origin xyz="2.1044529118136E-05 0.0192824862431322 0.022949235908343" rpy="0 0 0"/>

      <parent link="l_in_link"/>

      <child link="l_out_link"/>

      <axis xyz="-0.999999999996305 0 0"/>

      <limit lower="-1.57" upper="1.57" effort="1000" velocity="10"/>

      <mimic

         joint="r_joint"

         multiplier="1"

         offset="0" />

   </joint>

l_out_joint

Feature:

axis: -1.0, 0, 0, along the negative X-axis.

multiplier="1” follows in the forward direction, note the difference

parent: l_in_link → child: l_out_link

Motion Principle

  1. Opening and Closing Actions

Main Control: Only r_joint needs to be controlled.

Right Side: r_in_joint moves in the same direction, while r_out_joint moves in the opposite direction.

Left Side: l_joint and l_in_joint move in the opposite direction, while l_out_joint provides compensation.

  1. Geometric Effect:

When r_joint rotates positively:

The right claw closes inward.

The left claw, due to multiplier=-1, also closes inward.

This creates a symmetrical gripping action.

  1. Parallel Maintenance Mechanism:

Through the configuration of multiple links and their axes/multipliers, the ends of the left and right claws remain parallel and symmetric, ensuring a stable and reliable grasp.

6.1.3 SLAM Map Construction Principle

  • Introduction to SLAM

Consider humans as an example: before reaching a destination, we always need to know our current location, regardless of whether a map exists. Humans rely on their eyes, while robots use sensors such as Lidar. SLAM, the short form of Simultaneous Localization and Mapping, refers to the process of localization and mapping performed simultaneously.

Localization determines the robot’s pose within a coordinate system. The origin and orientation of the coordinate system can be established from the first keyframe, an existing global map, landmarks, or GPS.

Mapping creates a map of the environment as perceived by the robot. The basic geometric elements of the map are points. The main purpose of the map is to support localization and navigation. Navigation can be divided into guidance and motion control: guidance involves global and local path planning, while motion control executes the planned path to move the robot.

  • SLAM Mapping Principles

SLAM-based mapping generally involves three main processes:

  1. Preprocessing: The raw point cloud data from the Lidar is first optimized, which involves filtering and removing faulty or noisy data.

Lidar works by emitting laser pulses, which scatter when they hit surrounding obstacles.

A portion of the reflected light is received by the Lidar sensor, and based on the time-of-flight measurement principle, the distance between the Lidar and the target point can be calculated.

About Point Clouds: In simple terms, the environmental data collected by Lidar is referred to as a point cloud. It represents the “vision” of the robot, showing the part of the environment that the sensor can detect. The captured object information is expressed as a set of discrete data points, each carrying precise distance and angular information.

  1. Matching: The current point cloud data of the local environment is matched against the existing map.

In practice, Lidar-based SLAM systems align and compare point clouds captured at different times. By calculating the relative displacement and orientation changes between them, the robot’s position can be estimated.

  1. Map Fusion: Newly acquired Lidar data is integrated into the existing map, thereby updating and refining it over time.

  • Mapping Guidelines

  1. When starting the mapping process, it is recommended to position the robot facing a flat wall, or alternatively use an enclosed cardboard box. This helps the Lidar capture as many points as possible for a stable initialization.

  2. Ensure that the robot explores all areas within its potential path. The Lidar should scan the full 360° surroundings to maximize the completeness of the generated map.

  3. In wide or open spaces, it is best to first let the robot close the mapping loop before proceeding to scan smaller details, which improves accuracy and consistency in the final map.

  • Map Evaluation Criteria

After the mapping process is completed, the following points can be used to evaluate whether the map is suitable for navigation:

  1. Check if the boundaries of obstacles are well-defined.

  2. Verify whether the map aligns with the actual surroundings, for example, closed-loop detection.

  3. Identify any gray regions within the robot’s operating area that indicate missing scans.

  4. Ensure that the map does not include obstacles that will not exist during subsequent navigation, for instance, moving objects.

  5. Confirm that, within the robot’s working area, the Lidar has scanned the full 360° field of view from every position.

6.1.4 SLAM Toolbox Mapping Algorithm

  • Algorithm Overview

The Slam Toolbox package combines data from the Lidar using LaserScan messages and applies a TF transformation from the odom frame to the base_link frame, creating a 2D spatial map. This package allows full serialization of SLAM map data and pose graphs, enabling reloading for continuous mapping, localization, merging, or other operations. Slam Toolbox can operate in both synchronous mode to process all valid sensor measurements even if delayed, and asynchronous mode to process valid sensor measurements whenever possible.

Built on top of the powerful Karto scan matcher, which has been widely used and optimized for speed, SLAM Toolbox serves as a feature-complete SLAM system in ROS, replacing packages such as gmapping, cartographer, karto, and hector. It also integrates a Google Ceres–based optimization plugin and a novel method called Elastic Pose-Graph Localization, which uses a sliding measurement window for optimization. This allows the system to track local environmental changes rather than treating them as errors, and to remove redundant nodes once the robot leaves an area, without affecting the long-term map.

Slam Toolbox is a suite of tools for 2D SLAM. Its main features include:

  1. Create maps and save them as .pgm files.

  2. Refine maps, rebuild them, or continue mapping on an existing saved map.

  3. Long-term mapping: Load a saved map, continue mapping, and remove irrelevant data from new laser point clouds

  4. Improve localization using previously built maps. Run in localization mode using “laser odometry,” without requiring map building.

  5. Synchronous and asynchronous mapping.

  6. Dynamic map merging.

  7. Plugin-based optimization solvers, including a new Google Ceres–based optimization plugin.

  8. Interactive RViz plugin.

  9. Provides graphical tools to manipulate nodes and connections during mapping.

  10. Map serialization and lossless data storage.

KARTO:

Karto_SLAM is based on graph optimizationm, which uses a highly optimized, non-iterative Cholesky decomposition to solve sparse systems. The graph optimization method represents the map as a graph, where each node corresponds to a robot pose along its trajectory and the associated sensor measurement dataset. Whenever a new node is added, the system performs a computation update.

The ROS version of Karto_SLAM uses Sparse Pose Adjustment (SPA), which is closely related to scan matching and loop closure detection. The more landmarks there are, the higher the memory requirements. However, compared to other methods, the graph optimization approach has clear advantages for large-scale mapping, since it only maintains a pose graph (robot poses) rather than the full map. Once the poses are optimized, the map can then be reconstructed.

Karto SLAM algorithm framework:

As shown in the figure above, the workflow of Karto SLAM is relatively straightforward. It follows the traditional soft real-time mechanism of SLAM: each incoming data frame is processed immediately, and then the result is returned.

Related Resources:

  • Mapping and Navigation Setup

ROS2 mapping and navigation use a virtual machine connected to the robot within the same local network.

  1. Install and Import the Virtual Machine

(1) Locate the installation package under: 06 Virtual Machine Installation Package and double-click the installer to begin installation.

(2) Click the icon to launch VMware and open the virtual machine.

(3) In the VMware interface, click Open a Virtual Machine.

(4) Navigate to the extracted folder ubuntu_ros2_humble, select the file Ubuntu 22.04 ROS2.ovf, and click Open.

(5) Enter a name for your virtual machine and choose the desired installation path. Then click Import.

(6) The import process will start, and the progress will be displayed on screen.

(7) Once the import is complete, follow the prompts to finish the installation.

  1. Copying Robot Files to the Virtual Machine

Export Files from the Robot

(1) Power on the robot and connect it to a remote control tool like VNC. For detail informations, please refer to 1.4 Development Environment Setup and Configuration.

(2) Click the terminal icon in the system desktop to open a ROS2 command-line window.

(3) Enter the command to disable the app auto-start service.

~/.stop_ros.sh

(4) Compress the navigation, slam, and simulations directories from the workspace into a .tar package and copy it into the shared tmp directory:

tar -cvf ~/shared/src.tar -C ~/ros2_ws/src ./navigation ./slam ./simulations

(5) Copy the hidden configuration file .typerc into the same shared directory:

cp ~/ros2_ws/.typerc ~/shared

(6) Use the SSH client MobaXterm to transfer the files src.tar and .typerc from the Raspberry Pi system directory /home/pi/docker/tmp to your local PC.

If you cannot find the .typerc file, enable Show Hidden Files in the file manager.

For details on connecting with MobaXterm,, please refer to 1.4 Development Environment Setup and Configuration.

Import Files into the Virtual Machine

(1) In the virtual machine, click the desktop icon to open a terminal. By default, it starts in the /home directory.

(2) Drag and drop the exported files .typerc and src.tar in section Export Files from the Robot from your PC into the virtual machine.

  1. Create and Build the Workspace

(1) On the virtual machine, click the desktop icon to open a terminal.

(2) Enter the command to create the workspace directory:

mkdir -p ros2_ws/src

(3) Extract src.tar into the current /home/ubuntu directory.

tar -xvf src.tar -C ~/ros2_ws/src

(4) Move the .typerc file to the ros2_ws directory.

mv .typerc ~/ros2_ws

(5) Navigate to the workspace directory:

cd ~/ros2_ws

(6) Enter the command to verify that .typerc has been moved to the workspace:

ls -a

(7) Build the workspace:

colcon build

(8) Edit the .bashrc file to configure the environment:

gedit ~/.bashrc

Add the following lines at the end of the .bashrc file:

source /home/ubuntu/ros2_ws/.typerc

source /home/ubuntu/ros2_ws/install/setup.bash

(9) After editing, save and close the file by pressing Ctrl + S or clicking Save at the top right.

(10) Refresh the environment settings:

source ~/.bashrc

  1. Configure the Robot for LAN Mode

(1) On the VNC desktop, open the terminal and navigate to the wifi_manager directory:

cd ~/wifi_manager

(2) Edit the Wi-Fi configuration file to set the robot to local network mode. Update the file with your Wi-Fi name and password:

gedit wifi_manager/wifi_conf.py

(3) After editing, save and close the file by pressing Ctrl + S or clicking Save at the top right.

(4) Enter the command to restart the network or reboot the robot. It is recommand to perform a full reboot. After the robot restarts, check the IP address using the mobile app or the Wi-Fi router interface to ensure connectivity.

sudo systemctl restart wifi.service

(5) Make sure the virtual machine and the robot are connected to the same local network, and that their IP addresses are on the same subnet:

Virtual Machine:

Robot:

(6) After rebooting, open the robot’s ROS 2 terminal from the desktop . Verify that the ROS_DOMAIN_ID matches the virtual machine, for example 0.

Robot:

Virtual Machine:

  • SLAM Mapping Steps

  1. On the Robot

(1) Click the terminal icon in the system desktop to open a command-line window.

(2) Enter the command to disable the app auto-start service.

~/.stop_ros.sh

(3) Entering the following command to start mapping.

ros2 launch slam slam.launch.py

  1. On the Virtual Machine

(1) On the virtual machine, click the desktop icon to open a terminal.

(2) Launch RViz to visualize the mapping results:

ros2 launch slam rviz_slam.launch.py

  1. Enable Keyboard Control on the Robot

(1) Click the terminal icon in the system desktop to open a command-line window.

(2) Start the keyboard control node by entering the following command and pressing Enter:

ros2 launch peripherals teleop_key_control.launch.py

If the prompt shown below appears, the keyboard control service has started successfully.

(3) Use the keyboard to drive the robot around the environment to build a more complete map. The table below lists the keyboard keys that can be used to control the robot’s movement and their corresponding functions.

Keyboard Key Robot Action
W Short press to switch to forward state, then the robot will keep moving forward.
S Short press to switch to backward state, then the robot will keep moving backward.
A Long press to interrupt forward or backward state and turn left.
D Long press to interrupt forward or backward state and rotate clockwise in place.

(4) When using the keyboard to move the robot for mapping, you can reduce the robot’s speed. A slower speed results in smaller relative odometry errors and better mapping accuracy. As the robot moves, RViz will gradually display the map, expanding until the entire environment is mapped.

  • Saving the Map

  1. Click the terminal icon in the system desktop to open a command-line window.

  2. Save the map by entering the following command and pressing Enter:

cd ~/ros2_ws/src/slam/maps && ros2 run nav2_map_server map_saver_cli -f "map_01" --ros-args -p map_subscribe_transient_local:=true

  • Optimization

To achieve more precise mapping results, you can optimize the odometry. The robot relies on odometry for mapping, which in turn depends on the IMU.

The robot comes with preloaded calibrated IMU data, which allows it to perform mapping and navigation normally. However, you can recalibrate the IMU to achieve higher precision. For IMU calibration methods and steps, refer to 1. IMU, Linear Velocity, Angular Velocity Calibration.

  • Parameter Description

The parameter file can be found at: ros2_ws/src/slam/config/slam.yaml.

For detailed explanations about the parameters, please refer to the official documentation: https://wiki.ros.org/slam_toolbox.

  • launch File Analysis

The launch file is located at: /home/ubuntu/ros2_ws/src/slam/launch/slam.launch.py

  1. Imported Libraries

Detailed explanations for launch library can be found in the official ROS documentation:

https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

  1. Setting Paths

Use get_package_share_directory to obtain the path of the slam package.

  1. Starting Other Launch Files

base_launch – Launches the hardware required by the program.

slam_launch – Base launch for mapping.

bringup_launch – Initial pose launch.

6.1.5 RTAB-VSLAM 3D Mapping

  • Introduction to RTAB-VSLAM

RTAB-VSLAM is an appearance-based real-time 3D mapping system. It is an open-source library that implements loop closure detection using memory management techniques. The system limits the map size so that loop closure detection can always be processed within a fixed time frame, enabling long-term and large-scale online mapping.

  • RTAB-VSLAM Principles

The principle of RTAB-VSLAM involves feature-based 3D mapping. Its advantage is that most environments provide abundant feature points, which makes it highly adaptable to different scenes and enables relocalization using these features. However, it also has drawbacks: computing feature points is time-consuming, the amount of information utilized from the features is limited, much of the image data is lost, feature-based methods are ineffective in low-texture areas, and feature matching is prone to errors, which can significantly affect the results.

Once features are extracted from images, they must be matched across different time frames to form loop closures. After matching, the data is divided into two types: long-term memory (LTM) and short-term memory (STM). LTM is used for matching future data, while STM is used to match consecutive data within the current time frame.

During operation, the RTAB-VSLAM algorithm first updates the robot’s pose and map using STM, and when future data matches LTM, the corresponding LTM data is incorporated into STM to further update the pose and the map.

RTAB-VSLAM software resources: https://github.com/introlab/rtabmap

  • RTAB-VSLAM 3D Mapping Steps

  1. On the Robot

(1) Click the terminal icon in the system desktop to open a command-line window.

(2) Enter the command to disable the app auto-start service.

~/.stop_ros.sh

(3) Entering the following command to start mapping.

ros2 launch slam rtabmap_slam.launch.py

  1. On the Virtual Machine

(1) Click the terminal icon in the system desktop to open a command-line window.

(2) Launch RViz to visualize the mapping results:

ros2 launch slam rviz_rtabmap.launch.py

  1. Enable Keyboard Control on the Robot

(1) Click the terminal icon in the system desktop to open a command-line window.

(2) Start the keyboard control node by entering the following command and pressing Enter:

ros2 launch peripherals teleop_key_control.launch.py

If the prompt shown below appears, the keyboard control service has started successfully.

(3) Use the keyboard to drive the robot around the environment to build a more complete map. The table below lists the keyboard keys that can be used to control the robot’s movement and their corresponding functions.

Keyboard Key Robot Action
W Short press to switch to forward state, then the robot will keep moving forward.
S Short press to switch to backward state, then the robot will keep moving backward.
A Long press to interrupt forward or backward state and turn left.
D Long press to interrupt forward or backward state and rotate clockwise in place.

When using the keyboard to move the robot for mapping, you can reduce the robot’s speed. A slower speed results in smaller relative odometry errors and better mapping accuracy. As the robot moves, RViz will gradually display the map, expanding until the entire environment is mapped.

  1. Saving the Map

After completing the mapping, press Ctrl + C in each terminal window to stop the currently running programs.

Note

For 3D mapping, you do not need to manually save the map. When you stop the mapping process using Ctrl + C, the map will be saved automatically.

  • Launch File Analysis

The launch file is located at: /home/ubuntu/ros2_ws/src/slam/launch/rtabmap_slam.launch.py

1. Imported Libraries

Detailed explanations for launch library can be found in the official ROS documentation:

https://docs.ros.org/en/humble/How-To-Guides/Launching-composable-nodes.html

2. Setting Paths

Use get_package_share_directory to obtain the path of the slam package.

3. Starting Other Launch Files

base_launch – Launches the hardware required by the program.

rtabmap_launch – Launch file for RTAB 3D mapping.

bringup_launch – Initial pose launch.