What are the most common Mistakes while creating a model from Urdf? How to avoid wobbling?

edit

I'm following the tutorial on "Using a URDF in Gazebo" on the ROS wiki page.I tried to create the robot myself without just copy pasting but while the provided robot seems to work mine just floats and wobbles away. It seems to have something to do with the revolute joints for the gripper. I cannot figure it out why and how to fix it because I cannot see a qualitative difference. A already tried to change the inertial but that did not help. In Rviz everything seems to work fine . Here is my URDF :

<?xml version="1.0"?>
<robot name="P2R2" xmlns:xacro="http://www.ros.org/wiki/xacro" >
 <xacro:property name="main_body_radius" value="0.2"/>
 <xacro:property name="main_body_length" value="0.4"/>
 <xacro:property name="leg_length" value="0.4"/>
 <xacro:property name="leg_width" value="0.2"/>
 <xacro:property name="leg_heigth" value="0.1"/>
 <xacro:property name="head_radius" value="${main_body_radius}"/>
 <xacro:property name="eye_box_size" value="0.05"/>
 <xacro:property name="feet_length" value="0.4"/>
 <xacro:property name="feet_width" value="0.1"/>
 <xacro:property name="feet_heigth" value="0.1"/>
 <xacro:property name="roll_radius" value="0.05"/>
 <xacro:property name="roll_length" value="0.1"/>
 <xacro:property name="roll_offset" value="0.05"/>
 <xacro:property name="gripper_arm_radius" value="0.02"/>
 <xacro:property name="gripper_arm_length" value="0.3"/>
  <xacro:property name="gripper_arm_pos_to_middle_offset" value="0.1"/>
  <xacro:property name="gripper_hand_length" value="0.15"/>
  <xacro:property name="gripper_hand_width" value="0.05"/>
 <xacro:property name="gripper_hand_heigth" value="0.06"/>
<material name="white">
    <color rgba="1 1 1 1"/>
</material>
<material name="blue">
    <color rgba="0 0 1 1"/>
</material>
<material name="orange">
    <color rgba="1 0.5 0 1"/>
</material>

<xacro:macro name="box_inertial" params="heigth width length mass x y z r p yaw">
 <inertial>
  <mass value="${mass}"/>
  <inertia ixx="${(1/12)*mass*(heigth*heigth+length*length)}" ixy="0.0" ixz="0.0" iyy="${(1/12)*mass(width*width+length*length)}" iyz="0.0" izz="${(1/12)*mass*(width*width+heigth*heigth)}"/>
  <origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${yaw}"/>
</inertial>
</xacro:macro>

<xacro:macro name="cylinder_inertial" params="radius length mass x y z r p yaw">
 <inertial>
  <mass value="${mass}"/>
  <inertia ixx="${(1/12)*mass*(3*radius*radius+length*length)}" ixy="0.0" ixz="0.0" iyy="${(1/12)*mass*
    (3*radius*radius+length*length)}" iyz="0.0" izz="${(1/12)*mass*(radius*radius)}"/>
  <origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${yaw}"/>
</inertial>
</xacro:macro>

<xacro:macro name="sphere_inertial" params="radius mass x y z r p yaw">
 <inertial>
  <mass value="${mass}"/>
  <inertia ixx="${(2/5)*mass*(radius*radius)}" ixy="0.0" ixz="0.0" iyy="${(2/5)*mass*(radius*radius)}" iyz="0.0" 
    izz="${(2/5)*mass*(radius*radius)}"/>
  <origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${yaw}"/>
</inertial>
</xacro:macro>
<xacro:macro name="default_inertial" params="mass ">
 <inertial>
  <mass value="${mass}"/>
  <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</xacro:macro>

 <!-- <link name="world"/>
    <visual>
        <geometry>
            <box size="0 ...