Robot won't move diff drive nor arm
ROS Kinetic, Gazebo 7.0, Ubuntu 16.04
We have a custom designed mobile platform with 2 differential drive wheels, 4 caster wheels, and 6 DOF arm on top of that base. After running the launch file, gazebo is loaded without any errors, saying that all required plugins and controllers were loaded, all topics are published. When I send manual Twist to base or Position to arm, nothing moves. Although there is a constant slight drift in mobile base position (probably due to wrongly specified friction parameters). I'll provide snippets from various files I am using.
In robot urdf file I have things like this:
Wheel joint
<joint name="Cassis_Left_wheel" type="continuous">
<parent link="Cassis"/>
<child link="Left_wheel"/>
<origin rpy="3.14159 0 1.57079" xyz="-3.84092 4.53357 0.0975"/>
<axis xyz="1 0 0"/>
<limit effort="1.0" lower="0.0" upper="0.0" velocity="0.0"/>
</joint>
Arm joint
<joint name="Cassis_M_link1" type="revolute">
<parent link="Cassis"/>
<child link="M_link1"/>
<origin rpy="3.14159 0 1.57079" xyz="-3.41592 4.16774 0.93378"/>
<axis xyz="0 0 1"/>
<limit effort="1.0" lower="-3.14" upper="3.14" velocity="0.0"/>
</joint>
Wheel transmission
<transmission name="Cassis_Left_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Cassis_Left_wheel">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor_left">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
Arm joint transmission
<transmission name="Cassis_M_link1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Cassis_M_link1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="base_motor">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
Gazebo control plugin
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<!-- <robotNamespace>/telega</robotNamespace> -->
</plugin>
</gazebo>
My launch file is this
<launch>
<!-- Load the Telega sdf model into parameter server -->
<param name="robot_description" textfile="$(find telega_description)/sdf/Mobile_platform.urdf" />
<rosparam command="load" file="$(find telega_pkg)/src/robot_parameters.yaml" />
<!-- Turn on joint state publisher gui control -->
<param name="use_gui" value="True" />
<!-- Start Gazebo with an empty world -->
<!-- Defines Rviz custom config file for telega -->
<arg name="rvizconfig" default="$(find telega_pkg)/src/telega.rviz" />
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen"
args="joint_state_controller
arm_base_position_controller
sholder_position_controller
elbow_position_controller
wrist1_position_controller
wrist2_position_controller
wrist3_position_controller
mobile_base_controller"
/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- Spawn a Telega in Gazebo, taking the description from the parameter server -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model Mobile_platform" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
And finally my robot_parameters.yaml file
joint_state_controller:
type: "joint_state_controller/JointStateController"
publish_rate: 50
arm_base_position_controller:
type: "position_controllers/JointPositionController"
joint: "Cassis_M_link1"
pid: {p: 100.0, i: 0.01, d: 10.0}
sholder_position_controller:
type: "position_controllers/JointPositionController"
joint: "M_link1_M_link2"
pid: {p: 100.0, i: 0.01, d: 10.0}
elbow_position_controller:
type: "position_controllers/JointPositionController"
joint: "M_link2_M_link3"
pid: {p: 100.0, i: 0.01, d: 10.0}
wrist1_position_controller:
type: "position_controllers/JointPositionController"
joint: "M_link3_M_link4"
pid: {p: 100.0, i: 0.01, d: 10.0}
wrist2_position_controller:
type: "position_controllers/JointPositionController"
joint: "M_link4_M_link5"
pid: {p: 100.0, i: 0.01, d: 10.0}
wrist3_position_controller:
type: "position_controllers/JointPositionController"
joint: "M_link5_M_link6_Gripper"
pid: {p: 100.0, i: 0.01, d: 10.0}
mobile_base_controller:
type : "diff_drive_controller/DiffDriveController"
left_wheel : 'Cassis_Left_wheel'
right_wheel : 'Cassis_Right_wheel'
publish_rate: 50.0 # default: 50
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter
wheel_separation : 0.730
wheel_radius : 0.19
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 0.25
# Base frame_id
base_frame_id: Cassis #default: Cassis
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : true
max_velocity : 1.0 # m/s
min_velocity : -0.5 # m/s
has_acceleration_limits: true
max_acceleration : 0.8 # m/s^2
min_acceleration : -0.4 # m/s^2
has_jerk_limits : true
max_jerk : 5.0 # m/s^3
angular:
z:
has_velocity_limits : true
max_velocity : 1.7 # rad/s
has_acceleration_limits: true
max_acceleration : 1.5 # rad/s^2
has_jerk_limits : true
max_jerk : 2.5 # rad/s^3
So, when I send Twist to /mobilebasecontroller/cmdvel, or Float to /armbasepositioncontroller/command, model in Gazebo doesn't move at all. Do I need to load some additional gazebo plugins into urdf file? Or maybe it's just incorrect friction or PID parameters?
Second problem is that there is no tf transformation for those 4 passive caster wheels, although they are defined as continuous. That manifests in their displacement in Rviz.
Any ideas?
Asked by der.sonic on 2017-02-15 04:52:35 UTC
Answers
I can offer some debugging tips.
- Are you sure your joints are correctly specified? You can visualize joints in the Gazebo GUI.
- Similarly, are the collision and inertia elements correct? You can also visualize these in Gazebo.
- Can you move the joints using Gazebo graphical joint controller (pull out panel on the right side of the GUI)?
If everything looks good up to this point, then I would assume something is wrong with the ros control parameters.
Asked by nkoenig on 2017-02-15 14:07:30 UTC
Comments
The problem was in joint's effort and velocity variables, they were too low. I've change them for all joints to be 100 and 10, and now I can move every joint by publishing on the corresponding topics.
Second problem was because joint_state_controller publishes states only for joints that have transmission and hardware interface tags. Adding that to caster wheel joints fixes the problem. Small side note, transmission tags are not necessary when you run separate joint state publisher node.
Asked by der.sonic on 2017-02-15 23:49:30 UTC
Comments