当前位置：首页 > news >正文

第十六讲、isaaclab中使用任务空间(task-space)控制

news 来源：原创 2025/7/10 10:49:34

0 前言

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

在之前的教程中我们利用关节空间(joint-space)来控制机器人，但是，在很多情况下使用任务空间(task-space)控制机器人更直观。比如若想远程操控机器人，实际上指定末端夹具姿态比指定关节位置更容易。

本教程中，将学习如何使用任务空间(task-space)控制机器人，我们将使用controllers.DifferentialIKController类来跟踪末端执行器姿态命令。

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

运行该程序：

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

注意事项：

任务空间(task-space)控制机器时必须保证坐标系正确。
并行化环境中，不同环境之间只存在一个为一的模拟世界坐标系。
每个环境本身有自己的局部坐标系。

在我们的API中，我们使用以下符号来表示坐标系框架：

仿真世界坐标系 (World Frame, w)：全局静态坐标系，作为整个虚拟仿真环境的绝对参考系，通常位于场景加载时的初始地面中心点
- 固定不变，所有实体位置最终都相对于此坐标系
- Z轴默认垂直向上（遵循Isaac Sim的Z-up约定）
局部环境坐标系 (Local Environment Frame, e)：动态生成的相对坐标系，用于多环境并行训练场景，当使用num_envs > 1时，每个环境实例有自己的局部坐标系。
- 原点通常位于每个环境实例的初始生成位置
- 随环境重置而重新定位（如机器人复位时）
机器人基座坐标系 (Base Frame, b)：固定在机器人基座（通常为底盘或躯干中心）的动坐标系
- 根据机器人USD模型中/Robot/base Prim的位置确定
- 可通过ArticulationCfg配置调整安装点

1 创建IK控制器

DifferentialIKController类，可以机器人达到所需末端执行器姿态所需的关节位置。并且支持不同类型的逆运动学求解器，不同的算法可以通过ik_method参数指定。
官方文档：https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.controllers.html#isaaclab.controllers.DifferentialIKController
https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.controllers.html#isaaclab.controllers.DifferentialIKControllerCfg.ik_method

在本教程中，我们将使用Damped Moore-Penrose Pseudo-Inverse（阻尼摩尔-彭罗斯伪逆）来计算所需的关节位置。此外，由于我们想要追踪所需的末端执行器位姿，因此我们将使用绝对位姿命令模式。

    # 配置控制器参数diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")# DifferentialIKControllerCfg：控制器配置类，定义逆运动学控制器的行为参数。# command_type="pose"：指定控制器接收的目标输入类型为位姿（位置 + 姿态），而非速度或力。# use_relative_mode=False：使用绝对坐标系（相对于世界坐标系），若为 True 则使用相对于当前末端执行器的相对坐标系。# ik_method="dls"：选择逆运动学求解方法。# 实例化控制器diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)# DifferentialIKController：差分逆运动学控制器类，根据配置生成关节控制指令。# num_envs=scene.num_envs：指定并行环境数量（如批量仿真中的多个独立环境）。# device=sim.device：设置计算设备（如 "cuda:0" 或 "cpu"），确保与仿真环境的数据存储位置一致。

2 获取机器人的关节和身体索引

**数据依赖：**差分逆运动学控制器（Differential IK Controller）是一个纯计算模块，不直接访问机器人模型数据。用户需主动提供以下信息：

关节位置：机器人机械臂（非夹爪）的当前关节角度。
末端执行器位姿：末端在全局坐标系中的位置和姿态（[x, y, z, qx, qy, qz, qw]）。
雅可比矩阵：描述末端速度与关节速度关系的矩阵。
注意：需从机器人模型中精准提取机械臂关节和末端数据，避免包含无关部分（如夹爪关节）。

**数据提取：**机器人模型通常包含多个关节和身体（如夹爪、底盘等），但IK控制器仅需特定部分：

关节位置：ArticulationData.joint_pos 包含所有关节的位置，但需过滤出机械臂关节。(
若机器人有 ["arm_joint1", "arm_joint2", "gripper_joint"]，需排除 gripper_joint。)
末端位姿：ArticulationData.body_state_w 包含所有身体的状态，但需仅选择末端执行器。

索引获取:
1、通过名称查找关节或身体的索引（名称拼写错误会导致返回空索引，引发后续逻辑错误。）

