在机器人实际控制中，我们面对的指令往往基于笛卡尔空间（如机械臂末端的绝对位姿），而非单纯的关节空间位置。从目标位姿反解出关节角度（即逆运动学，IK）只是第一步，更为关键的挑战在于：如何在此基础上，规划出一条既满足物理约束、又保证执行安全的运动轨迹，即运动规划（Motion Planning）。这要求轨迹不仅在空间上不能与桌面、周围物体发生碰撞，还必须规避机器人与自身的干涉，本质上是一个计算量巨大的复杂优化问题。

为解决上述痛点，NVIDIA 推出了 cuRobo——一个深度重构的运动规划库。其核心技术突破在于，将逆运动学、碰撞检测以及轨迹优化等核心模块，全程运行在 GPU 上，并通过 CUDA 技术实现了深度并行化与加速。这种“全 GPU 化”的设计显著提高了计算效率，使得单条运动轨迹的规划耗时能够压缩在几十毫秒的极低量级。

作为 NVIDIA 生态的一员，cuRobo 与 Isaac Sim 模拟器天然适配且协同流畅。在实际应用中，它通过解耦双核心组件来运作：MotionGen 负责完整的运动规划流程（由目标位姿到生成无碰撞的关节轨迹），而 IKSolver 则专注于纯逆运动学求解。开发者通常可以基于 Planner 类进行统一封装与管理。此外，该开源库也提供了与 MPLib、PyRoki 等其他主流运动库的接口。

cuRobo官方链接：https://curobo.org/

1. cuRobo 的调用

cuRobo 的核心对象有两个，MotionGen 负责完整的运动规划（给一个目标位姿，生成一条无碰撞的关节轨迹），IKSolver 负责纯粹的逆运动学（给一个目标位姿，生成一组关节角，不管轨迹）。我们一般会把它们封装在一个 Planner 类里统一管理。

首先是初始化。cuRobo 通过一份 robot config（描述机器人的运动学、碰撞球、关节限制等）和一份 world config（描述环境中的障碍物）来构建求解器：

from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.state import JointState
from curobo.util.usd_helper import UsdHelper
from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.wrap.reacher.motion_gen import (
    MotionGen,
    MotionGenConfig,
    MotionGenPlanConfig,
)

from omni.isaac.core.utils.stage import get_current_stage


class CuroboPlanner:
    def __init__(self, robot_cfg: dict, robot_prim_path: str) -> None:
        self.robot_prim_path = robot_prim_path
        self.robot_cfg = robot_cfg
        self.tensor_args = TensorDeviceType()

        # UsdHelper 是 cuRobo 与 Isaac Sim 之间的桥梁，
        # 它可以直接读取当前的 USD Stage，把里面的物体转换成障碍物
        self.usd_helper = UsdHelper()
        self.usd_helper.load_stage(get_current_stage())

        # 一开始世界里没有障碍物，后面用 update() 从场景里同步
        self.world_cfg = WorldConfig()

        # 运动规划的"要求"：尝试次数、图搜索开关、姿态约束
        self.plan_config = MotionGenPlanConfig(
            enable_graph=False,
            max_attempts=10,
            enable_finetune_trajopt=True,
            time_dilation_factor=1.0,
        )
        # 运动规划的"身份"：机器人模型、碰撞设置、精度参数、障碍物
        self.motion_gen_config = MotionGenConfig.load_from_robot_config(
            self.robot_cfg,  # 机器人配置文件
            self.world_cfg,  # 障碍物
            self.tensor_args,
            interpolation_dt=0.01,
            collision_checker_type=CollisionCheckerType.MESH,
            collision_cache={"obb": 3000, "mesh": 3000},
            use_cuda_graph=True,
            self_collision_check=True,
            num_trajopt_seeds=12,
            num_graph_seeds=12,
            optimize_dt=True,
        )
        self.motion_gen = MotionGen(self.motion_gen_config)
        # warmup 会预先编译 CUDA graph，第一次规划会比较慢，之后就快了
        self.motion_gen.warmup(warmup_js_trajopt=False)

        # 单独的 IK 求解器
        self.ik_config = IKSolverConfig.load_from_robot_config(
            self.robot_cfg,
            None,
            rotation_threshold=0.05,
            position_threshold=0.005,
            num_seeds=128,
            self_collision_check=True,
            tensor_args=self.tensor_args,
            use_cuda_graph=True,
        )
        self.ik_solver = IKSolver(self.ik_config)

        # 关键：这个顺序需要和 cuRobo 内部的关节顺序对齐
        self.ordered_js_names = []
        self.dof_len = 7

