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

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

    imu_sensor_broadcaster:
      type: imu_sensor_broadcaster/IMUSensorBroadcaster

    unitree_guide_controller:
      type: unitree_guide_controller/UnitreeGuideController

    ocs2_quadruped_controller:
      type: ocs2_quadruped_controller/Ocs2QuadrupedController

    rl_quadruped_controller:
      type: rl_quadruped_controller/LeggedGymController

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

ocs2_quadruped_controller:
  ros__parameters:
    update_rate: 200  # Hz
    robot_pkg: "lite3_description"
    default_kd: 2.0
    joints:
      - FL_HipX
      - FL_HipY
      - FL_Knee
      - FR_HipX
      - FR_HipY
      - FR_Knee
      - HL_HipX
      - HL_HipY
      - HL_Knee
      - HR_HipX
      - HR_HipY
      - HR_Knee

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

    state_interfaces:
      - effort
      - position
      - velocity

    feet:
      - FL_FOOT
      - FR_FOOT
      - HL_FOOT
      - HR_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

#    estimator_type: "odom"
    foot_force_name: "foot_force"
    foot_force_interfaces:
      - FL_foot_force
      - HL_foot_force
      - FR_foot_force
      - HR_foot_force

unitree_guide_controller:
  ros__parameters:
    update_rate: 200  # Hz
#    stand_kp: 30.0
    stand_kd: 1.0
    joints:
      - FR_HipX
      - FR_HipY
      - FR_Knee
      - FL_HipX
      - FL_HipY
      - FL_Knee
      - HR_HipX
      - HR_HipY
      - HR_Knee
      - HL_HipX
      - HL_HipY
      - HL_Knee

    down_pos:
      - 0.0
      - -1.22
      - 2.61
      - 0.0
      - -1.22
      - 2.61
      - 0.0
      - -1.22
      - 2.61
      - 0.0
      - -1.22
      - 2.61

    stand_pos:
      - 0.0
      - -0.732
      - 1.361
      - 0.0
      - -0.732
      - 1.361
      - 0.0
      - -0.732
      - 1.361
      - 0.0
      - -0.732
      - 1.361

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

    state_interfaces:
      - effort
      - position
      - velocity

    feet_names:
      - FR_FOOT
      - FL_FOOT
      - HR_FOOT
      - HL_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