Why does my robot shake / wobble?

edit

Dear Gazebo community,

I am having a problem with the simulation of a ABB IRB2400 robot in Gazebo. After sending a command to a controller of any link the robot model starts to misbehave.

I took the IRB2400 model from here: https://github.com/culletom/abb_dev/t...

When I use the EffortJointInterface with a controller for every joint the model starts spinning out of control as shown.

Since I wanted to use MoveIt! I changed the EffortJointInterface to PositionJointInterface and the controller to a JointTrajectoryController. Sending a command now makes the model shake a little bit (shown in the following gif). I don't know where this comes from. Is this the fault of the controllers or is the model decription incorrect? Or is this even a problem with gazebo?

Turning the gravity on lets the models shake right from the beginning. I guess that the problem is the controller. Is there a way to manipulate the way the position controller works? If i understood correctly you can't set PID parameters for a position controller. I tried to give PID parameters anyways but that don't seem to have any effect.

I am using Ubuntu 14.04, Gazebo 2.2 and ROS Indigo. Any ideas or hints are much appreciated.

My Controller YAML:

    # Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  

# Controlls the given joints 
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - joint_1
     - joint_2
     - joint_3
     - joint_4
     - joint_5
     - joint_6
  constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
     joint_1: {trajectory: 0.1, goal: 0.1}
     joint_2: {trajectory: 0.1, goal: 0.1}
     joint_3: {trajectory: 0.1, goal: 0.1}
     joint_4: {trajectory: 0.1, goal: 0.1}
     joint_5: {trajectory: 0.1, goal: 0.1}
     joint_6: {trajectory: 0.1, goal: 0.1}
  gains: # Required because we're controlling an effort interface
     joint_1: {p: 1000,  d: 0, i: 0, i_clamp: 1}
     joint_2: {p: 1000,  d: 0, i: 0, i_clamp: 1}
     joint_3: {p: 1000,  d: 0, i: 0, i_clamp: 1}
     joint_4: {p: 1000,  d: 0, i: 0, i_clamp: 1}
     joint_5: {p: 1000,  d: 0, i: 0, i_clamp: 1}
     joint_6: {p: 1000,  d: 0, i: 0, i_clamp: 1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

The URDF

<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/richard/catkin_ws/src/abb_dev/abb_irb2400_gazebo/urdf/irb2400.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="abb_irb2400" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!--  <gazebo>
        <static>false</static>
    </gazebo> -->
  <!-- ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <!-- <robotNamespace>/abb_irb2400</robotNamespace> -->
      <robotNamespace>/</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
  <gazebo reference="base_link">
    <material>Gazebo/Orange</material>
    <turnGravityOff>true</turnGravityOff>
    <!-- org: true -->
  </gazebo>
  <gazebo reference="link_1">
    <material>Gazebo/Orange</material>
    <turnGravityOff>true</turnGravityOff>
    <mu1>0.0</mu1>
    <!-- org: 0.2 -->
    <mu2>0.0</mu2>
    <!-- org: 0.2 -->
  </gazebo>
  <gazebo reference="link_2">
    <material>Gazebo/Orange</material>
    <turnGravityOff>true</turnGravityOff>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
  </gazebo>
  <gazebo reference="link_3">
    <material>Gazebo/Orange</material>
    <turnGravityOff>true</turnGravityOff>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
  </gazebo>
  <gazebo reference="link_4">
    <material ...