We will be creating this robot in a URDF file.
Create a new file inside your code editor named ed_robot.urdf
. Open the file and start with a XML declaration and a root robot
tag with a name attribute. Everything else goes inside this <robot> </robot>
.
<?xml version="1.0"?>
<robot name="ed_robot">
</robot>
Moment of Intertia of various shapes Wikipedia: List of moments of inertia
We will take the following steps:
- Create robot body/base.
- Create wheels.
- Joint wheels and the body.
Robot Body
Now we start by defining our first link and we will name it base_link. This will be the main chassis/body of the robot.
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.180 0.076 0.055"/>
</geometry>
<material name="blue">
<color rgba="0.0 0.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.194 0.168 0.1"/>
</geometry>
</collision>
<inertial>
<mass value="0.86"/>
<inertia ixx="0.0029257" ixy="0.0" ixz="0.0"
iyy="0.014314" iyz="0.0"
izz="0.0119673"/>
</inertial>
</link>
Robot Wheels
This robot has 4 wheels so we need to add these 4 wheels to the URDF file. Let's do that now:
<!-- Wheels -->
<link name="front_right_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="front_left_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="rear_right_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="rear_left_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
Our robot has a body and 4 wheels. But they are not connected yet. We will be connecting them using Joints now:
Joints
These 4 wheels need to be attached to the body in 4 different places and the type of joint also needs to be defined which will help them move the right way.
<!-- Joints connecting wheels to the base -->
<joint name="front_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_right_wheel"/>
<origin xyz="0.045 -0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="front_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_left_wheel"/>
<origin xyz="0.045 0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="rear_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_right_wheel"/>
<origin xyz="-0.045 -0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="rear_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_left_wheel"/>
<origin xyz="-0.045 0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
Complete URDF File
All these components combined result into a URDF file which represents our mobile robot.
<?xml version="1.0"?>
<robot name="ed_robot">
<!-- Base Link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.180 0.076 0.055"/>
</geometry>
<material name="blue">
<color rgba="0.0 0.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.194 0.168 0.1"/>
</geometry>
</collision>
<inertial>
<mass value="0.86"/>
<inertia ixx="0.0029257" ixy="0.0" ixz="0.0"
iyy="0.014314" iyz="0.0"
izz="0.0119673"/>
</inertial>
</link>
<!-- Wheels -->
<link name="front_right_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="front_left_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="rear_right_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<link name="rear_left_wheel">
<visual>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<cylinder radius="0.04" length="0.0425"/>
</geometry>
</collision>
</link>
<!-- Joints connecting wheels to the base -->
<joint name="front_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_right_wheel"/>
<origin xyz="0.045 -0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="front_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="front_left_wheel"/>
<origin xyz="0.045 0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="rear_right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_right_wheel"/>
<origin xyz="-0.045 -0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="rear_left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="rear_left_wheel"/>
<origin xyz="-0.045 0.06 -0.025" rpy="1.5708 0 0"/>
<axis xyz="0 0 1"/>
</joint>
</robot>
Visualize this robot in rviz
-- insert image --