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