r_arm_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityImpedanceController"
  joints:
    - right_arm_0_joint
    - right_arm_1_joint
    - right_arm_2_joint
    - right_arm_3_joint
    - right_arm_4_joint
    - right_arm_5_joint
    - right_arm_6_joint
  gains:
    right_arm_0_joint: &pid_gains
    # for perfect sim: chose p=m/dt with damping=0
      p: 2.5
      i: 0.0
      d: 0.0
    right_arm_1_joint: *pid_gains
    right_arm_2_joint: *pid_gains
    right_arm_3_joint: *pid_gains
    right_arm_4_joint: *pid_gains
    right_arm_5_joint: *pid_gains
    right_arm_6_joint: *pid_gains

l_arm_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityImpedanceController"
  joints:
    - left_arm_0_joint
    - left_arm_1_joint
    - left_arm_2_joint
    - left_arm_3_joint
    - left_arm_4_joint
    - left_arm_5_joint
    - left_arm_6_joint
  gains:
    left_arm_0_joint: *pid_gains
    left_arm_1_joint: *pid_gains
    left_arm_2_joint: *pid_gains
    left_arm_3_joint: *pid_gains
    left_arm_4_joint: *pid_gains
    left_arm_5_joint: *pid_gains
    left_arm_6_joint: *pid_gains

head_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityController"
  joints:
    - neck_shoulder_pan_joint
    - neck_shoulder_lift_joint
    - neck_elbow_joint
    - neck_wrist_1_joint
    - neck_wrist_2_joint
    - neck_wrist_3_joint
  gains:
    neck_shoulder_pan_joint: *pid_gains
    neck_shoulder_lift_joint: *pid_gains
    neck_elbow_joint: *pid_gains
    neck_wrist_1_joint: *pid_gains
    neck_wrist_2_joint: *pid_gains
    neck_wrist_3_joint: *pid_gains

torso_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityController"
  joints:
    - triangle_base_joint
  gains:
    triangle_base_joint: *pid_gains

r_gripper_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityController"
  joints:
    - right_gripper_base_gripper_left_joint
    - right_gripper_base_gripper_right_joint
  gains:
    right_gripper_base_gripper_left_joint: *pid_gains
    right_gripper_base_gripper_right_joint: *pid_gains

l_gripper_vel:
  type: "iai_robot_mechanism_controllers/MultiJointVelocityController"
  joints:
    - left_gripper_base_gripper_left_joint
    - left_gripper_base_gripper_right_joint
  gains:
    left_gripper_base_gripper_left_joint: *pid_gains
    left_gripper_base_gripper_right_joint: *pid_gains
