Issue using Skid Steer Plugin, specifically when rotating
Hi, the robot I am trying to simulate is a 4 Wheel Differential drive (skid-steer) robot called the Monsterborg. I have used the Skid Steering Drive plugin from the website http://gazebosim.org/tutorials?tut=ros_gzplugins and made the modifications to suit the Monsterborg. When sending a Twist message through cmd_vel
to drive in a straight line the robot does exactly that and there are no problems. The real problem occurs when I send a Twist message for pure angular rotation around the z-axis. The robot moves extremely slow no matter the value I set (I assume it is in rad/s). What seems to be the issue? I will attach the URDF file in case there might be issues in the frictions values (but I don't think friction is the issue because when I send velocity commands to each of the controllers the robot moves the way its supposed to move). I am considering using the equations the developers have used in the gazeborosskidsteerdrive.cpp to then send individual velocity commands to the joint controllers, however I will lose the odometry information publications but its a sacrifice I am willing to make.
<?xml version="1.0"?>
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.16 0.14 0.05"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="3"/>
<inertia ixx="0.007025" ixy="0" ixz="0"
iyy="0.005525" iyz="0" izz=" 0.0113"/>
</inertial>
<collision>
<origin xyz="0 0 0 " rpy="0 0 0"/>
<geometry>
<box size=" 0.16 0.14 0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="base_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/White</material>
</gazebo>
<link name="lb_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.104"/>
<inertia ixx="9.332916666666665e-05" ixy="0" ixz="0"
iyy="9.332916666666665e-05" iyz="0" izz="0.00014332499999999997"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="lb_wheel">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<!-- If a <gazebo> element is used without a reference="" property,
it is assumed the <gazebo> element is for the whole robot model. -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/monsterborg</robotNamespace>
</plugin>
</gazebo>
<joint name="base_to_lb_wheel" type="continuous">
<origin xyz="-0.065 0.0935 0" rpy="-1.5708 0 0"/>
<parent link="base_link"/>
<child link="lb_wheel"/>
<axis xyz="0 0 1"/>
<limit effort="10" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_lb_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<link name="lf_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.104"/>
<inertia ixx="9.332916666666665e-05" ixy="0" ixz="0"
iyy="9.332916666666665e-05" iyz="0" izz="0.00014332499999999997"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="lf_wheel">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<joint name="base_to_lf_wheel" type="continuous">
<origin xyz="0.065 0.0935 0" rpy="-1.5708 0 0"/>
<parent link="base_link"/>
<child link="lf_wheel"/>
<axis xyz="0 0 1"/>
<limit effort="10" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_lf_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<link name="rf_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.104"/>
<inertia ixx="9.332916666666665e-05" ixy="0" ixz="0"
iyy="9.332916666666665e-05" iyz="0" izz="0.00014332499999999997"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="rf_wheel">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<joint name="base_to_rf_wheel" type="continuous">
<origin xyz="0.065 -0.0935 0" rpy="-1.5708 0 0"/>
<parent link="base_link"/>
<child link="rf_wheel"/>
<axis xyz="0 0 1"/>
<limit effort="10" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_rf_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<link name="rb_wheel">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.104"/>
<inertia ixx="9.332916666666665e-05" ixy="0" ixz="0"
iyy="9.332916666666665e-05" iyz="0" izz="0.00014332499999999997"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0525" length="0.05"/>
</geometry>
</collision>
</link>
<gazebo reference="rb_wheel">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<joint name="base_to_rb_wheel" type="continuous">
<origin xyz="-0.065 -0.0935 0" rpy="-1.5708 0 0"/>
<parent link="base_link"/>
<child link="rb_wheel"/>
<axis xyz="0 0 1"/>
<limit effort="10" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="tran4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_rb_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="motor4">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="imu_joint" type="fixed">
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.025"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<gazebo>
<plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
<alwayson>true</alwayson>
<updaterate>1.0</updaterate> <!-- the update rate of the sensor in milliseconds -->
<bodyname>base_link</bodyname> <!-- name of the body the GNSS receiver is attached to -->
<topicname>/fix</topicname> <!-- The simulated GNSS position in WGS84 coordinates (lat, long, alt). -->
<velocitytopicname>/fix_velocity</velocitytopicname>
<drift>5.0 5.0 5.0</drift> <!-- SD of the POSITION drift error -->
<gaussiannoise>0.1 0.1 0.1</gaussiannoise> <!-- SD the additive Gaussian noise added to the position -->
<velocitydrift>0 0 0</velocitydrift>
<velocitygaussiannoise>0.1 0.1 0.1</velocitygaussiannoise>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>_default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>10.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
<initialOrientationAsReference>false</initialOrientationAsReference>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>base_to_lf_wheel</leftFrontJoint>
<rightFrontJoint>base_to_rf_wheel</rightFrontJoint>
<leftRearJoint>base_to_lb_wheel</leftRearJoint>
<rightRearJoint>base_to_rb_wheel</rightRearJoint>
<wheelSeparation>0.09</wheelSeparation>
<wheelDiameter>0.105</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>100</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>true</broadcastTF>
</plugin>
</gazebo>
Bottom line WHY IS THE ROTATION USING THE PLUGIN EXTREMELY SLOW? I am new to ROS so thank you in advance if the issue seems to be from my behalf. I am on Ubuntu 18.04, ROS Melodic, Gazebo-9. Side question I assumed WheelSeperation in the plugin is the distance between the wheel to the centre of the robot)
Asked by moshahin on 2020-07-23 21:29:42 UTC
Comments