Controller 控制器

对应目录：demo/controller/，展示如何在控制线程中通过 Command 队列安全地驱动仿真实体。

示例一览

目录	重点	入口脚本
`01_spawn_controller`	运行时动态创建物体	`spawn_controller.py`
`02_robot_arm_control`	多线程关节轨迹控制	`single_gripper_arm_robot_controller.py`
`03_robot_gripper_control`	夹爪开合与状态轮询	`robot_gripper_controller.py`
`04_planning_control`	多机器人规划与跟随	`motion_plan_controller.py`
`05_keyboard_control`	GUI + 键盘实时控制	`keyboard_control.py`
`06_mobile_robot_control`	移动机器人控制	`mobile_robot_control.py`

01：运行时 Spawn（01_spawn_controller）

在控制线程中周期性调用 SpawnableController 动态生成物体：

python

from fastsim.controllers.spawnable_controller import SpawnableController

def spawn_loop(sim):
    while sim.is_running():
        SpawnableController.spawn_with_config(object_config)

SpawnableController.spawn_with_config() 内部封装为 Command，在主线程的下一帧安全执行，不会与物理推进产生竞争。

02：机械臂轨迹控制（02_robot_arm_control）

展示多控制线程并发运行：通过 SpawnableController 获取关节信息，构造轨迹后交由 RobotController 执行。

python

from fastsim.app import FastSim
from fastsim.controllers.spawnable_controller import SpawnableController
from fastsim.controllers.robot_controller import RobotController
import numpy as np

def rotate_arm(robot_name: str):
    joint_names = SpawnableController.control_robot(
        robot_name, "get_joint_names"
    ).unwrap()
    limits = SpawnableController.control_robot(
        robot_name, "get_joint_position_limit"
    ).unwrap()

    for i in range(5):
        if i % 2 == 0:
            traj = np.linspace(limits[0][0], limits[0][1], 100).tolist()
        else:
            traj = np.linspace(limits[0][1], limits[0][0], 100).tolist()
        RobotController.move_robot_along_trajectory(
            robot_name, traj, [joint_names[0]]
        ).unwrap()

sim = FastSim("single_gripper_arm_robot_config.yaml")
sim.add_controller_thread(lambda: rotate_arm("franka_panda_2f85"), thread_name="arm_2f85")
sim.add_controller_thread(lambda: rotate_arm("franka_panda_120s"), thread_name="arm_120s")
sim.start()

多个控制线程同时注册到 FastSim，主循环在每帧 run_all_commands() 时统一消费所有 Command。

03：夹爪控制（03_robot_gripper_control）

展示夹爪开合与状态轮询：

python

from fastsim.controllers.spawnable_controller import SpawnableController

robot_name = "franka_panda_2f85"

# 关闭夹爪
SpawnableController.control_robot(robot_name, "close_gripper").unwrap()

# 轮询夹爪状态（直到完全关闭）
while True:
    stopped = SpawnableController.control_robot(
        robot_name, "is_gripper_stopped"
    ).unwrap()
    if stopped:
        break

# 查询关节状态
velocity = SpawnableController.control_robot(
    robot_name, "get_joint_velocity"
).unwrap()

04：规划控制（04_planning_control）

Leader 线程随机生成目标位姿并规划轨迹，Follower 线程复用轨迹实现多机器人跟随：

python

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

def leader_loop(robot_name, target_pose):
    result = MotionPlanController.gen_motion_plan(
        robot_name=robot_name,
        base_frame_ee_pose=target_pose
    ).unwrap()
    trajectory = result["result"]["position"]
    joint_names = result["result"]["joint_names"]
    RobotController.move_robot_along_trajectory(
        robot_name, trajectory, joint_names
    ).unwrap()
    return trajectory, joint_names

规划器需在机器人配置中启用（use_planner: true），详见规划器示例。

05：键盘与 GUI 控制（05_keyboard_control）

基于 DearPyGUI 构建实时控制面板，以控制线程方式运行，不阻塞仿真主循环：

python

from fastsim.app import FastSim
from fastsim.controllers.robot_controller import RobotController
from fastsim.utils.pose import Pose
import dearpygui.dearpygui as dpg

def solve_and_move():
    ee_pose = Pose(position=ee_position, quaternion=quat)
    RobotController.move_robot_to_ee_pose_no_collision_check(
        robot_name="franka_panda",
        base_frame_ee_pose=ee_pose
    ).unwrap()

def launch_gui():
    # 构建 DearPyGUI 窗口，注册键盘回调 ...
    while dpg.is_dearpygui_running():
        dpg.render_dearpygui_frame()

sim = FastSim("keyboard_control_config.yaml")
sim.add_controller_thread(thread_func=launch_gui, thread_name="gui_thread")
sim.start()

键位映射：

按键	动作
W / S	EE Z 轴 +/−
A / D	EE Y 轴 −/+
Z / X	EE Z 轴 −/+
U / J	Roll +/−
I / K	Pitch +/−
O / L	Yaw +/−

需额外安装：pip install dearpygui

06：移动机器人控制（06_mobile_robot_control）

展示对移动底盘类机器人的速度/位置控制，通过 SpawnableController 调用底盘控制方法，模式与机械臂控制一致。

bash

cd demo/controller/06_mobile_robot_control
python mobile_robot_control.py