Robotics StackExchange | Archived questions

How to make ROS differential drive robot move forward with teleop?

Hi. I'm new to ROS and Gazebo simulation. I tried to create a differential drive robot for various experiments such sa SLAM etc.

I have set my joint to rotate on the y-axis as setting them on x or z makes them move weird. But when I try to make my robot move forward using teleop, the wheels move vertically instead of horizontally.

Tested on Ubuntu 18.04 LTS, ROS Melodic, Gazebo 9 and 20.04 LTS, ROS Noetic, Gazebo 11

Here is the relevant part of my urdf:

 <link name="base_link">
 </link>

<joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="chassis"/>
</joint>

<link name="chassis">

    <inertial>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <mass value="25"/>
        <inertia ixx="3.002" ixy="0.0" ixz="0.0" iyy="4.011" iyz="0.0" izz="2.8465"/>
    </inertial>
    <visual name="chassis_visual">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <geometry>
            <box size="0.962 0.664 1" />
        </geometry>
    </visual>
    <collision name="chassis_collision">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <geometry>
            <box size="0.962 0.664 1" />
        </geometry>
    </collision>
</link>

<link name="front_left_wheel">
    <inertial>
        <origin xyz="0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
   <visual name="front_left_wheel_visual">
        <origin xyz="0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="front_left_wheel_collision">
        <origin xyz="0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="front_right_wheel">
    <inertial>
        <origin xyz="0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="front_right_wheel_visual">
        <origin xyz="0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="front_right_wheel_collision">
        <origin xyz="0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="rear_left_wheel">
    <inertial>
        <origin xyz="-0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="rear_left_wheel_visual">
        <origin xyz="-0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="rear_left_wheel_collision">
        <origin xyz="-0.3 0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="rear_right_wheel">
    <inertial>
        <origin xyz="-0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="rear_right_wheel_visual">
        <origin xyz="-0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="rear_right_wheel_collision">
        <origin xyz="-0.3 -0.232 -0.525" rpy="1.5707 0.0 0.0"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<joint name="front_left_wheel_hinge" type="continuous">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="front_left_wheel"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="100" velocity="100"/>
</joint>

<joint name="front_right_wheel_hinge" type="continuous">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="front_right_wheel"/>
    <axis xyz="0.0 1.0 0.0"/>
    <limit effort="100" velocity="100"/>
</joint>

<joint name="rear_left_wheel_hinge" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="rear_left_wheel"/>
    <axis xyz="0.0 1.0 0.0"/>
    <dynamics friction="0.0" />
    <limit lower="0.0" upper="0.0" effort="100" velocity="100"/>
</joint>

<joint name="rear_right_wheel_hinge" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="rear_right_wheel"/>
    <axis xyz="0.0 1.0 0.0"/>
    <dynamics friction="0.0" />
    <limit lower="0.0" upper="0.0" effort="100" velocity="100"/>
</joint>

<gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/test_robot</robotNamespace> -->
    </plugin>
</gazebo>

<transmission name="chassis_to_front_left_wheel_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_left_wheel_hinge">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<transmission name="chassis_to_front_right_wheel_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor2">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_right_wheel_hinge">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <updateRate>20</updateRate>
        <leftJoint>front_left_wheel_hinge</leftJoint>
        <rightJoint>front_right_wheel_hinge</rightJoint>
        <wheelSeparation>0.464</wheelSeparation>
        <wheelDiameter>0.2</wheelDiameter>
        <wheelAcceleration>1.0</wheelAcceleration>
        <wheelTorque>20</wheelTorque>
        <commandTopic>/test_robot/diff_drive_controller/cmd_vel</commandTopic>
        <odometryTopic>/test_robot/diff_drive_controller/odom</odometryTopic>
        <odometryFrame>/test_robot/diff_drive_controller/odom</odometryFrame>
        <robotBaseFrame>base_link</robotBaseFrame>
        <odometrySource>1</odometrySource>
        <publishWheelTF>true</publishWheelTF>
        <publishOdom>true</publishOdom>
        <publishWheelJointState>true</publishWheelJointState>
        <legacyMode>false</legacyMode>
    </plugin>
</gazebo>

Asked by jcjmagno on 2020-12-15 23:18:00 UTC

Comments

Answers