# =============================================================================
# devices/sim/gazebo/config/controllers.yaml
#
# Gazebo 里 controller_manager 的控制器配置。由 arm.sim.gazebo.xacro 的系统插件
# 通过 <parameters> 指过来（路径由启动器按仓库根拼成绝对路径）。
#
# -----------------------------------------------------------------------------
# 硬约束：控制器名不能改
# -----------------------------------------------------------------------------
#   应用层（arm_application）按 rizon_arm_controller/follow_joint_trajectory
#   发轨迹，真机、mock、moveit 三个后端都是这个名字。仿真沿用同名，
#   应用层代码一行都不用改 —— 这是"同一套接口、换后端"的核心契约。
#   想让仿真改名，等于让应用层感知后端，那就退回到"每个后端一套代码"了。
#
# -----------------------------------------------------------------------------
# 关节名必须是 sim_ 前缀
# -----------------------------------------------------------------------------
#   与 arm.sim.urdf.xacro 的 arm_prefix:=sim 对应。
#   这里写错（比如写成 Rizon4-063989_joint1）的症状：
#   controller_manager 起来了，但控制器报
#     "Joint 'xxx' not found in the robot model"
#   而且 ros2 control list_hardware_interfaces 里明明有 7 个关节 ——
#   因为硬件接口是按 sim_ 前缀注册的，两边名字对不上。
# =============================================================================

controller_manager:
  ros__parameters:
    # 仿真步进与控制器更新率的比例。Gazebo 的物理步长在 world 文件里（1ms），
    # 控制器跑 100Hz（10ms）—— 物理比控制细十倍，接触力的瞬态才不会被采丢。
    # 两者是独立的：调这里不会改变物理精度，只是控制器下发的频率。
    update_rate: 100

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    # 名字与真机一致，勿改（见文件头说明）
    rizon_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

# --- 关节状态广播 -------------------------------------------------------------
# TF 与 /joint_states 的来源。robot_state_publisher 靠 /joint_states 算 TF，
# 少了它 RViz / 跟踪节点全都查不到 tcp 的位置。
joint_state_broadcaster:
  ros__parameters: {}

# --- 机械臂轨迹控制器 ---------------------------------------------------------
rizon_arm_controller:
  ros__parameters:
    joints:
      - sim_joint1
      - sim_joint2
      - sim_joint3
      - sim_joint4
      - sim_joint5
      - sim_joint6
      - sim_joint7

    # 只下发位置。Gazebo 侧 position 可写；应用层也不做力矩控制
    # （柔顺下压是靠应用层按力反馈改位置目标实现的，不是靠力矩接口）。
    command_interfaces:
      - position

    # velocity 要读回来：handeye 的"稳定判定"、柔顺下压的限速都依赖关节速度。
    state_interfaces:
      - position
      - velocity

    # false = 轨迹必须包含全部 7 个关节才接受。
    # 应用层发的都是全关节轨迹，设 false 能在漏关节时直接报错而不是补零乱动。
    allow_partial_joints_goal: false

    # 开环：Gazebo 是理想执行器，不需要靠状态反馈纠正，开环反而更平滑。
    open_loop_control: true

    constraints:
      # 与官方 joint_limits.yaml 的 soft 限位对齐（真机软限位）。
      # 不设的话 JTC 只按硬限位卡，仿真里会走到真机不敢走的姿态。
      stopped_velocity_tolerance: 0.05