这里 robot_cfg 是一份 YAML 配置，用来描述了机器人的 URDF 路径、base link、end-effector link、碰撞球（cuRobo 用一堆球来近似机器人的碰撞体积，这样碰撞检测可以做得更快）、关节限制等等

2. 障碍物/避障

uRobo 并不知道 Isaac Sim 场景里有什么东西，在每次规划之前，把当前 Stage 里的物体作为障碍物同步给它。UsdHelper 提供了直接从 Stage 抓取障碍物的能力

def update(self, ignore_list: list[str] = []) -> None:
    robot_name = self.robot_prim_path.split("/")[-1]
    obstacles = self.usd_helper.get_obstacles_from_stage(
        ignore_substring=[robot_name, "Camera"] + ignore_list,
        reference_prim_path=self.robot_prim_path,
    ).get_collision_check_world()
    self.motion_gen.update_world(obstacles)

有几个细节注意：第一，一定要把机器人自己排除掉，否则机器人会把自己的身体当成障碍物，导致规划永远失败；第二，相机这种没有实体的 prim 也要排掉；第三，被抓取的物体也通常要临时排掉，因为我们恰恰是要让夹爪靠近它。所以 ignore_list 里一般会动态地加上当前要抓取的物体和桌子

3. 关节顺序

Isaac Sim 的 dof_names 顺序和 cuRobo 内部的关节顺序不一定一致。cuRobo 期望接收和返回的关节状态都是按照它自己的 ordered_js_names 来排列的，所以我们需要在两边之间做一次重排。cuRobo 的 JointState 提供了 get_ordered_joint_state() 来做这件事

# 设置 cuRobo 期望的关节顺序（和 robot_cfg 里的 cspace 对应）
planner.ordered_js_names = [
    "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
    "panda_joint5", "panda_joint6", "panda_joint7",
]

在规划的时候，我们从 Isaac Sim 拿到当前的关节状态（顺序是 Isaac 的），构造成 cuRobo 的 JointState，然后用 get_ordered_joint_state() 重排成 cuRobo 的顺序；规划完成之后，再把结果按照 Isaac 的顺序重排回来，这样才能正确地喂给 set_joint_position_targets()

4. 规划轨迹

把上面这些拼起来，一个完整的 plan 方法长这样：

def plan(
    self,
    ee_translation_goal: np.ndarray,   # 目标位置 (3,)，在机器人坐标系下
    ee_orientation_goal: np.ndarray,   # 目标姿态四元数 (4,)，wxyz
    sim_js,                            # Isaac 当前的关节状态
    dof_names: list | None = None,
    grasp: bool = False,
) -> list[np.ndarray] | None:
    # 构造目标位姿
    ik_goal = Pose(
        position=self.tensor_args.to_device(ee_translation_goal),
        quaternion=self.tensor_args.to_device(ee_orientation_goal),
    )
    # 把 Isaac 的关节状态搬到 GPU 上，构造成 cuRobo 的 JointState
    cu_js = JointState(
        position=self.tensor_args.to_device(sim_js.positions),
        velocity=self.tensor_args.to_device(sim_js.velocities) * 0.0,
        acceleration=self.tensor_args.to_device(sim_js.velocities) * 0.0,
        jerk=self.tensor_args.to_device(sim_js.velocities) * 0.0,
        joint_names=self.ordered_js_names if dof_names is None else dof_names,
    )
    # 按 cuRobo 期望的顺序重排
    cu_js = cu_js.get_ordered_joint_state(self.ordered_js_names)

    # plan_single 是核心，返回一系列轨迹
    result = self.motion_gen.plan_single(
        cu_js.unsqueeze(0), ik_goal, self.plan_config.clone()
    )

    if result.success is not None and result.success.item():
        # 拿到插值后的稠密轨迹，再重排回 Isaac 的关节顺序
        cmd_plan = result.get_interpolated_plan()
        cmd_plan = cmd_plan.get_ordered_joint_state(self.raw_js_names)
        position_list = []
        for idx in range(len(cmd_plan.position)):
            joint_positions = cmd_plan.position[idx].cpu().numpy()
            position_list.append(joint_positions[: self.dof_len])
        return position_list   # 一连串关节角，就是轨迹上的每一个点
    else:
        return None            # 规划失败（够不着 / 有碰撞 / 自碰撞）

