controller_manager:
  ros__parameters:
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    io_and_status_controller:
      type: ur_controllers/GPIOController

    speed_scaling_state_broadcaster:
      type: ur_controllers/SpeedScalingStateBroadcaster

    force_torque_sensor_broadcaster:
      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster

    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    scaled_joint_trajectory_controller:
      type: ur_controllers/ScaledJointTrajectoryController

    forward_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController

    forward_position_controller:
      type: position_controllers/JointGroupPositionController

    forward_gripper_position_controller:
      type: position_controllers/JointGroupPositionController

    robotiq_gripper_controller:
      type: position_controllers/GripperActionController

    robotiq_gripper_joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    robotiq_activation_controller:
      type: robotiq_controllers/RobotiqActivationController

speed_scaling_state_broadcaster:
  ros__parameters:
    state_publish_rate: 100.0
    tf_prefix: "$(var tf_prefix)"

io_and_status_controller:
  ros__parameters:
    tf_prefix: "$(var tf_prefix)"

force_torque_sensor_broadcaster:
  ros__parameters:
    sensor_name: $(var tf_prefix)tcp_fts_sensor
    state_interface_names:
      - force.x
      - force.y
      - force.z
      - torque.x
      - torque.y
      - torque.z
    frame_id: $(var tf_prefix)tool0
    topic_name: ft_data


joint_trajectory_controller:
  ros__parameters:
    joints:
      - $(var tf_prefix)shoulder_pan_joint
      - $(var tf_prefix)shoulder_lift_joint
      - $(var tf_prefix)elbow_joint
      - $(var tf_prefix)wrist_1_joint
      - $(var tf_prefix)wrist_2_joint
      - $(var tf_prefix)wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }


scaled_joint_trajectory_controller:
  ros__parameters:
    joints:
      - $(var tf_prefix)shoulder_pan_joint
      - $(var tf_prefix)shoulder_lift_joint
      - $(var tf_prefix)elbow_joint
      - $(var tf_prefix)wrist_1_joint
      - $(var tf_prefix)wrist_2_joint
      - $(var tf_prefix)wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor

forward_velocity_controller:
  ros__parameters:
    joints:
      - $(var tf_prefix)shoulder_pan_joint
      - $(var tf_prefix)shoulder_lift_joint
      - $(var tf_prefix)elbow_joint
      - $(var tf_prefix)wrist_1_joint
      - $(var tf_prefix)wrist_2_joint
      - $(var tf_prefix)wrist_3_joint
    interface_name: velocity

forward_position_controller:
  ros__parameters:
    joints:
      - $(var tf_prefix)shoulder_pan_joint
      - $(var tf_prefix)shoulder_lift_joint
      - $(var tf_prefix)elbow_joint
      - $(var tf_prefix)wrist_1_joint
      - $(var tf_prefix)wrist_2_joint
      - $(var tf_prefix)wrist_3_joint

forward_gripper_position_controller:
  ros__parameters:
    joints:
      - $(var tf_prefix)finger_joint


robotiq_gripper_controller:
  ros__parameters:
    joint: $(var tf_prefix)finger_joint
    use_effort_interface: true
    use_speed_interface: true

robotiq_gripper_joint_trajectory_controller:
  ros__parameters:
    joints:
     - $(var tf_prefix)finger_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }

robotiq_activation_controller:
  ros__parameters:
    default: true