在机器人实际控制中我们面对的指令往往基于笛卡尔空间如机械臂末端的绝对位姿而非单纯的关节空间位置。从目标位姿反解出关节角度即逆运动学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_graphFalse, max_attempts10, enable_finetune_trajoptTrue, time_dilation_factor1.0, ) # 运动规划的身份机器人模型、碰撞设置、精度参数、障碍物 self.motion_gen_config MotionGenConfig.load_from_robot_config( self.robot_cfg, # 机器人配置文件 self.world_cfg, # 障碍物 self.tensor_args, interpolation_dt0.01, collision_checker_typeCollisionCheckerType.MESH, collision_cache{obb: 3000, mesh: 3000}, use_cuda_graphTrue, self_collision_checkTrue, num_trajopt_seeds12, num_graph_seeds12, optimize_dtTrue, ) self.motion_gen MotionGen(self.motion_gen_config) # warmup 会预先编译 CUDA graph第一次规划会比较慢之后就快了 self.motion_gen.warmup(warmup_js_trajoptFalse) # 单独的 IK 求解器 self.ik_config IKSolverConfig.load_from_robot_config( self.robot_cfg, None, rotation_threshold0.05, position_threshold0.005, num_seeds128, self_collision_checkTrue, tensor_argsself.tensor_args, use_cuda_graphTrue, ) 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_pathself.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( positionself.tensor_args.to_device(ee_translation_goal), quaternionself.tensor_args.to_device(ee_orientation_goal), ) # 把 Isaac 的关节状态搬到 GPU 上构造成 cuRobo 的 JointState cu_js JointState( positionself.tensor_args.to_device(sim_js.positions), velocityself.tensor_args.to_device(sim_js.velocities) * 0.0, accelerationself.tensor_args.to_device(sim_js.velocities) * 0.0, jerkself.tensor_args.to_device(sim_js.velocities) * 0.0, joint_namesself.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_poseTrue, # 前三位Roll左右翻滚, Pitch上下翻滚, Yaw左右转向 后面三位xyz hold_vec_weightself.motion_gen.tensor_args.to_device([1, 0, 0, 0, 0, 0]) 这里就是限制roll其他五个维度不限制#方法2 self.grasp_metric PoseCostMetric.create_grasp_approach_metric( offset_position0.1, # 预抓取距离 linear_axis2, # 沿z轴接近 tstep_fraction0.6, # 前60%自由 → 后40%约束减少末端突变增强对准 project_to_goal_frameTrue, # True代表在Goal / 夹爪坐标系, False代表Robot Base坐标系。 )方法1比较简单这里重点讲解方法2offset_position0.1表示在目标位姿的 Local Z 方向构造一个距离目标 10 cm 的预抓取点Offset Pose。TrajOpt 会把它作为一个软约束引导夹爪沿 Local Z 接近目标但不会保证轨迹一定经过这个点也不会在这个点停留。linear_axis20、1、2分别代表x、y、z轴阅读源码可以得知linear_axis2就是等同于hold_vec_weight[1, 1, 1, 1, 1, 0]代表允许z轴方向是自由的tstep_fraction0.6表示前60%自由后40%施加约束。例如夹爪运动到目标pose需要100个轨迹点tstep_fraction0.6表示前60个轨迹没有约束后面40个轨迹点施加hold_vec_weight[1, 1, 1, 1, 1, 0]约束。注意offset_position作用于空间tstep_fraction作用于时间二者没有必然联系project_to_goal_frameTrueTrue代表是Goal / 夹爪坐标系, False代表Robot Base坐标系。通常来说在夹爪坐标系中Z轴代表夹爪的前进方向下图在isaacsim也可以进行查看确认同时阅读源码之后方法2还可以进行自定义self.grasp_metric PoseCostMetric( hold_partial_poseTrue, hold_vec_weightself.motion_gen.tensor_args.to_device( [1.0, 1.0, 1.0, 0.2, 0.2, 0.0] # 不完全锁死XY只是给予较大的约束 ), offset_positionself.motion_gen.tensor_args.to_device([0.0, 0.0, 0.1]), offset_tstep_fraction0.6, project_to_goal_frameTrue, ) # 通过这种方式 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, orientationik求解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_configretract_config, seed_configseed_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, graspFalse) # 拼上夹爪状态 robot_view.set_joint_position_targets( action, joint_indicesdefault_dof_indices ) world.step(renderFalse) # 推进物理需要画面时再单独 render()这里又回到了上一章节中讨论过的step问题在机械臂的 Drive 参数不那么完美的情况下用step(renderFalse)推进物理、再单独render()出画面会比直接step()更接近真实的物理表现。6. 坐标系转化最后一个坑是坐标系。AnyGrasp 也好、我们自己指定的目标也好给出来的目标位姿一般是在世界坐标系下的但是 cuRobo 规划时用的是机器人 base link 坐标系。所以在调用plan之前需要把目标位姿从世界系变换到机器人系。变换的逻辑就是一次标准的位姿求逆再左乘注意 Isaac Sim 的四元数约定是wxyz而scipy的Rotation用的是xyzw