Robotics StackExchange | Archived questions

Wheels rotating without any order given

Hello everyone, I know there are a lot of topics about this kind of problems but I read through a lot of them without getting an answer to my problem. So basically, I have a robot which is made of a baselink (collada file, but it's similar to a rectangle), 2 caster wheels to the front and 2 rotating wheels to the back. So what happens when I launch the simulation, the robot rotates around the Z axis without any cmdvel being applied to the wheel or for or whatever. I'm really lost here guys so I request your help. I also add that when I turn on collisions view on Gazebo, it shows that every part of my robot is in collision, even with the self-collide turned on false, and the floor itslef is also in collision (it's the basic empty world). I join you my xacro file so that you may help me, and thanks everyone for reading to this point. I'm using Gazebo 7.0.

robot.xacro:

<?xml version='1.0'?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
        <!-- Put here the robot description -->

<xacro:include filename="$(find robot_description)/urdf/robot.gazebo" />
<xacro:include filename="$(find robot_description)/urdf/macros.xacro" />


<xacro:property name="PI" value="3.1415926535897931"/>

<xacro:property name="chassis_Height" value="0.002"/>
<xacro:property name="chassis_Length" value="0.310"/>
<xacro:property name="chassis_Width" value="0.240"/>
<xacro:property name="chassis_Mass" value="0.375"/>

<xacro:property name="casterRadius" value="0.0075"/>
<xacro:property name="casterMass" value="0.2"/>

<xacro:property name="wheelWidth" value="0.03175"/>
<xacro:property name="wheelRadius" value="0.03175"/>
<xacro:property name="wheelMass" value="0.2"/>


<link name="base_link"/>
<!-- Link chassis -->
<link name='chassis_link'>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://robot_description/meshes/chassis_milieu.dae"/>
    </geometry>
  </collision>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://robot_description/meshes/chassis_milieu.dae"/>
    </geometry>
  </visual>
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="${chassis_Mass}"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <box_inertia m="${chassis_Mass}" x="${chassis_Length}" y="${chassis_Width}"           z="${chassis_Height}"/>
  </inertial>
</link>

<!-- Joint chassis sur base_link -->
<joint name="base_joint" type="fixed">
  <origin xyz="0 0 100" rpy="0 0 0"/> <!-- The 100 here was to make sure my robot isn't actually conflicting with the floor -->
  <parent link="base_link"/>
  <child link="chassis_link"/>
</joint>

<!-- Macro (link+joint) roues -->
<wheel lr="left" tY="1"/>
<wheel lr="right" tY="-1"/>

<!-- Link roue libre gauche  -->
<link name="caster_wheel_left">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${casterRadius}"/>
    </geometry>
  </collision>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${casterRadius}"/>
    </geometry>
  </visual>
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="${casterMass}"/>
    <sphere_inertia m="${casterMass}" r="${casterRadius}"/>
  </inertial>
</link>

<joint name="caster_wheel_left_joint" type="continuous">
 <origin xyz="0.15 0.05 ${-casterRadius-chassis_Height/2}" rpy="0 0 0"/>
 <axis xyz="1 1 1" rpy="0 0 0"/>
  <parent link="chassis_link"/>
  <child link="caster_wheel_left"/>
</joint>

<!-- Link roue libre droite  -->
<link name="caster_wheel_right">
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${casterRadius}"/>
    </geometry>
  </collision>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <sphere radius="${casterRadius}"/>
    </geometry>
     </visual>
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="${casterMass}"/>
    <sphere_inertia m="${casterMass}" r="${casterRadius}"/>
  </inertial>
</link>

<joint name="caster_wheel_right_joint" type="continuous">
 <origin xyz="0.15 -0.05 ${-casterRadius-chassis_Height/2}" rpy="0 0 0"/>
 <axis xyz="1 1 1" rpy="0 0 0"/>
  <parent link="chassis_link"/>
  <child link="caster_wheel_right"/>
</joint>
</robot>

robot.gazebo:
<?xml version="1.0"?>
<robot>
  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <legacyMode>false</legacyMode>
      <alwaysOn>true</alwaysOn>
      <updateRate>10</updateRate>
      <leftJoint>left_wheel_hinge</leftJoint>
      <rightJoint>right_wheel_hinge</rightJoint>
      <wheelSeparation>${wheelWidth+chassis_Width}</wheelSeparation>
      <wheelDiameter>${wheelRadius}</wheelDiameter>
      <torque>0</torque>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
  </gazebo>
</robot>

macros.xacro:
<?xml version='1.0'?>
<robot>

<!-- macro pour les inerties -->

<macro name="cylinder_inertia" params="m r h">
  <inertia  ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
    iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
    izz="${m*r*r/2}"
  />
</macro>

<macro name="box_inertia" params="m x y z">
  <inertia  ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
    iyy="${m*(x*x+z*z)/12}" iyz = "0"
    izz="${m*(x*x+z*z)/12}"
  />
</macro>

<macro name="sphere_inertia" params="m r">
  <inertia  ixx="${2*m*r*r/5}" ixy = "0" ixz = "0"
    iyy="${2*m*r*r/5}" iyz = "0"
    izz="${2*m*r*r/5}"
  />
</macro>

<!-- Macro des roues -->

<macro name="wheel" params="lr tY">

<link name="${lr}_wheel">
  <collision>
    <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
    <geometry>
      <cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
    </geometry>
  </collision>

  <visual>
    <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
    <geometry>
      <cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
    </geometry>
  </visual>

  <inertial>
    <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
    <mass value="${wheelMass}"/>
    <cylinder_inertia m="${wheelMass}" r="${wheelRadius}" h="${wheelWidth}"/>
  </inertial>
</link>

<gazebo reference="${lr}_wheel">
  <mu1 value="1.0"/>
  <mu2 value="1.0"/>
  <kp  value="1.0" />
  <kd  value="1.0" />
  <fdir1 value="1 0 0"/>
</gazebo>

<joint name="${lr}_wheel_hinge" type="continuous">
  <parent link="chassis_link"/>
  <child link="${lr}_wheel"/>
  <origin xyz="-0.015 ${tY*wheelWidth/2+tY*0.1} 0" rpy="0 0 0" />
  <axis xyz="0 1 0" rpy="0 0 0" />
  <limit effort="100" velocity="100"/>
  <joint_properties damping="0.0" friction="0.0"/>
</joint>
</macro>

Asked by DonutsSauvage on 2018-04-25 11:23:05 UTC

Comments

Answers

There are some things you can try to minimize the drift.

1) Increase the kp value of your wheels like so:

<gazebo reference="${lr}_wheel">
  <mu1 value="1.0"/>
  <mu2 value="1.0"/>
  <kp  value="10000000.0" />
  <kd  value="1.0" />
  <fdir1 value="1 0 0"/>
</gazebo>

This is the value I use and I get minimal drift. (though my robot is 4-wheeled)

2) Lower your robot's center of mass by lowering the body's intertia z position.

3) Increase real_time_update rate in your physics settings (while lowering max_step_size) if your computer can handle it.

Though because it is a vehicle with two casters some of the drift may be unavoidable considering one side of the robot is very easy to move.

Asked by Raskkii on 2018-04-27 02:37:02 UTC

Comments