规划方式对比：

规划方式	输入	适用场景
plan_single	[1 起点，1 目标]	目标唯一确定
plan_goalset	[1 起点 , N 个候选目标]	目标不确定，让求解器自动选最优
plan_batch	[B 个起点，B 个独立目标对应]	多任务并行，需要批量求解，GPU 加速
plan_batch_goalset	[B 组任务，每组 N 个候选目标]	批量 + 多候选（最常用多目标批次求解）

在实际仿真抓取中，利用anygrasp模型生成目标物体多个抓取点之后，经过过滤，往往只剩下几个比较合适，再对这些过滤点进行扩充，然后采用plan_goalset函数，往往都能求解ik；如果只是单个抓取点，大概率会出现ik_fail

下面列举一些常见的规划失败的错误：
规划失败常见错误：

• IK_FAIL：对于给定的目标末端位姿，逆运动学求解器找不到任何有效的关节角度组合。

  常见原因：目标超出工作空间；要求末端同时满足某个位置和某个朝向，但不存在这样的关节配置

• INVALID_START_STATE_SELF_COLLISION： 规划开始前，机器人当前的关节姿态就已经和自己的连杆发生了碰撞

  常见原因：关节配置不合理；碰撞球设置不合理等

• INVALID_START_STATE_ENV_COLLISION：起始状态环境碰撞

  常见原因：环境初始化之后，机器人和物体发生了碰撞

• JOINT_LIMITS_VIOLATION：规划的轨迹中某个关节超出了它的物理限位。

  常见原因：多关节超限，比如机器人的腰部关节 waist_pitch 限制在 -0.7~1.22，规划时可能推到极限附近

带约束的规划

curobo官网详解：https://curobo.org/advanced_examples/3_constrained_planning.html
官方这里给了两种用法：

