Planning 规划器

对应目录：demo/planning/，展示如何在 FastSim 中启用规划器并完成逆运动学与运动规划。

示例一览

目录重点入口脚本
01_arm_planningIK + 运动规划 + 轨迹执行arm_planning.py

01：机械臂规划（01_arm_planning）

配置

规划器通过机器人配置启用，以 Curobo 为例：

yaml
robot_cfg_dict:
  franka_panda:
    stereotype: general_robot
    asset_path: assets://robots/franka.usd
    ee_link_name: panda_hand
    use_planner: true
    planner_cfg:
      stereotype: curobo
      lazy_init: true
      robot_config_file: assets://curobo/franka/franka.yml
      world_config_source: stage          # 从 USD Stage 提取障碍物
      world_stage_reference_prim_path: /World/franka_panda

逆运动学（IK）

python
from fastsim.app import FastSim
from fastsim.unisim.scene_manager import SceneManager
from fastsim.utils.pose import Pose

sim = FastSim("demo_config.yaml")
sim.setup_all_before_loop()

# 稳定物理
for _ in range(10):
    sim.step()

robot = SceneManager.get_robot("franka_panda")
planner = robot.get_planner()

ee_pose = Pose(
    position=[0.37523, 0.0, 0.17532],
    quaternion=[0, 1, 0, 0],   # wxyz
)

ik_result = planner.solve_ik(base_frame_ee_pose=ee_pose)
if ik_result["success"]:
    print("IK 成功，关节角：", ik_result["result"]["position"])
else:
    print("IK 失败：", ik_result["status"])

运动规划与执行

python
joint_names = planner.get_plannable_joint_names()
joint_positions = robot.get_joint_position(joint_names)

plan_result = planner.gen_motion_plan(
    start_state={"position": joint_positions, "joint_names": joint_names},
    base_frame_ee_pose=ee_pose,
)

if plan_result["success"]:
    for waypoint in plan_result["result"]["position"]:
        robot.set_joint_position_target(waypoint, joint_names)
        robot.write_data_to_sim()
        sim.step()
else:
    print("规划失败：", plan_result["status"])

在控制线程中使用（推荐）

对于长时间运行的任务，建议将规划与执行放在控制线程中，通过 MotionPlanController 调用（自动封装为 Command）：

python
from fastsim.controllers.motion_plan_controller import MotionPlanController
from fastsim.controllers.robot_controller import RobotController

def planning_loop(sim):
    while sim.is_running():
        result = MotionPlanController.move_robot_to_ee_pose(
            "franka_panda", target_pose, sync=True
        ).unwrap()

sim.add_controller_thread(planning_loop)
sim.start()