第十七讲、Isaaclab中使用操作空间控制器

0 前言

官方教程：https://isaac-sim.github.io/IsaacLab/main/source/tutorials/05_controllers/run_osc.html
Isaacsim+Isaaclab安装：https://blog.csdn.net/m0_47719040/article/details/146389391?spm=1001.2014.3001.5502

有时候，仅使用differential IK控制器来控制机器人末端执行器的姿态是不够的。比如，我们可能希望在任务空间中强制执行非常特定的姿态跟踪动态误差，使用关节力/扭矩指令来驱动机器人，或者在特定方向上施加接触力，同时控制其他方向的运动（例如，用布擦拭桌面）。在这类任务中，可以使用空间控制器(OSC)。

本教程中，我们将学习如何使用一个OSC控制机器人。我们将使用controllers.OperationalSpaceController类施加一个垂直于倾斜墙面的恒定力，同时在所有其他方向上跟踪所需的末端执行器姿态。

教程对应的脚本为run_osc.py在scripts/tutorials/05_controllers目录下。

运行该程序：

进入安装 isaac lab 时创建的conda虚拟环境
在该环境下进入 isaac sim文件夹中运行source setup_conda_env.sh
终端中输入./isaaclab.sh -p scripts/tutorials/05_controllers/run_osc.py --num_envs 128运行你的代码。

在这里插入图片描述

1 创建操作空间控制器

OperationalSpaceController类计算机器人在任务空间中同时进行运动和力控制的联合努力/扭矩。
官方文档：https://isaac-sim.github.io/IsaacLab/main/source/tutorials/05_controllers/run_osc.html

任务坐标系定义：
控制器的任务空间参考坐标系可自由指定（默认机器人基座坐标系）。但是，在某些情况下，定义相对于不同坐标系的目标坐标可能更容易。在这种情况下，应在set_command方法的current_task_frame_pose_b参数中提供此任务参考系相对于机器人基座坐标系的姿态。

场景示例:在墙面接触任务中，定义与墙面平行的坐标系（如Z轴垂直墙面），此时：

力控制：只需在Z轴方向施加接触力。
运动控制：在x/y平面调整位置，绕z轴旋转调整姿态。

# 假设已定义墙面坐标系到基座的变换矩阵 T_wall_to_base
controller.set_command(
    target_pose=target_in_wall_frame, 
    current_task_frame_pose_b=T_wall_to_base
)

控制类型与轴向选择:
1、目标类型（target_types）

“pose_abs”：绝对位姿控制（基座坐标系）
“pose_rel”：相对当前末端位姿的增量控制。
“wrench_abs”：绝对力/力矩控制。

还可以组合选择

# 同时进行位姿和力控制
cfg.target_types = ["pose_abs", "wrench_abs"]

2、控制轴向（motion_control_axes_task 和 force_control_axes_task）

两个参数为6维二进制列表（0/1），分别对应位置（XYZ）和旋转（RxRyRz）。
同一轴向不能同时激活运动和力控制（互补）。

# 在墙面坐标系中：
# - 运动控制XY平面位置和绕Z轴旋转（索引0,1,5）
# - 力控制Z轴方向（索引2）
cfg.motion_control_axes_task = [1, 1, 0, 0, 0, 1]  # X, Y, _, _, _, Rz
cfg.force_control_axes_task  = [0, 0, 1, 0, 0, 0]  # Z方向力控制

运动控制参数：
刚度与阻尼比（motion_control_stiffness 和 motion_damping_ratio_task）

标量（所有轴向相同值）或6维列表（各轴向独立）。
临界阻尼公式：kd = 2 * sqrt(kp)（当impedance_mode="variable_kp"时自动计算阻尼）。

cfg.impedance_mode = "variable_kp"  # 允许通过命令实时调整刚度
cfg.motion_stiffness_limits_task = (min_kp, max_kp)  # 刚度范围约束

力控制模式：
开环控制：直接设置目标力，无反馈

cfg.contact_wrench_stiffness_task = None

闭环控制：通过刚度参数调节力跟踪：

# 仅线性轴（XYZ）有效，旋转轴刚度被忽略
cfg.contact_wrench_stiffness_task = [100, 100, 100, 0, 0, 0]  # Z轴刚度100 N/m

动力学解耦与重力补偿：
惯性解耦（inertial_dynamics_decoupling）：利用机器人惯性矩阵解耦任务空间加速度，提升动态响应精度。

cfg.inertial_dynamics_decoupling = True  # 启用完全惯性解耦
cfg.partial_inertial_dynamics_decoupling = False  # 不忽略平移-旋转耦合

