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
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