find_joints():arm_joint_indices = articulation.find_joints(["arm_joint1", "arm_joint2"])
find_bodies():ee_body_index = articulation.find_bodies(["end_effector"])[0]
2、封装索引解析逻辑，增加名称有效性检查，避免无效索引。（SceneEntityCfg）

from omni.isaac.lab.scene import SceneEntityCfg# 配置机械臂关节和末端执行器
robot_cfg = SceneEntityCfg("robot",joint_names=["arm_joint1", "arm_joint2"],body_names=["end_effector"]
)
# 解析索引
arm_joint_indices = robot_cfg.resolve_joint_indices(articulation)
ee_body_index = robot_cfg.resolve_body_indices(articulation)[0]

	# 配置可视化标记（坐标系标记）frame_marker_cfg = FRAME_MARKER_CFG.copy()  # 复制默认的坐标系标记配置frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)  # 缩小坐标系标记尺寸为原10%（X/Y/Z轴长度均为0.1米）# 在指定路径创建末端执行器当前位置标记（蓝色坐标系）ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")  # 在USD场景的/Visuals/ee_current路径下创建)# 在指定路径创建目标位置标记（红色坐标系）goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")  # 在USD场景的/Visuals/ee_goal路径下创建)# 定义末端执行器的目标位姿序列（位置+四元数姿态）ee_goals = [# 目标1: 位置(0.5, 0.5, 0.7)，姿态绕X轴旋转180度（四元数[0.707, 0, 0.707, 0]）[0.5, 0.5, 0.7, 0.707, 0, 0.707, 0],  # 目标2: 位置(0.5, -0.4, 0.6)，姿态绕Z轴旋转90度（四元数[0.707, 0.707, 0.0, 0.0]）[0.5, -0.4, 0.6, 0.707, 0.707, 0.0, 0.0],  # 目标3: 位置(0.5, 0, 0.5)，姿态绕Y轴旋转180度（四元数[0.0, 1.0, 0.0, 0.0]）[0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0],  ]ee_goals = torch.tensor(ee_goals, device=sim.device)  # 将列表转换为PyTorch张量，并放置在仿真设备（GPU/CPU）上current_goal_idx = 0  # 当前目标索引初始化为0（指向第一个目标）# 创建逆运动学指令缓冲区（shape: [num_envs, action_dim]）ik_commands = torch.zeros(scene.num_envs,  # 环境数量（并行仿真时可能有多个独立环境）diff_ik_controller.action_dim,  # 动作维度（7维：3位置 + 4姿态四元数）device=robot.device  # 设备与机器人数据一致（GPU加速）)ik_commands[:] = ee_goals[current_goal_idx]  # 将所有环境的指令初始化为第一个目标位姿# 根据命令行参数 args_cli.robot 选择机器人类型，并配置对应的关节和末端执行器名称。if args_cli.robot == "franka_panda":robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], # 使用正则表达式匹配所有以 panda_joint 开头的关节（如 panda_joint1, panda_joint2 等），排除夹爪关节。body_names=["panda_hand"])# 指定末端执行器为 Panda 的夹爪（panda_hand 身体）。elif args_cli.robot == "ur10":robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], # 正则表达式 .* 匹配所有关节（假设 UR10 模型没有夹爪，或需要全部关节参与计算）。body_names=["ee_link"])# 指定末端执行器为 UR10 的 ee_link 身体。else:raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")# 解析场景实体索引robot_entity_cfg.resolve(scene)# 调用 SceneEntityCfg.resolve() 方法，根据场景中的实际机器人模型解析关节和身体的索引。# 内部调用 find_joints() 和 find_bodies()，通过正则表达式匹配关节/身体名称。# 解析结果存储在 robot_entity_cfg.joint_ids 和 robot_entity_cfg.body_ids 中。#计算末端执行器的雅可比索引if robot.is_fixed_base:ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1# 固定基座机器人（如 Franka Panda）：# 1 物理引擎（如 Isaac Sim）返回的雅可比矩阵 不包含根身体（root body）。# 2 若末端执行器的身体索引为 N，则其在雅可比矩阵中的索引为 N-1。# 3 示例：假设 panda_hand 的 body_ids[0] 是 8（根身体为 0），则雅可比索引为 8-1=7。else:ee_jacobi_idx = robot_entity_cfg.body_ids[0]# 移动基座机器人（如移动机械臂）# 1 雅可比矩阵包含根身体（如移动底盘），因此直接使用 body_ids[0]。

3 计算机器人指令

STEP1：目标设定阶段