重力补偿（gravity_compensation）：若机器人模型已包含重力项（如仿真中启用重力），需关闭此选项：

cfg.gravity_compensation = False  # 假设仿真中机器人已处理重力

冗余机器人的零空间控制
冗余自由度机器人的关节运动不影响末端位姿（零空间）。

cfg.nullspace_control = "position"  # 吸引关节至中立位置
cfg.nullspace_stiffness = 10.0       # 零空间刚度
cfg.nullspace_damping_ratio = 0.7    # 阻尼比

本教程中的参数配置如下：

# 创建操作空间控制器配置
osc_cfg = OperationalSpaceControllerCfg(
    target_types=["pose_abs", "wrench_abs"],  # 绝对位姿 + 绝对力控制
    motion_control_axes_task=[1, 1, 0, 1, 1, 1],  # 控制XY位置和绕Z旋转（假设墙面Z轴）
    force_control_axes_task=[0, 0, 1, 0, 0, 0],   # Z轴力控制
    motion_control_stiffness=[200, 200, 0, 50, 50, 50],  # XY高刚度，旋转适中
    motion_damping_ratio_task=1.0,  # 临界阻尼（kd=2*sqrt(kp)）
    impedance_mode="variable_kp",   # 允许动态调整刚度
    inertial_dynamics_decoupling=True,  # 启用惯性解耦
    partial_inertial_dynamics_decoupling=False,
    gravity_compensation=False,     # 仿真中已禁用重力
    nullspace_control="position",   # 零空间关节位置控制
    nullspace_stiffness=10.0,       # 零空间刚度
    nullspace_damping_ratio=0.7
)

2 更新机器人的状态

与前一节任务空间(task-space)控制类似，OSC 实现是一个仅用于计算的类。因此，需要提供机器人的必要信息，包括机器人的雅可比矩阵、质量/惯性矩阵、末端执行器位姿、速度、接触力（均在根坐标系中），以及关节位置和速度。此外，如果需要，用户还应提供重力补偿矢量和零空间关节位置目标。

