第十七讲、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_worldjacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :])# 转换旋转部分雅可比：J_rot_base = R_base^T * J_rot_worldjacobian_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^tasktarget_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_taskcommand_task[:, cmd_idx+3 : cmd_idx+7] = target_quat_taskcmd_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()