# Controller Manager configuration
controller_manager:
  ros__parameters:
    update_rate: 500  # Hz

    # Define the available controllers
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    imu_sensor_broadcaster:
      type: imu_sensor_broadcaster/IMUSensorBroadcaster

#    leg_pd_controller:
#      type: leg_pd_controller/LegPdController
#
#    unitree_guide_controller:
#      type: unitree_guide_controller/UnitreeGuideController

    ocs2_quadruped_controller:
      type: ocs2_quadruped_controller/Ocs2QuadrupedController

imu_sensor_broadcaster:
  ros__parameters:
    sensor_name: "imu_sensor"
    frame_id: "imu_link"

#leg_pd_controller:
#  ros__parameters:
#    joints:
#      - FR_abad_joint
#      - FR_hip_joint
#      - FR_knee_joint
#      - FL_abad_joint
#      - FL_hip_joint
#      - FL_knee_joint
#      - RR_abad_joint
#      - RR_hip_joint
#      - RR_knee_joint
#      - RL_abad_joint
#      - RL_hip_joint
#      - RL_knee_joint
#
#    command_interfaces:
#      - effort
#
#    state_interfaces:
#      - position
#      - velocity
#
#unitree_guide_controller:
#  ros__parameters:
#    command_prefix: "leg_pd_controller"
#    joints:
#      - FR_abad_joint
#      - FR_hip_joint
#      - FR_knee_joint
#      - FL_abad_joint
#      - FL_hip_joint
#      - FL_knee_joint
#      - RR_abad_joint
#      - RR_hip_joint
#      - RR_knee_joint
#      - RL_abad_joint
#      - RL_hip_joint
#      - RL_knee_joint
#
#    command_interfaces:
#      - effort
#      - position
#      - velocity
#      - kp
#      - kd
#
#    state_interfaces:
#      - effort
#      - position
#      - velocity
#
#    feet_names:
#      - FR_foot
#      - FL_foot
#      - RR_foot
#      - RL_foot
#
#    imu_name: "imu_sensor"
#    base_name: "base_link"
#
#    imu_interfaces:
#      - orientation.w
#      - orientation.x
#      - orientation.y
#      - orientation.z
#      - angular_velocity.x
#      - angular_velocity.y
#      - angular_velocity.z
#      - linear_acceleration.x
#      - linear_acceleration.y
#      - linear_acceleration.z


ocs2_quadruped_controller:
  ros__parameters:
    update_rate: 500  # Hz
    default_kd: 1.0

    joints:
      - FL_hip_joint
      - FL_thigh_joint
      - FL_calf_joint
      - FR_hip_joint
      - FR_thigh_joint
      - FR_calf_joint
      - RL_hip_joint
      - RL_thigh_joint
      - RL_calf_joint
      - RR_hip_joint
      - RR_thigh_joint
      - RR_calf_joint

    command_interfaces:
      - effort
      - position
      - velocity
      - kp
      - kd

    state_interfaces:
      - effort
      - position
      - velocity

    feet:
      - FL_foot
      - FR_foot
      - RL_foot
      - RR_foot


    imu_name: "imu_sensor"
    base_name: "base"

    imu_interfaces:
      - orientation.w
      - orientation.x
      - orientation.y
      - orientation.z
      - angular_velocity.x
      - angular_velocity.y
      - angular_velocity.z
      - linear_acceleration.x
      - linear_acceleration.y
      - linear_acceleration.z

    foot_force_name: "foot_force"
    foot_force_interfaces:
      - FL
      - RL
      - FR
      - RR