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

    force_mode_controller:
      type: ur_controllers/ForceModeController

    freedrive_mode_controller:
      type: ur_controllers/FreedriveModeController

    passthrough_trajectory_controller:
      type: ur_controllers/PassthroughTrajectoryController

    tcp_pose_broadcaster:
      type: pose_broadcaster/PoseBroadcaster

    passthrough_trajectory_controller:
      type: ur_controllers/PassthroughTrajectoryController

    ur_configuration_controller:
      type: ur_controllers/URConfigurationController

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)"

ur_configuration_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: 10.0
    allow_partial_joints_goal: false 
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 5.0
      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)elbow_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_1_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_2_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_3_joint: { trajectory: 0.5, goal: 0.5 }


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: 10.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 5.0
      $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)elbow_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_1_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_2_joint: { trajectory: 0.5, goal: 0.5 }
      $(var tf_prefix)wrist_3_joint: { trajectory: 0.5, goal: 0.5 }
    speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor

passthrough_trajectory_controller:
  ros__parameters:
    tf_prefix: "$(var tf_prefix)"
    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
    state_interfaces:
      - position
      - velocity
    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

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

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

tcp_pose_broadcaster:
  ros__parameters:
    frame_id: $(var tf_prefix)base
    pose_name: $(var tf_prefix)tcp_pose
    tf:
      child_frame_id: $(var tf_prefix)tool0_controller