ROS Fuerte Gazebo, model desintegrates when using any controller
Hi, I am fighting with this problem for two days. When I load a model of my robot to Gazebo, I am able to put effort to joint and it moves. but when I try to use my joint position controller (or use that PID one that is in Gazebo) model just disintegrates, some links disappear, base dives.
Maybe I am doing something obviously wrong while defining urdf for my model?
I hope some of you had the same problem. I am rather new to Gazebo, although I made trough PR2 controller tutorials and based on that I've built my own.
I attach urdf for model below, it is based on SolidWorks file conversion.
<robot
name="dlon2">
<link
name="base_link">
<inertial>
<origin
xyz="-0.000538 -0.010977 -0.0035996"
rpy="0 0 0" />
<mass
value="0.038882" />
<inertia
ixx="1.4734E-05"
ixy="1.5355E-22"
ixz="-1.1613E-21"
iyy="2.1266E-05"
iyz="1.0497E-07"
izz="8.7373E-06" />
</inertial>
<visual>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/podstawa.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/podstawa.STL" />
</geometry>
</collision>
</link>
<link
name="kciuk">
<inertial>
<origin
xyz="-0.00068428 -0.018063 3.1225E-17"
rpy="0 0 0" />
<mass
value="0.0033556" />
<inertia
ixx="6.2442E-07"
ixy="-8.8668E-09"
ixz="-5.6869E-25"
iyy="6.7857E-08"
iyz="5.1285E-23"
izz="5.7531E-07" />
</inertial>
<visual>
<material name="violet">
<color rgba="1 0 .8 1"/>
</material>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/kciuk.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/kciuk.STL" />
</geometry>
</collision>
</link>
<joint
name="kciukzlacze"
type="revolute">
<origin
xyz="0 -0.018118 -0.0036859"
rpy="-0.03604 0 0" />
<parent
link="base_link" />
<child
link="kciuk" />
<axis
xyz="-1 0 0" />
<limit effort="100" velocity="1000" lower="-1.57079632679" upper="1.57079632679"/>
</joint>
<gazebo reference="kciukzlacze">
<turnGravityOff>true</turnGravityOff>
</gazebo>
<gazebo reference="kciukzlacze">
<stopKd value="1.0" />
<stopKp value="1000000.0" />
</gazebo>
<link
name="kciukkoniec">
<inertial>
<origin
xyz="0.0043157 -0.018063 4.3368E-19"
rpy="0 0 0" />
<mass
value="0.0033556" />
<inertia
ixx="6.2442E-07"
ixy="-8.8668E-09"
ixz="-2.2489E-24"
iyy="6.7857E-08"
iyz="-6.3176E-23"
izz="5.7531E-07" />
</inertial>
<visual>
<material name="violet" />
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/kciukkoniec.STL" />
</geometry>
<material
name="">
<color
rgba="0.79216 0.81961 0.93333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://dlonurdf/meshes/kciukkoniec.STL" />
</geometry>
</collision>
</link>
<joint
name="zlaczekonieckciuka"
type="revolute">
<origin
xyz="0 -0.038372 0"
rpy="0.011337 0 0" />
<parent
link="kciuk" />
<child
link ...
Hi, could you write your answer as an answer, not an edit, and then select it as resolved, so people will know when they see the question.