# 方法1
self.grasp_metric = PoseCostMetric(
    hold_partial_pose=True,
    # 前三位：Roll左右翻滚, Pitch上下翻滚, Yaw左右转向， 后面三位xyz
    hold_vec_weight=self.motion_gen.tensor_args.to_device([1, 0, 0, 0, 0, 0])
    这里就是限制roll，其他五个维度不限制

#方法2
self.grasp_metric = PoseCostMetric.create_grasp_approach_metric(
        offset_position=0.1,      # 预抓取距离
        linear_axis=2,             # 沿z轴接近
        tstep_fraction=0.6,       # 前60%自由 → 后40%约束（减少末端突变，增强对准）
        project_to_goal_frame=True,  # True代表在Goal / 夹爪坐标系, False代表Robot Base坐标系。
     )

方法1比较简单，这里重点讲解方法2

• offset_position=0.1表示在目标位姿的 Local Z 方向构造一个距离目标 10 cm 的预抓取点（Offset Pose）。TrajOpt 会把它作为一个软约束，引导夹爪沿 Local Z 接近目标，但不会保证轨迹一定经过这个点，也不会在这个点停留。
• linear_axis=2：0、1、2分别代表x、y、z轴，阅读源码可以得知，linear_axis=2就是等同于hold_vec_weight=[1, 1, 1, 1, 1, 0]，代表允许z轴方向是自由的
• tstep_fraction=0.6：表示前60%自由，后40%施加约束。例如：夹爪运动到目标pose需要100个轨迹点，tstep_fraction=0.6表示前60个轨迹没有约束，后面40个轨迹点施加hold_vec_weight=[1, 1, 1, 1, 1, 0]约束。注意，offset_position作用于空间，tstep_fraction作用于时间，二者没有必然联系，
• project_to_goal_frame=True，True代表是Goal / 夹爪坐标系, False代表Robot Base坐标系。通常来说，在夹爪坐标系中，Z轴代表夹爪的前进方向，下图在isaacsim也可以进行查看确认
  

同时，阅读源码之后，方法2还可以进行自定义：

self.grasp_metric = PoseCostMetric(
        hold_partial_pose=True,
        hold_vec_weight=self.motion_gen.tensor_args.to_device(
            [1.0, 1.0, 1.0,
             0.2, 0.2, 0.0]  # 不完全锁死XY，只是给予较大的约束
        ),
        offset_position=self.motion_gen.tensor_args.to_device([0.0, 0.0, 0.1]),  
        offset_tstep_fraction=0.6,
        project_to_goal_frame=True,
        )
 # 通过这种方式， hold_vec_weight可以进行修改，而且可以修改值的大小，按照自己实际需求进行设置

方法1和方法2区别

• 方法1：按照指定的姿态约束到达 Goal Pose，更适合运动过程中保持某种姿态。
• 方法2：约束姿态之外，还会额外生成一个 Offset Pose软约束。

例如图中的水果分拣场景，个人采用方法2中的自定义方法，效果很好

fk求解
fk，即Foward Kinematics （逆运动学）, 关节角度----> 末端位姿，这个是唯一的

def fk_single(self, joint_positions: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    joint_positions_tensor = torch.from_numpy(
        joint_positions.astype(np.float32)
    ).to(self.tensor_args.device)
    result = self.ik_solver.fk(joint_positions_tensor.unsqueeze(0))
    position = result.ee_position.cpu().numpy().squeeze()
    orientation = result.ee_quaternion.cpu().numpy().squeeze()
    return position, orientation

ik求解

ik，即Inverse Kinematics （逆运动学），末端位姿----> 关节角度
FK 是正着算，IK 是反着算。IK 比 FK 难得多，因为同一个末端位置可能对应 多组关节角度解 （甚至无解），所以需要求解器。

def ik_single(
    self, target_pose: np.ndarray, cur_joint_positions: np.ndarray
) -> np.ndarray | None:
    retract_config = self.tensor_args.to_device(cur_joint_positions.reshape(1, -1))
    seed_config = self.tensor_args.to_device(cur_joint_positions.reshape(1, 1, -1))
    pose = Pose(
        self.tensor_args.to_device(target_pose[:3]),
        self.tensor_args.to_device(target_pose[3:]),
    )
    ik_result = self.ik_solver_ik.solve_single(
        pose, retract_config=retract_config, seed_config=seed_config
    )
    return ik_result.js_solution.position.cpu().numpy().squeeze()

5. 执行轨迹

规划完成之后，剩下的就是在仿真循环里执行这条轨迹。前面讲过，我们用 set_joint_position_targets() 来驱动手臂，并且每一步都要 step 一下让物理引擎推进：

# trajectory 是 plan() 返回的一连串关节角度
for arm_joints in trajectory:
    action = make_action(arm_joints, grasp=False)  # 拼上夹爪状态
    robot_view.set_joint_position_targets(
        action, joint_indices=default_dof_indices
    )
    world.step(render=False)   # 推进物理；需要画面时再单独 render()

这里又回到了上一章节中讨论过的 step 问题：在机械臂的 Drive 参数不那么完美的情况下，用 step(render=False) 推进物理、再单独 render() 出画面，会比直接 step() 更接近真实的物理表现。

6. 坐标系转化

最后一个坑是坐标系。AnyGrasp 也好、我们自己指定的目标也好，给出来的目标位姿一般是在世界坐标系下的，但是 cuRobo 规划时用的是机器人 base link 坐标系。所以在调用 plan 之前，需要把目标位姿从世界系变换到机器人系。

变换的逻辑就是一次标准的位姿求逆再左乘，注意 Isaac Sim 的四元数约定是 wxyz，而 scipy 的 Rotation 用的是 xyzw，