通过 set_command() 设置基座坐标系下的目标位姿，控制器内部存储该指令。
可与控制循环异步执行（例如每100ms更新一次目标）。

# 重置逆运动学控制器内部状态（如积分项、历史误差等）
# 通常在每次开始新任务或目标切换时调用，确保控制器从干净状态开始
diff_ik_controller.reset()# 设置目标末端执行器位姿指令（所有并行环境共享同一目标）
# ik_commands 是形状为 [num_envs, 7] 的张量，包含 [x, y, z, qx, qy, qz, qw]
# 坐标系为机器人基座坐标系（base frame），而非世界坐标系
diff_ik_controller.set_command(ik_commands)

STEP2：状态获取阶段

雅可比矩阵：反映末端速度与关节速度关系的瞬时线性化矩阵，物理引擎实时计算。
坐标系转换：控制器需基于基座坐标系计算误差，因此需将末端位姿从世界坐标系转换到基座坐标系。

# 从物理引擎获取机器人雅可比矩阵数据（实时更新）
# root_physx_view.get_jacobians() 返回形状为 [num_envs, num_bodies, 6, total_dof] 的张量
# 通过索引操作提取特定末端执行器对应的雅可比矩阵：
#   - ee_jacobi_idx : 末端执行器在雅可比矩阵中的索引（已考虑固定基座偏移）
#   - robot_entity_cfg.joint_ids : 机械臂关节的物理引擎索引列表
# 最终 jacobian 形状为 [num_envs, 6, num_arm_joints]
jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids]# 获取末端执行器在世界坐标系中的位姿（位置+四元数姿态）
# body_state_w 形状为 [num_envs, num_bodies, 13]（包含位置、姿态、速度等信息）
# 索引 [:, robot_entity_cfg.body_ids[0], 0:7] 提取第一个末端身体的 [x, y, z, qx, qy, qz, qw]
ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7]# 获取机器人基座（root）在世界坐标系中的位姿
# root_state_w 形状为 [num_envs, 13]，索引 [:, 0:7] 提取基座 [x, y, z, qx, qy, qz, qw]
root_pose_w = robot.data.root_state_w[:, 0:7]# 获取当前机械臂关节角度（仅目标关节，排除夹爪等无关关节）
# joint_pos 形状为 [num_envs, num_arm_joints]
joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids]# 将末端执行器的世界坐标系位姿转换为基座坐标系下的位姿
# 输入：基座位置(root_pos_w)和姿态(root_quat_w)，末端位置(ee_pos_w)和姿态(ee_quat_w)
# 输出：基座坐标系下的末端位置(ee_pos_b)和姿态(ee_quat_b)
# 数学等效：T_base_to_world * T_ee_to_base = T_ee_to_world → 求解 T_ee_to_base
ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pose_w[:, 0:3],  # 基座位置 (x, y, z)root_pose_w[:, 3:7],  # 基座姿态四元数 (qx, qy, qz, qw)ee_pose_w[:, 0:3],    # 末端世界位置ee_pose_w[:, 3:7]     # 末端世界姿态
)

STEP3：逆解计算阶段

调用 compute() 结合当前状态与目标，通过数值优化（如DLS）求解关节角度增量。
内部可能包含阻尼最小二乘、关节限位处理等逻辑。

# 逆运动学核心计算：基于当前状态求解目标关节角度
# 输入：
#   - ee_pos_b : 基座坐标系下的末端当前位置 [num_envs, 3]
#   - ee_quat_b : 基座坐标系下的末端当前姿态 [num_envs, 4]
#   - jacobian : 当前雅可比矩阵 [num_envs, 6, num_arm_joints]
#   - joint_pos : 当前关节角度 [num_envs, num_arm_joints]
# 输出：
#   - joint_pos_des : 期望关节角度 [num_envs, num_arm_joints]
joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)

STEP4：动作应用阶段

将计算的关节角度直接设为目标位置，底层可能通过PD控制器实现跟踪。
write_data_to_sim() 将数据同步到物理引擎，触发下一帧仿真计算。

# 将计算得到的关节角度目标值应用到机器人
# 通过 joint_ids 指定仅更新机械臂关节（不包含夹爪等）
robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)# 将更新后的关节目标值写入物理引擎，准备下一仿真步计算
scene.write_data_to_sim()

0 前言

1 创建IK控制器

2 获取机器人的关节和身体索引

3 计算机器人指令

相关文章：