Robotics StackExchange | Archived questions

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

Answers