# 函数功能：更新机器人状态信息，为操作空间控制器（OSC）提供必要的输入数据
def update_states(
    sim: sim_utils.SimulationContext,
    scene: InteractiveScene,
    robot: Articulation,
    ee_frame_idx: int,
    arm_joint_ids: list[int],
    contact_forces,
):
    """Update the robot states.

    Args:
        sim: (SimulationContext) 仿真上下文对象，包含时间步长等物理参数
        scene: (InteractiveScene) 交互场景对象，管理多环境
        robot: (Articulation) 机器人刚体关节树对象
        ee_frame_idx: (int) 末端执行器在body_state_w中的索引
        arm_joint_ids: (list[int]) 机械臂关节的物理引擎索引列表
        contact_forces: (ContactSensor) 接触力传感器对象

    Returns:
        返回OSC控制器所需的11个关键参数，所有张量均为batch格式（shape: [num_envs, ...]）
    """
    # ====================== 动力学参数获取 ======================
    # 获取雅可比矩阵（世界坐标系）
    # 注意：对于固定基座机器人，物理引擎返回的雅可比矩阵索引需要-1（排除根身体）
    ee_jacobi_idx = ee_frame_idx - 1  # 计算物理引擎中的雅可比索引
    # 形状：[num_envs, 6, num_arm_joints] (6=3平移+3旋转)
    jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids]
    
    # 获取质量矩阵（仅机械臂关节部分）
    # 形状：[num_envs, num_arm_joints, num_arm_joints]
    mass_matrix = robot.root_physx_view.get_generalized_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids]
    
    # 获取重力补偿力矩（已考虑机器人质量分布）
    # 形状：[num_envs, num_arm_joints]
    gravity = robot.root_physx_view.get_gravity_compensation_forces()[:, arm_joint_ids]

    # ================= 雅可比矩阵坐标系转换（世界→基座） =================
    jacobian_b = jacobian_w.clone()  # 创建副本避免修改原始数据
    # 计算基座旋转矩阵的逆（将世界坐标系转换到基座坐标系）
    root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w))  # shape: [num_envs, 3, 3]
    # 转换平移部分雅可比：J_lin_base = R_base^T * J_lin_world
    jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :])
    # 转换旋转部分雅可比：J_rot_base = R_base^T * J_rot_world
    jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :])

    # ===================== 末端执行器位姿计算 =====================
    # 世界坐标系下的基座位姿
    root_pos_w = robot.data.root_pos_w  # 位置 [num_envs, 3]
    root_quat_w = robot.data.root_quat_w  # 四元数 [num_envs, 4]
    # 世界坐标系下的末端位姿
    ee_pos_w = robot.data.body_pos_w[:, ee_frame_idx]  # 末端位置
    ee_quat_w = robot.data.body_quat_w[:, ee_frame_idx]  # 末端姿态
    # 将末端位姿转换到基座坐标系
    ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
    # 拼接完整位姿张量
    root_pose_w = torch.cat([root_pos_w, root_quat_w], dim=-1)  # [num_envs, 7]
    ee_pose_w = torch.cat([ee_pos_w, ee_quat_w], dim=-1)        # [num_envs, 7]
    ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1)        # [num_envs, 7]

    # ===================== 末端执行器速度计算 =====================
    # 世界坐标系下的末端线速度+角速度 [num_envs, 6]
    ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :]  
    # 基座速度（线速度 + 角速度）[num_envs, 6]
    root_vel_w = robot.data.root_vel_w  
    # 计算末端相对于基座的速度（世界坐标系）
    relative_vel_w = ee_vel_w - root_vel_w  
    # 转换到基座坐标系：线速度
    ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3])  # [num_envs, 3]
    # 转换到基座坐标系：角速度
    ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6])  # [num_envs, 3]
    # 拼接完整速度张量 [num_envs, 6]
    ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1)

    # ====================== 接触力处理 ======================
    ee_force_w = torch.zeros(scene.num_envs, 3, device=sim.device)  # 初始化接触力
    sim_dt = sim.get_physics_dt()  # 获取物理时间步长
    contact_forces.update(sim_dt)  # 更新接触传感器数据
    # 数据平滑处理：取最近4步的平均值，再取三个接触面中的最大值（假设仅有一个有效接触）
    # net_forces_w_history形状：[num_envs, history_len=4, num_surfaces=3, 3]
    ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_forces_w_history, dim=1), dim=1)
    # 简化假设：直接使用世界坐标系的力（实际应根据任务坐标系转换）
    ee_force_b = ee_force_w  # 此处应优化为基座坐标系转换

    # ===================== 关节状态获取 =====================
    # 仅提取机械臂关节（排除夹爪等）
    joint_pos = robot.data.joint_pos[:, arm_joint_ids]  # [num_envs, num_arm_joints]
    joint_vel = robot.data.joint_vel[:, arm_joint_ids]  # [num_envs, num_arm_joints]

    # 返回所有必要参数（用于OSC控制器计算）
    return (
        jacobian_b,        # 基座坐标系雅可比矩阵 [num_envs, 6, num_arm_joints]
        mass_matrix,       # 质量矩阵 [num_envs, num_arm_joints, num_arm_joints]
        gravity,           # 重力补偿力矩 [num_envs, num_arm_joints]
        ee_pose_b,         # 基座坐标系末端位姿 [num_envs, 7]
        ee_vel_b,          # 基座坐标系末端速度 [num_envs, 6]
        root_pose_w,       # 世界坐标系基座位姿 [num_envs, 7]
        ee_pose_w,         # 世界坐标系末端位姿 [num_envs, 7]
        ee_force_b,        # 基座坐标系接触力（当前简化处理）[num_envs, 3]
        joint_pos,         # 关节角度 [num_envs, num_arm_joints]
        joint_vel,         # 关节速度 [num_envs, num_arm_joints]
    )

3 计算机器人指令

命令坐标系转换
- 位姿控制 (“pose_abs”)：通过 subtract_frame_transforms 将基座坐标系下的目标位姿转换到任务帧坐标系。例如，若任务帧是墙面坐标系，则目标位置将表示为相对于墙面的坐标。
- 力控制 (“wrench_abs”)：假设力命令已在任务帧中定义（如Z轴垂直墙面），无需转换。
OSC命令设置
- 任务帧位姿传递：current_task_frame_pose_b 告知OSC任务帧相对于基座的位置，用于内部计算误差。
- 当前末端位姿：提供实时反馈，用于计算位姿误差和速度误差。
力矩计算参数
- 质量矩阵与惯性解耦：启用 inertial_dynamics_decoupling 时，质量矩阵用于解耦任务空间加速度，提升动态响应。
- 零空间控制：nullspace_joint_pos_target 吸引关节至中立位置（如 joint_centers），避免极限位置。
力控制实现
- 开环 vs 闭环：若 contact_wrench_stiffness_task 未设置，为开环力控；若设置刚度值，则闭环调整力误差。
- 接触力测量：ee_force_b 来自接触传感器，需平滑处理（如示例中的四步平均）。

