Revision history [back]

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

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

either one of these changes made the system stable. If the inertia values are correct, you can also add more viscous joint damping.

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

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

either more specifically:

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. If Alternatively, if the inertia values are correct, correct and you wanted to keep the controller gains unmodified, you can also add more play around with viscous joint damping.damping by adding dynamics block to your urdf, e.g.:

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

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: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>