Simple heavy differential drive Robot with urdf
Hey, I have a rather simple differential drive robot model. The problem is that even though the wheels are restricted to moving only around one axis they move weirdly as shown in this video.
The main body has a high weight.
How can I make this whole thing stable? How can I make the wheels fixed to the main body, except rotation?
Here is the xarco:
<?xml version="1.0" ?>
<robot name="gocart_v2_gazebo" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="base_link" value="base_link" />
<xacro:property name="pi" value="3.1415926535897931" />
<!-- cannot put these properties within the macro tags -->
<xacro:property name="gocart_bias" value="0.400" />
<xacro:property name="gocart_wheel_radius" value="0.0751" />
<xacro:property name="gocart_caster_radius" value="0.0375" />
<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>
<xacro:macro name="wheel" params="name parent length radius off_x off_y off_z type">
<joint name="${name}" type="${type}">
<parent link="${parent}"/>
<child link="${name}_link"/>
<origin rpy="0 0 0" xyz="${off_x} ${off_y} ${off_z}"/>
<axis xyz="1 0 0"/>
<dynamics damping="1.0" friction="1.0" spring_stiffness="1" />
</joint>
<link name="${name}_link">
<visual>
<geometry>
<cylinder length="${length}" radius="${radius}"/>
</geometry>
<origin rpy="0 ${pi/2} 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<cylinder length="${length}" radius="${radius}"/>
</geometry>
<origin rpy="0 ${pi/2} 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.1" />
<origin rpy="0 ${pi/2} 0" xyz="0 0 0"/>
<cylinder_inertia m="0.1" r="${radius}" h="${length}"/>
</inertial>
</link>
<gazebo reference="${name}">
<!-- <implicitSpringDamper>true</implicitSpringDamper> -->
</gazebo>
<gazebo reference="${name}_link">
<mu1 value="100000.0" />
<mu2 value="100000.0" />
<kp value="100000.0" />
<kd value="0.00001" />
<maxVel value="1.0" />
<minDepth value="0.0001" />
</gazebo>
</xacro:macro>
<xacro:macro name="caster" params="name parent radius off_x off_y off_z type">
<joint name="${name}" type="${type}">
<parent link="${parent}"/>
<child link="${name}_link"/>
<origin rpy="0 0 ${pi/2}" xyz="${off_x} ${off_y} ${off_z}"/>
<dynamics damping="1.0" friction="0.0" />
</joint>
<link name="${name}_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${radius}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${radius}"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<sphere_inertia m="0.01" r="${radius}"/>
</inertial>
</link>
<gazebo reference="${name}_link">
<mu1 value="0.0" />
<mu2 value="0.0" />
<kp value="1000000.0" />
<kd value="1.0" />
<maxVel value ...