# 将目标命令从基座坐标系转换到任务帧（如墙面坐标系）
def convert_to_task_frame(
    osc: OperationalSpaceController, 
    command: torch.tensor, 
    ee_target_pose_b: torch.tensor
):
    """将基座坐标系下的命令转换为任务帧坐标系下的命令
    
    Args:
        osc: 操作空间控制器实例，包含配置信息（如目标类型）
        command: 原始命令张量，形状为 [num_envs, command_dim]
        ee_target_pose_b: 末端在基座坐标系下的目标位姿（用于定义任务帧）

    Returns:
        command_task: 转换到任务帧后的命令张量
        task_frame_pose_b: 任务帧在基座坐标系下的位姿（用于后续计算）
    """
    command_task = command.clone()  # 避免修改原始命令
    task_frame_pose_b = ee_target_pose_b.clone()  # 任务帧位姿（例如墙面坐标系）

    cmd_idx = 0  # 命令张量的当前处理位置索引
    # 遍历所有目标类型（如 ["pose_abs", "wrench_abs"]）
    for target_type in osc.cfg.target_types:
        if target_type == "pose_abs":
            # 提取当前位姿命令部分（位置+四元数姿态）
            target_pos_b = command_task[:, cmd_idx : cmd_idx+3]  # [num_envs, 3]
            target_quat_b = command_task[:, cmd_idx+3 : cmd_idx+7]  # [num_envs, 4]
            # 将位姿从基座坐标系转换到任务帧坐标系
            # 数学等效：T_task^base * T_target^task = T_target^base → 求T_target^task
            target_pos_task, target_quat_task = subtract_frame_transforms(
                task_frame_pose_b[:, :3],   # 任务帧位置（基座坐标系）
                task_frame_pose_b[:, 3:7],  # 任务帧姿态（基座坐标系）
                target_pos_b, 
                target_quat_b
            )
            # 更新命令张量中的位姿部分
            command_task[:, cmd_idx : cmd_idx+3] = target_pos_task
            command_task[:, cmd_idx+3 : cmd_idx+7] = target_quat_task
            cmd_idx += 7  # 移动索引到下个命令部分
        elif target_type == "wrench_abs":
            # 力/力矩命令已在任务帧中定义，无需转换
            cmd_idx += 6  # 力控制为6维（3力 + 3力矩）
        else:
            raise ValueError(f"未定义的目标类型：{target_type}")

    return command_task, task_frame_pose_b

# ====================== OSC命令设置与力矩计算 ======================
# 重置控制器内部状态（如积分项、历史误差）
osc.reset()

# 将目标命令转换到任务帧坐标系
command_task, task_frame_pose_b = convert_to_task_frame(
    osc, 
    command=command_raw,  # 原始命令（基座坐标系）
    ee_target_pose_b=ee_target_pose_b  # 任务帧在基座下的位姿（如墙面坐标系）
)

# 设置OSC命令（任务帧坐标系下的命令 + 当前末端位姿 + 任务帧位姿）
osc.set_command(
    command=command_task,  # 转换后的命令（任务帧）
    current_ee_pose_b=ee_pose_b,  # 当前末端在基座下的位姿（来自update_states）
    current_task_frame_pose_b=task_frame_pose_b  # 任务帧在基座下的位姿
)

# 计算关节力矩（核心运算）
joint_efforts = osc.compute(
    jacobian_b=jacobian_b,            # 基座坐标系下的雅可比矩阵
    current_ee_pose_b=ee_pose_b,      # 当前末端基座位姿
    current_ee_vel_b=ee_vel_b,        # 当前末端基座速度
    current_ee_force_b=ee_force_b,    # 当前末端基座接触力（简化处理）
    mass_matrix=mass_matrix,          # 质量矩阵（仅机械臂关节）
    gravity=gravity,                  # 重力补偿力矩
    current_joint_pos=joint_pos,      # 当前关节角度
    current_joint_vel=joint_vel,      # 当前关节速度
    nullspace_joint_pos_target=joint_centers  # 零空间目标关节位置（如限位中点）
)

# 将计算的关节力矩应用到机器人
robot.set_joint_effort_target(
    joint_efforts,          # 计算得到的力矩 [num_envs, num_arm_joints]
    joint_ids=arm_joint_ids  # 仅控制机械臂关节（排除夹爪）
)
# 将数据写入仿真环境，准备下一步物理计算
robot.write_data_to_sim()