Model of Katana arm jumps around violently in Gazebo

edit

Pretty sure it's controller instability. I tried two things

• reduced time step size from 1ms to 0.7ms
• changed i-terms to 0 and reduced d-term by half.

more specifically, this controller yaml worked for me:

katana_arm_controller:
  type: "robot_mechanism_controllers/JointTrajectoryActionController"
  joints:
    - katana_motor1_pan_joint
    - katana_motor2_lift_joint
    - katana_motor3_lift_joint
    - katana_motor4_lift_joint
    - katana_motor5_wrist_roll_joint
  gains:
    katana_motor1_pan_joint: {p: 6.0, d: 4.0, i: 0.0, i_clamp: 0.3}
    katana_motor2_lift_joint: {p: 4.0, d: 3.0, i: 0.0, i_clamp: 0.3}
    katana_motor3_lift_joint: {p: 2.0, d: 1.5, i: 0.0, i_clamp: 0.3}
    katana_motor4_lift_joint: {p: 1.0, d: 1.0, i: 0.0, i_clamp: 0.3}
    katana_motor5_wrist_roll_joint: {p: 0.1, d: 0.05, i: 0.00, i_clamp: 0.01}
  joint_trajectory_action_node:
    joints:
      - katana_motor1_pan_joint
      - katana_motor2_lift_joint
      - katana_motor3_lift_joint
      - katana_motor4_lift_joint
      - katana_motor5_wrist_roll_joint
    constraints:
      goal_time: 0.6
      katana_motor1_pan_joint:
        goal: 0.02
      katana_motor2_lift_joint:
        goal: 0.02
      katana_motor3_lift_joint:
        goal: 0.02
      katana_motor4_lift_joint:
        goal: 0.02
      katana_motor5_wrist_roll_joint:
        goal: 0.02

Either one of these changes made the system stable. Alternatively, if the inertia values are correct and you wanted to keep the controller gains unmodified, you can play around with viscous joint damping by adding dynamics block to your urdf, e.g.:

<joint ...>
  ...
  <dynamics damping="1"/>
</joint>