运动控制——MPlib

github

doc

MPlib: a Lightweight Motion Planning Library

MPlib is a lightweight python package for motion planning, which is decoupled from ROS and is easy to set up.
With a few lines of python code, one can achieve most of the motion planning functionalities in robot manipulation.

install

pip install mplib

Dependencies

1. Linear algebra library Eigen3, which one can find here and install by following the instructions in the unzipped file called INSTALL. If one is on Ubuntu, one can also install it by sudo apt install libeigen3-dev.

2. C++ extras library Boost by following the instructions here. Similarly, Ubuntu users get a shortcut by sudo apt install libboost-all-dev.

3. Collision detection libccd. Follow the guides here or do sudo apt install libccd-dev.

4. Open Motional Planning Library ompl. Install by following these instructions. Ubuntu users can also do sudo apt install libompl-dev.

5. Point cloud library octomap. Install from source here. Ubuntu users can also do sudo apt install liboctomap-dev.

6. Flexible Collision Library fcl (our fork). This depends on Eigen3 and libccd. Follow the instructions here. (Note: you can also add in the faster hpp-fcl from here)

7. Asset import library assimp. Install instructions here

8. URDF parser urdfdom. Clone from this repo and do the usual cmake+make+make install. Ubuntu users can also do sudo apt install liburdfdom-dev.

9. Rigid-body dynamics library pinocchio. Full installation instructions here. It’s recommended if you install from source as other methods might not configure the paths well.

10. Additional dynamics library orocos_kdl. Install by following the instructions here. Ubuntu users can also do sudo apt install liborocos-kdl-dev.

对比 pinocchio 和 kdl

核心摘要对比

特性维度	Pinocchio	Orocos KDL
​​核心理念​​	​​研究前沿​​，提供最快、最全的算法实现	​​稳定可靠​​，提供经典、成熟的算法实现
​​核心优势​​	​​性能极致​​（稀疏性、自动微分、C++模板元编程）、​​功能全面​​	​​简单易用​​、​​接口清晰​​、​​稳定可靠​​、​​文档良好​​
​​计算性能​​	​​极高​​。利用现代C++和算法优化，比KDL快​​一个数量级​​以上	​​中等​​。满足实时控制需求，但算法实现较传统
​​功能覆盖​​	​​极其广泛​​（动力学、导数、碰撞、几何、AD、…）	​​核心功能​​（运动学、动力学、雅可比、…）
​​依赖复杂度​​	​​较高​​（Eigen, Boost, urdfdom, hpp-fcl, …）	​​极低​​（仅Eigen，可选Python绑定）
​​学习曲线​​	​​较陡峭​​（功能多，现代C++）	​​平缓​​（接口直观，易于上手）
​​应用场景​​	​​高端研究​​、​​优化控制​​、​​运动规划​​、​​需要导数​​的场景	​​教学​​、​​快速原型​​、​​传统控制​​、​​ROS基础应用​​

详细对比分析

1. 起源与设计哲学

• ​​Pinocchio​​:

  • ​​起源​​: 由法国国家信息与自动化研究所（INRIA）的Gepetto团队开发，旨在为机器人模拟和控制提供最先进的算法工具。
  • ​​哲学​​: ​​性能优先​​。大量使用现代C++（C++11/14/17）、模板元编程和Eigen库来最大化计算效率。其设计考虑了​​算法微分（Algorithmic Differentiation, AD）​​，以便于优化问题中的梯度计算。

• ​​Orocos KDL​​:

  • ​​起源​​: 源自​​Orocos（Open Robot Control Software）​​ 项目，是一个更古老、更成熟的项目。
  • 哲学​​: ​​简单与鲁棒​**​。提供了一个干净、易于理解的API，专注于机器人学中最经典、最公认的算法。它的目标是稳定和可靠，而不是追求极致的性能或最前沿的功能。

2. 功能特性

功能	Pinocchio	Orocos KDL	说明
​​正向运动学​​	✅	✅	两者都完美支持
​​雅可比矩阵计算​​	✅	✅	两者都支持
​​逆向运动学​​	✅	✅	KDL内置求解器；Pinocchio需搭配优化库（e.g. NLopt, Ipopt）
​​正向动力学​​ (ABA)	✅	✅	​​Pinocchio极快​​，利用稀疏性
​​逆向动力学​​ (RNEA)	✅	✅	​​Pinocchio极快​​，利用稀疏性
​​动力学导数​​	✅	❌	​​Pinocchio独有优势​​。可计算动力学模型的解析导数（computeABADerivatives），对优化、MPC、强化学习至关重要
​​碰撞检测​​	✅ (集成hpp-fcl)	❌	​​Pinocchio独有优势​​。可直接计算物体间距离及其导数
​​算法微分 (AD)​​	✅ (支持CppAD, CasADi)	❌	​​Pinocchio独有优势​​。方便用户自定义模型并自动求导
​​URDF解析​​	✅	✅	两者都支持
​​关节类型支持​​	非常全面	基础全面	Pinocchio支持更复杂的关节，如自由浮基、复合关节等

3. 性能对比

这是两者最显著的差异点。Pinocchio在动力学计算上比KDL快得多，主要原因如下：

• ​​算法优化​​: Pinocchio实现了最新、最高效的算法（如 articulated-body algorithm - ABA）。
• ​​稀疏性利用​​: Pinocchio的代码充分利用了动力学模型中固有的稀疏性，大大减少了计算量。
• ​​现代C++​​: 大量使用模板和元编程，在编译期完成很多工作，运行时效率极高。

​​基准测试表明，Pinocchio的正向动力学（ABA）计算速度通常比KDL快 10-50 倍​​，对于高自由度机器人（如人形机器人），优势更加明显。

4. 代码示例与API风格

• ​​Orocos KDL (C++)​​:

 #include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

// 1. 创建运动链
KDL::Chain chain;
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame::DH(0.0, 0.0, 0.3, 0.0)));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame::DH(0.0, 0.0, 0.3, 0.0)));

// 2. 创建求解器
KDL::ChainFkSolverPos_recursive fk_solver(chain);

// 3. 计算正运动学
KDL::JntArray q(2);
q(0) = M_PI/4; q(1) = M_PI/4; // 设置关节角度
KDL::Frame end_effector_pose;
fk_solver.JntToCart(q, end_effector_pose);

std::cout << "End Effector Pose: " << end_effector_pose << std::endl;

风格：面向对象，清晰直观，易于学习和使用。

• ​Pinocchio (C++)​​:

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/kinematics.hpp"

// 1. 从URDF文件加载模型
pinocchio::Model model;
pinocchio::urdf::buildModel("robot.urdf", model);
pinocchio::Data data(model);

// 2. 计算正运动学
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
q[0] = M_PI/4; q[1] = M_PI/4; // 设置关节角度
pinocchio::forwardKinematics(model, data, q);

// 获取末端执行器帧ID（需要提前知道名称）
auto frame_id = model.getFrameId("tool_frame");
pinocchio::Frame frame_pose = data.oMf[frame_id];

std::cout << "End Effector Pose: " << frame_pose.translation().transpose() << std::endl;

风格：更底层，功能更强大，但需要更多设置和理解。

5. 社区与生态

• ​​Pinocchio​​:
  • ​​生态​​: 与一系列高端工具紧密集成，如​​Crocoddyl（最优控制）​​、​​TSID（任务空间逆动力学）​​、​​HPP（运动规划）​​。
  • ​​社区​​: 活跃的研究社区，更新频繁，持续添加新功能。
• ​​Orocos KDL​​:
  • ​生态​​: ​​ROS（Robot Operating System）​​ 的标准组件。许多ROS包（如moveit、ros_control）都默认或广泛使用KDL
  • ​​社区​​: 非常稳定，更新较慢，但被工业界和学术界广泛验证和使用。

如何选择？

选择 ​​Pinocchio​​ 如果：

1. 你需要​​极致的性能​​，例如用于模型预测控制（MPC）、强化学习（RL）等高频实时控制。
2. 你的工作涉及​​优化问题​​（如轨迹优化、状态估计），需要计算​​动力学模型的解析导数​​。
3. 你需要进行​​碰撞检测​​或与几何相关的复杂计算。
4. 你正在研究​​人形机器人、四足机器人​​等复杂系统。
5. 你打算使用​​Crocoddyl​​等高级控制框架。

选择 ​​Orocos KDL​​ 如果：

1. 你正在​​学习机器人学基础​​，想要一个简单可靠的库。
2. 你正在进行​​快速原型开发​​，需要一个易于理解和调试的工具。
3. 你的项目深度集成于 ​​ROS（ROS1）​​ 生态，并且不想引入复杂的依赖。
4. 你的应用对性能要求不是极端苛刻，传统的控制循环（~1kHz）足以满足。
5. ​​稳定性和可预测性​​比新特性更重要。

总结

可以把它们看作机器人学领域的两种不同工具：

• ​​Orocos KDL​​ 像一把​​可靠的老虎钳​​：坚固、简单、用途明确，任何时候用它都不会错，尤其适合教学和基础应用。
• ​​Pinocchio​​ 像一套​​高精度的数控机床​​：功能强大、效率极高、能处理极其复杂的任务（如计算梯度），但学习和设置成本更高，是为解决尖端问题而生的。

对于大多数​​新项目​​，尤其是涉及优化、高频控制或复杂系统的研究，​​Pinocchio是更强大、更面向未来的选择​​。而对于​​教育​​、​​快速集成​​或​​维护基于ROS的现有系统​​，​​Orocos KDL​​ 仍然是一个优秀且可靠的选择。

Compile

源码是.cpp的，编译成.so库，然后python调用

One can compile the dynamic library by doing the following:

1. mkdir build && cd build
2. cmake .. && make -j8

Depending on your python version, you will get a file called pymp.cpython-310-x86_64-linux-gnu.so or similar. This is directly importable in python by doing import pymp.

To install the entire package along with python glue code, do python3.[version] -m pip install . inside the root directory of the project.

编译输出

cmake

在 Linux 环境下，运行 cmake . 会在当前目录生成如下主要文件和文件夹：

1. Makefile

   • 这是最核心的文件，cmake . 会根据 CMakeLists.txt 生成一个标准的 GNU Make 构建脚本。你后续只需要运行 make 命令就可以自动编译整个项目。

2. CMakeFiles/

   • 这是一个文件夹，里面包含了 CMake 的中间文件、编译配置、依赖信息等。通常不用手动修改。

3. CMakeCache.txt

   • CMake 的缓存文件，记录了各种变量和路径，方便下次增量构建。

4. 其他辅助文件

   • 包括 cmake_install.cmake、CTestTestfile.cmake（如果有测试）等。

注意：

• cmake . 并不会直接编译生成可执行文件或库，只是生成了用于自动化编译的 Makefile。
• 你需要再运行 make 才会生成最终的二进制文件（如可执行程序、库文件等）。

总结：
cmake . 生成的是构建脚本（Makefile）和配置文件，而不是最终产品。
后续编译请用 make 命令。

---

make

在 Linux 环境下，运行 cmake . 后再执行 make -j8，会根据项目的 CMakeLists.txt 文件自动编译源码并生成最终的二进制文件和库。以 MPlib 项目为例，具体会生成以下内容：

1. 静态库

• libmp.a
  • 这是由 src/*.cpp 源码生成的 C++ 静态库，包含主要的运动规划算法实现。

2. Python 扩展模块

• pymp.so
  • 这是用 pybind11 编译的 Python C++ 扩展模块，允许你在 Python 中直接使用 MPlib 的核心功能。

3. 测试可执行文件

• test_articulated_model
  • 这是一个编译后的测试程序，可以用于验证核心动力学/运动规划功能是否正常。

4. 文档和辅助文件

• 可能自动生成一些文档头文件（如 pybind/docstring/xxx.h），用于 Python 绑定的 docstring。

具体路径

这些文件通常会生成在项目的 build 目录（如果你用 cmake .. 在 build 目录下执行），或者直接在项目根目录（如果你在根目录运行 cmake 和 make）。

总结

• libmp.a：C++ 静态库，给 C++ 工程用，提供算法库
• pymp.so：Python 扩展模块，给 Python 用，提供 Python 绑定接口，实现“Python调用C++高性能算法”
• test_articulated_model：测试可执行文件
• 可能还有文档头文件和其他辅助产物

你可以用 ls 命令查看这些生成文件。如果需要知道具体某个文件的用途或结构，可以继续问！

libmp.a

• 类型：C++静态库文件
• 作用：
  • 这是 MPlib 项目的核心算法库，包含了所有用 C++ 实现的运动规划相关的功能和逻辑。
  • 如果你用 C++ 开发，可以直接在你的工程中链接这个库，调用 MPlib 的底层接口和算法。
  • 它不包含 Python 接口，只能被 C++ 代码使用。

pymp.so

• 类型：Python扩展模块（动态库，通常后缀为 .so 在 Linux 下）
• 作用：
  • 这是用 pybind11 生成的 Python/C++ 绑定模块。
  • 你可以在 Python 里直接 import pymp，然后像用普通 Python 库一样使用 MPlib 的功能，底层实际是调用 C++ 实现，效率高。
  • 适合用 Python 做机器人算法开发、研究、脚本测试等工作。

如何生成 pymp.so

pymp.so 是通过 CMake 配合 pybind11 生成的 Python C++扩展模块，它允许你在 Python 中直接使用 MPlib 的核心功能。

生成流程

1. CMake 配置
   在 CMakeLists.txt 文件中，核心相关内容如下：

   # 添加 pybind11 子模块
   add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/third_party/pybind11")
   
   # 收集 Python 绑定相关源码
   file(GLOB_RECURSE PYBIND_SRC "${CMAKE_CURRENT_SOURCE_DIR}/pybind/*.cpp")
   
   # 生成 Python 扩展模块 pymp
   pybind11_add_module(pymp ${PYBIND_SRC})
   
   # 链接 mp 库（libmp.a）
   target_link_libraries(pymp PRIVATE mp)
   

2. 编译过程

   • CMake 根据上述配置，调用 pybind11 相关的 CMake命令，将 pybind 目录下的 C++ 源码编译成一个 Python 可加载的动态库（.so 文件）。
   • 该模块名为 pymp，即 pymp.so。

3. 依赖关系

   • pymp.so 会调用并链接 C++ 静态库 libmp.a（即上面配置的 target_link_libraries(pymp PRIVATE mp)）。
   • 这样，Python 层的接口实际调用的是 libmp.a 里的底层算法和功能。
     总结

• pymp.so 是用 pybind11 生成的 Python C++ 扩展模块。
• 编译时会链接 libmp.a，Python 调用的其实是 C++ 实现的核心算法。
• 这样既保证了 Python 的易用性，又有 C++ 的高性能。

setup.py

这个 setup.py 文件是用于 Python 包的构建和安装脚本，主要功能如下：

1. 构建 C++ 扩展：

   • 通过自定义的 CMakeExtension 和 CMakeBuild 类，利用 CMake 编译 C++（或其他）扩展，最终生成 Python 可以调用的模块（如 mplib.pymp）。
   • CMakeExtension 仅指定源码目录，不直接指定源码文件。
   • CMakeBuild 类在实际编译时会调用 cmake 命令，并支持自定义构建参数（如并行度、编译类型）。

2. 兼容 pybind11（或类似工具）：

   • setup.py 通过 CMake 构建，便于集成 pybind11 生成的 Python 绑定（通常用于 C++/Python 混合项目）。

3. 包含包管理和数据文件：

   • find_packages(include=“mplib*”) 自动寻找并包含符合条件的 Python 包。
   • package_data 和 py.typed 文件支持类型检查。

4. 配置参数：

   • name: 包名为 “mplib”。
   • ext_modules: 构建 mplib.pymp 这个扩展模块。
   • cmdclass: 指定 build_ext 使用自定义的 CMakeBuild。
   • zip_safe: 设置为 False，说明该包不能以 zip 形式安全安装。

**总结：**这个文件用于将 haosulab/MPlib 的 C++ 核心代码编译为 Python 可用的扩展模块，实现 Python 与 C++ 的高效混合调用，并自动处理包发现与类型支持。

pyproject.toml

这个文件是 MPlib 项目的 pyproject.toml，主要用于定义 Python 项目的构建、元数据和工具配置。下面是分块解释：

1. [build-system]

• 指定用 setuptools 构建项目，需要的依赖包括 setuptools、setuptools-git-versioning 和 libclang。

2. [project]

• name: 项目名称 mplib
• dynamic: version 由 git 自动生成
• dependencies: 安装时需要 numpy、toppra、transforms3d
• requires-python: 最低 Python 版本 3.8
• authors/maintainers: 项目作者和维护者信息
• description: 项目简介
• readme: 项目主页说明文件
• license: 使用的许可证（MIT）
• keywords: 关键词，方便检索
• classifiers: 用于 PyPI 分类（操作系统、受众、编程语言等）

3. [tool.setuptools-git-versioning]

• 用于自动生成版本号，基于 git 标签和提交记录。

4. [tool.ruff]

• 配置 Ruff（一个 Python 代码静态检查和格式化工具），如行长度、排除目录（third_party）。

5. [tool.ruff.lint]

• 配置 Ruff 静态检查启用的规则和忽略的规则，支持多种流行 Python 检查插件。

6. [tool.ruff.lint.per-file-ignores]

• 针对特定文件设置忽略规则，比如 init.py 允许未使用的导入。

7. [tool.ruff.format]

• 配置 Ruff 格式化工具，启用 docstring 格式化。

8. [tool.cibuildwheel]

• 配置 cibuildwheel（用于自动构建 Python 轮子包的工具），指定跳过的架构、构建镜像、测试要求和测试命令等。
• 只在 Python 3.10 上运行测试，跳过其他 Python 版本。

总结：
这个文件让 MPlib 支持标准化 Python 构建、自动化版本管理、代码质量检查和多平台打包，是 Python 项目现代化开发的重要配置文件。如果要安装或开发这个包，pyproject.toml 提供了所有必要的元信息和工具配置。

这个文件与setup.py有什么区别，安装是掉用的是哪一个

区别如下：

1. pyproject.toml 和 setup.py 的主要区别

• pyproject.toml 是一种新标准（PEP 518/PEP 621），用于声明项目的元数据和构建方式。它可以定义项目依赖、作者信息、构建工具等，是现代 Python 项目的推荐做法。
• setup.py 是传统的 Python 包安装脚本，里面用 Python 代码描述包信息和安装逻辑。早期 Python 包通常只用 setup.py。

具体来说：

	pyproject.toml	setup.py
格式	TOML 配置文件	Python 脚本
主要内容	元数据、依赖、构建工具、工具配置等	元数据、依赖、构建逻辑
标准	PEP 518, PEP 621	早期 setuptools 标准
优势	更规范、可声明多个工具、更易于自动化	灵活，可自定义复杂逻辑

2. pip install . 时调用哪个文件？

• 如果目录下有 pyproject.toml，pip 首先读取 pyproject.toml，根据 [build-system] 里的配置选择合适的构建工具（比如 setuptools），然后再去用 setup.py（如果声明用 setuptools）。
• 如果只有 setup.py，pip 直接用 setup.py 作为构建入口。

在 MPlib 这个项目里，两者都存在，但实际安装时，pip 主要是先读 pyproject.toml，根据里面的 [build-system] 配置，最终还是会调用 setuptools（即 setup.py）来完成安装。

简而言之：

• pip install . 首先读取 pyproject.toml
• 如果用 setuptools，则 setup.py 也会被调用
• pyproject.toml 是新标准，setup.py 是旧标准，推荐用 pyproject.toml 完全替代 setup.py，但很多项目兼容两者

如果你想了解具体的安装流程或项目结构，可以继续问！

CMakeLists.txt

该文件是 MPlib 项目的 CMakeLists.txt，是 CMake 构建系统的配置文件。它主要描述了如何编译和链接 C++ 源码、依赖库，以及如何生成 Python 扩展模块。具体说明如下：

1. 基础设置

• 指定最低 CMake 版本为 3.12。
• 项目名称为 mp，使用 C++ 语言。
• 设置 C++ 标准为 C++17。

2. 编译选项

• RELEASE 模式下使用高优化等级（-O3）、调试信息（-g3）、警告为错误（-Werror）等。
• C 和 C++ 都设置了优化选项。

3. 链接器和编译加速

• 自动检测 lld 链接器，若存在则使用它加速链接。
• 检测并使用 ccache 加速编译。

4. 查找依赖库

• 指定 CMake 模块搜索路径。
• 查找并要求一系列依赖库：Eigen3、Boost、ompl、fcl、pinocchio、assimp、orocos_kdl、urdfdom。

5. 源码和库设置

• 自动收集 src 目录下所有 C++ 源码文件，生成静态库 mp。
• 链接所有找到的依赖库。
• 设置头文件搜索路径（include 目录及相关依赖的 include 目录）。
• 设置库属性为位置无关代码（方便动态链接）。

6. 文档自动生成

• 定义 pybind11_mkdoc 目标，自动运行脚本生成 Python 绑定的文档字符串头文件。

7. Pybind11 Python 扩展

• 添加 pybind11 子模块，用于生成 Python 接口。
• 收集 pybind 目录下所有源码，生成 Python 模块 pymp。
• 让 pymp 依赖 mp 库和文档生成目标。

8. 单元测试

• 编译 test_articulated_model 测试用例并添加到 CTest，让构建时可以自动运行测试。

总结：
该文件用于自动化编译 C++ 核心库、管理依赖、生成 Python 扩展模块和测试。它让 MPlib 能在不同平台上高效、可复现地构建和测试，是 C++/Python 混合项目的标准做法。

pybind11

在 haosulab/MPlib 这个库中，pybind11 的作用是作为桥梁，把 C++ 编写的运动规划算法或底层逻辑，封装成 Python 可以直接调用的模块。

具体来说：

• MPlib 的核心功能大多用 C++ 实现，这样可以获得高效的性能。
• 但很多用户和应用习惯用 Python 进行开发和实验，所以需要一种方式，让 Python 代码能直接调用 C++ 实现的功能。
• pybind11 就是实现这种桥接的主流工具。它可以把 C++ 类、函数、数据结构“绑定”成 Python 模块，用户可以像用普通 Python 库一样使用 C++ 写的代码。

结合你的 setup.py 文件：

• setup.py 通过 CMake 构建扩展模块（如 mplib.pymp），pybind11 通常在 CMake 配置和 C++ 源码里被使用，用于生成 Python 可用的接口。
• 安装好以后，用户可以在 Python 中 import mplib.pymp，直接调用 C++ 实现的高性能运动规划功能。

总结： pybind11 在 MPlib 的主要作用是让 Python 能直接调用和使用 C++ 实现的运动规划算法，实现高效的跨语言集成和易用性。

Getting Started

仿真环境 sapien(需要cuda)

pip install sapien

官网demo

Planner Configuration

Plan a Path

Plan with sampling-based algorithms

Follow a path

Plan with screw motion

Move the boxes

Inverse Kinematics

Collision Avoidance

The robot will avoid self-collisions (i.e., collisions between the robot links), but may collide with the environment.

Detecting Collision

Planning with Fixed Joints

Constrained Planning

sapien

#!/usr/bin/env python3

import sapien.core as sapien
from sapien.utils.viewer import Viewer

import mplib
from mplib import Pose

class DemoSetup:
    """
    This class is the super class to abstract away some of the setups for the demos.
    You will need to install Sapien via `pip install sapien` for this to work
    if you want to use the viewer.
    """

    def __init__(self):
        """Nothing to do"""
        pass

    def setup_scene(self, **kwargs):
        """This is the Sapien simulator setup and has nothing to do with mplib"""
        # declare sapien sim
        self.engine = sapien.Engine()
        # declare sapien renderer
        self.renderer = sapien.SapienRenderer()
        # give renderer to sapien sim
        self.engine.set_renderer(self.renderer)

        # declare sapien scene
        scene_config = sapien.SceneConfig()
        self.scene = self.engine.create_scene(scene_config)
        # set simulation timestep
        self.scene.set_timestep(kwargs.get("timestep", 1 / 240))
        # add ground to scene
        self.scene.add_ground(kwargs.get("ground_height", 0))
        # set default physical material
        self.scene.default_physical_material = self.scene.create_physical_material(
            kwargs.get("static_friction", 1),
            kwargs.get("dynamic_friction", 1),
            kwargs.get("restitution", 0),
        )
        # give some white ambient light of moderate intensity
        self.scene.set_ambient_light(kwargs.get("ambient_light", [0.5, 0.5, 0.5]))
        # default enable shadow unless specified otherwise
        shadow = kwargs.get("shadow", True)
        # default spotlight angle and intensity
        direction_lights = kwargs.get(
            "direction_lights", [[[0, 1, -1], [0.5, 0.5, 0.5]]]
        )
        for direction_light in direction_lights:
            self.scene.add_directional_light(
                direction_light[0], direction_light[1], shadow=shadow
            )
        # default point lights position and intensity
        point_lights = kwargs.get(
            "point_lights",
            [[[1, 2, 2], [1, 1, 1]], [[1, -2, 2], [1, 1, 1]], [[-1, 0, 1], [1, 1, 1]]],
        )
        for point_light in point_lights:
            self.scene.add_point_light(point_light[0], point_light[1], shadow=shadow)

        # initialize viewer with camera position and orientation
        self.viewer = Viewer(self.renderer)
        self.viewer.set_scene(self.scene)
        self.viewer.set_camera_xyz(
            x=kwargs.get("camera_xyz_x", 1.2),
            y=kwargs.get("camera_xyz_y", 0.25),
            z=kwargs.get("camera_xyz_z", 0.4),
        )
        self.viewer.set_camera_rpy(
            r=kwargs.get("camera_rpy_r", 0),
            p=kwargs.get("camera_rpy_p", -0.4),
            y=kwargs.get("camera_rpy_y", 2.7),
        )

    def load_robot(self, **kwargs):
        """
        This function loads a robot from a URDF file into the Sapien Scene.
        Note that does mean that setup_scene() must be called before this function.
        """
        loader: sapien.URDFLoader = self.scene.create_urdf_loader()
        loader.fix_root_link = True
        self.robot: sapien.Articulation = loader.load(
            kwargs.get("urdf_path", "./data/panda/panda.urdf")
        )
        self.robot.set_root_pose(
            sapien.Pose(
                kwargs.get("robot_origin_xyz", [0, 0, 0]),
                kwargs.get("robot_origin_quat", [1, 0, 0, 0]),
            )
        )
        self.active_joints = self.robot.get_active_joints()
        for joint in self.active_joints:
            joint.set_drive_property(
                stiffness=kwargs.get("joint_stiffness", 1000),
                damping=kwargs.get("joint_damping", 200),
            )

    def setup_planner(self, **kwargs):
        """
        Create an mplib planner using the default robot.
        See planner.py for more details on the arguments.
        """
        self.planner = mplib.Planner(
            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
            move_group=kwargs.get("move_group", "panda_hand"),
        )

    # follow path ankor
    def follow_path(self, result):
        """Helper function to follow a path generated by the planner"""
        # number of waypoints in the path
        n_step = result["position"].shape[0]
        # this makes sure the robot stays neutrally boyant instead of sagging
        # under gravity
        for i in range(n_step):
            qf = self.robot.compute_passive_force(
                gravity=True, coriolis_and_centrifugal=True
            )
            self.robot.set_qf(qf)
            # set the joint positions and velocities for move group joints only.
            # The others are not the responsibility of the planner
            for j in range(len(self.planner.move_group_joint_indices)):
                self.active_joints[j].set_drive_target(result["position"][i][j])
                self.active_joints[j].set_drive_velocity_target(
                    result["velocity"][i][j]
                )
            # simulation step
            self.scene.step()
            # render every 4 simulation steps to make it faster
            if i % 4 == 0:
                self.scene.update_render()
                self.viewer.render()

    # follow path ankor end
    def set_gripper(self, pos):
        """
        Helper function to activate gripper joints
        Args:
            pos: position of the gripper joint in real number
        """
        # The following two lines are particular to the panda robot
        for joint in self.active_joints[-2:]:
            joint.set_drive_target(pos)
        # 100 steps is plenty to reach the target position
        for i in range(100):
            qf = self.robot.compute_passive_force(
                gravity=True, coriolis_and_centrifugal=True
            )
            self.robot.set_qf(qf)
            self.scene.step()
            if i % 4 == 0:
                self.scene.update_render()
                self.viewer.render()

    def open_gripper(self):
        self.set_gripper(0.4)

    def close_gripper(self):
        self.set_gripper(0)

    def move_to_pose_with_RRTConnect(self, pose: Pose):
        """
        Plan and follow a path to a pose using RRTConnect

        Args:
            pose: mplib.Pose
        """
        # result is a dictionary with keys 'status', 'time', 'position', 'velocity',
        # 'acceleration', 'duration'
        # plan_pose ankor
        result = self.planner.plan_pose(
	        pose, 
	        self.robot.get_qpos(), 
	        time_step=1 / 250)
        # plan_pose ankor end
        if result["status"] != "Success":
            print(result["status"])
            return -1
        # do nothing if the planning fails; follow the path if the planning succeeds
        self.follow_path(result)
        return 0

    def move_to_pose_with_screw(self, pose):
        """
        Interpolative planning with screw motion.
        Will not avoid collision and will fail if the path contains collision.
        """
        result = self.planner.plan_screw(
            pose,
            self.robot.get_qpos(),
            time_step=1 / 250,
        )
        if result["status"] == "Success":
            self.follow_path(result)
            return 0
        else:
            # fall back to RRTConnect if the screw motion fails (say contains collision)
            return self.move_to_pose_with_RRTConnect(pose)

    def move_to_pose(self, pose, with_screw=True):
        """API to multiplex between the two planning methods"""
        if with_screw:
            return self.move_to_pose_with_screw(pose)
        else:
            return self.move_to_pose_with_RRTConnect(pose)

mujoco_1

#!/usr/bin/env python3

import mujoco
import mujoco.viewer
import numpy as np

import mplib
from mplib import Pose


class DemoSetup:
    """
    This class is the super class to abstract away some of the setups for the demos.
    Modified to use MuJoCo instead of Sapien.
    """

    def __init__(self):
        """Nothing to do"""
        pass

    def setup_scene(self, **kwargs):
        """This is the MuJoCo simulator setup"""
        # Load empty scene
        self.model = mujoco.MjModel.from_xml_string('your_scene.xml')
        self.data = mujoco.MjData(self.model)
        
        # Set simulation timestep
        self.model.opt.timestep = kwargs.get("timestep", 1 / 240)
        
        # Set default physical material
        self.model.opt.gravity[:] = [0, 0, -9.81]
        
        # Initialize viewer
        self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
        
        # Set camera position and orientation
        self.viewer.cam.distance = 1.5
        self.viewer.cam.azimuth = 150
        self.viewer.cam.elevation = -25

    def load_robot(self, **kwargs):
        """
        This function loads a robot from a URDF file into the MuJoCo model.
        """
        # Load robot URDF
        robot_xml = f"""
        <mujoco>
            <compiler angle="radian" meshdir="./assets" texturedir="./textures"/>
            <worldbody>
                <body name="robot_base" pos="0 0 0">
                    <include file="{kwargs.get('urdf_path', './data/panda/panda.urdf')}"/>
                </body>
            </worldbody>
        </mujoco>
        """
        
        self.model = mujoco.MjModel.from_xml_string(robot_xml)
        self.data = mujoco.MjData(self.model)
        
        # Reinitialize viewer with new model
        if hasattr(self, 'viewer'):
            self.viewer.close()
        self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
        
        # Set camera position
        self.viewer.cam.distance = 1.5
        self.viewer.cam.azimuth = 150
        self.viewer.cam.elevation = -25
        
        # Get active joints
        self.active_joints = [i for i in range(self.model.nu)]

    def setup_planner(self, **kwargs):
        """
        Create an mplib planner using the default robot.
        See planner.py for more details on the arguments.
        """
        self.planner = mplib.Planner(
            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
            move_group=kwargs.get("move_group", "panda_hand"),
        )

    def follow_path(self, result):
        """Helper function to follow a path generated by the planner"""
        n_step = result["position"].shape[0]
        
        for i in range(n_step):
            # Set joint positions
            self.data.ctrl[:] = result["position"][i]
            # Step the simulation
            mujoco.mj_step(self.model, self.data)
            # Update viewer
            self.viewer.sync()
            # Small delay for visualization
            if i % 4 == 0:
                mujoco.mj_step(self.model, self.data)
                self.viewer.sync()

    def set_gripper(self, pos):
        """
        Helper function to activate gripper joints
        Args:
            pos: position of the gripper joint in real number
        """
        for i in range(100):
            # Set gripper position (last two joints for Panda)
            self.data.ctrl[-2:] = [pos, pos]
            
            # Step the simulation
            mujoco.mj_step(self.model, self.data)
            self.viewer.sync()
            
            if i % 4 == 0:
                mujoco.mj_step(self.model, self.data)
                self.viewer.sync()

    def open_gripper(self):
        self.set_gripper(0.4)

    def close_gripper(self):
        self.set_gripper(0)

    def move_to_pose_with_RRTConnect(self, pose: Pose):
        """
        Plan and follow a path to a pose using RRTConnect
        Args:
            pose: mplib.Pose
        """
        result = self.planner.plan_pose(pose, self.data.qpos[:self.model.nu], time_step=1 / 250)
        
        if result["status"] != "Success":
            print(result["status"])
            return -1
            
        self.follow_path(result)
        return 0

    def move_to_pose_with_screw(self, pose):
        """
        Interpolative planning with screw motion.
        Will not avoid collision and will fail if the path contains collision.
        """
        result = self.planner.plan_screw(
            pose,
            self.data.qpos[:self.model.nu],
            time_step=1 / 250,
        )
        if result["status"] == "Success":
            self.follow_path(result)
            return 0
        else:
            # fall back to RRTConnect if the screw motion fails
            return self.move_to_pose_with_RRTConnect(pose)

    def move_to_pose(self, pose, with_screw=True):
        """API to multiplex between the two planning methods"""
        if with_screw:
            return self.move_to_pose_with_screw(pose)
        else:
            return self.move_to_pose_with_RRTConnect(pose)

主要修改内容：

1. 移除了所有Sapien相关的导入和代码，替换为MuJoCo的导入和实现
2. 修改了setup_scene方法，使用MuJoCo的基本场景设置
3. 重写了load_robot方法，使用MuJoCo的URDF加载方式
4. 更新了follow_path方法，使用MuJoCo的控制和步进机制
5. 修改了set_gripper和相关方法，使用MuJoCo的控制接口
6. 调整了相机设置，使用MuJoCo的查看器控制方式
7. 更新了所有与机器人状态相关的方法，使用MuJoCo的数据结构

mujoco_2

#!/usr/bin/env python3

import mujoco
import numpy as np
import mplib
from mplib import Pose

class DemoSetup:
    """
    This class abstracts away some setups for the demos, adapted for Mujoco.
    You will need to install mujoco via `pip install mujoco`.
    """

    def __init__(self):
        self.model = None
        self.data = None
        self.viewer = None

    def setup_scene(self, **kwargs):
        """
        Setup the Mujoco simulator and scene.
        """
        # Load the ground plane and empty world
        xml_path = kwargs.get("xml_path", "./data/panda/panda.xml")
        self.model = mujoco.MjModel.from_xml_path(xml_path)
        self.data = mujoco.MjData(self.model)
        # Set gravity and timestep
        self.model.opt.gravity[:] = kwargs.get("gravity", [0, 0, -9.81])
        self.model.opt.timestep = kwargs.get("timestep", 1/240)
        # Viewer will be created on demand

    def load_robot(self, **kwargs):
        """
        In Mujoco, robot is loaded as part of XML. This function can reset robot pose.
        """
        # Set root position and orientation (assuming base is movable)
        origin_xyz = kwargs.get("robot_origin_xyz", [0, 0, 0])
        origin_quat = kwargs.get("robot_origin_quat", [1, 0, 0, 0])  # wxyz
        try:
            # If robot has a free joint as root
            root_id = self.model.joint('root') if 'root' in self.model.joint_names else None
            if root_id is not None:
                self.data.qpos[root_id:root_id+3] = origin_xyz
                self.data.qpos[root_id+3:root_id+7] = origin_quat
        except Exception:
            pass
        # Find active joints (exclude fixed)
        self.active_joint_indices = [
            i for i, t in enumerate(self.model.jnt_type)
            if t != mujoco.mjtJoint.mjJNT_FREE and t != mujoco.mjtJoint.mjJNT_BALL
        ]

    def setup_planner(self, **kwargs):
        """
        Create an mplib planner using the default robot.
        """
        self.planner = mplib.Planner(
            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
            move_group=kwargs.get("move_group", "panda_hand"),
        )

    def follow_path(self, result):
        """
        Helper function to follow a path generated by the planner.
        """
        n_step = result["position"].shape[0]
        for i in range(n_step):
            # Set joint positions for move group joints only
            for j, idx in enumerate(self.active_joint_indices):
                self.data.qpos[idx] = result["position"][i][j]
                self.data.qvel[idx] = result["velocity"][i][j]
            mujoco.mj_step(self.model, self.data)
            if i % 4 == 0:
                if self.viewer is None:
                    self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
                else:
                    self.viewer.sync()

    def set_gripper(self, pos):
        """
        Helper function to activate gripper joints (assuming last two joints are gripper).
        """
        for idx in self.active_joint_indices[-2:]:
            self.data.qpos[idx] = pos
        for i in range(100):
            mujoco.mj_step(self.model, self.data)
            if i % 4 == 0 and self.viewer is not None:
                self.viewer.sync()

    def open_gripper(self):
        self.set_gripper(0.04)  # typical panda gripper open value

    def close_gripper(self):
        self.set_gripper(0.0)

    def move_to_pose_with_RRTConnect(self, pose: Pose):
        """
        Plan and follow a path to a pose using RRTConnect.
        """
        result = self.planner.plan_pose(pose, np.array(self.data.qpos[self.active_joint_indices]), time_step=1/250)
        if result["status"] != "Success":
            print(result["status"])
            return -1
        self.follow_path(result)
        return 0

    def move_to_pose_with_screw(self, pose):
        result = self.planner.plan_screw(
            pose,
            np.array(self.data.qpos[self.active_joint_indices]),
            time_step=1 / 250,
        )
        if result["status"] == "Success":
            self.follow_path(result)
            return 0
        else:
            return self.move_to_pose_with_RRTConnect(pose)

    def move_to_pose(self, pose, with_screw=True):
        if with_screw:
            return self.move_to_pose_with_screw(pose)
        else:
            return self.move_to_pose_with_RRTConnect(pose)

1. 删除 sapien 相关的导入和类成员，并改用 mujoco-py 或 mujoco 官方 Python API（如 mujoco）。
2. 场景和机器人加载：MuJoCo 通过 XML（MJCF/URDF）加载模型，场景设置也不同。
3. 渲染相关操作：MuJoCo 可以用 mujoco.viewer 或 mujoco.mj_render.
4. 机器人控制：MuJoCo 用 qpos, qvel, ctrl 直接控制关节。

pybullet

#!/usr/bin/env python3

import numpy as np
import pybullet as p
import pybullet_data
from typing import List, Optional

import mplib
from mplib import Pose

class DemoSetup:
    """
    This class is the super class to abstract away some of the setups for the demos.
    Now using PyBullet instead of Sapien for simulation.
    """

    def __init__(self):
        """Nothing to do"""
        pass

    def setup_scene(self, **kwargs):
        """Setup PyBullet simulation environment"""
        # Initialize PyBullet
        self.physics_client = p.connect(p.GUI if kwargs.get("use_gui", True) else p.DIRECT)
        p.setAdditionalSearchPath(pybullet_data.getDataPath())
        
        # Set simulation parameters
        p.setTimeStep(kwargs.get("timestep", 1 / 240))
        p.setGravity(0, 0, -9.81)
        
        # Load ground plane
        self.ground_id = p.loadURDF("plane.urdf", basePosition=[0, 0, kwargs.get("ground_height", 0)])
        
        # Set camera parameters
        camera_xyz = [
            kwargs.get("camera_xyz_x", 1.2),
            kwargs.get("camera_xyz_y", 0.25),
            kwargs.get("camera_xyz_z", 0.4)
        ]
        camera_target = [0, 0, 0.1]
        p.resetDebugVisualizerCamera(
            cameraDistance=np.linalg.norm(camera_xyz),
            cameraYaw=kwargs.get("camera_rpy_y", 2.7) * 180 / np.pi,
            cameraPitch=kwargs.get("camera_rpy_p", -0.4) * 180 / np.pi,
            cameraTargetPosition=camera_target
        )
        
        # Store viewer reference (PyBullet handles rendering internally)
        self.viewer = self.physics_client

    def load_robot(self, **kwargs):
        """
        This function loads a robot from a URDF file into the PyBullet simulation.
        """
        robot_origin_xyz = kwargs.get("robot_origin_xyz", [0, 0, 0])
        robot_origin_quat = kwargs.get("robot_origin_quat", [0, 0, 0, 1])
        
        # Load robot URDF
        self.robot = p.loadURDF(
            kwargs.get("urdf_path", "./data/panda/panda.urdf"),
            basePosition=robot_origin_xyz,
            baseOrientation=robot_origin_quat,
            useFixedBase=True
        )
        
        # Get active joints
        self.active_joints = []
        for i in range(p.getNumJoints(self.robot)):
            joint_info = p.getJointInfo(self.robot, i)
            if joint_info[2] != p.JOINT_FIXED:  # Only non-fixed joints
                self.active_joints.append(i)
        
        # Set joint properties
        self.joint_stiffness = kwargs.get("joint_stiffness", 1000)
        self.joint_damping = kwargs.get("joint_damping", 200)
        
        # Enable torque control for active joints
        p.setJointMotorControlArray(
            self.robot,
            self.active_joints,
            p.VELOCITY_CONTROL,
            forces=[0] * len(self.active_joints)
        )

    def setup_planner(self, **kwargs):
        """
        Create an mplib planner using the default robot.
        See planner.py for more details on the arguments.
        """
        self.planner = mplib.Planner(
            urdf=kwargs.get("urdf_path", "./data/panda/panda.urdf"),
            srdf=kwargs.get("srdf_path", "./data/panda/panda.srdf"),
            move_group=kwargs.get("move_group", "panda_hand"),
        )

    def get_qpos(self):
        """Get current joint positions"""
        joint_states = p.getJointStates(self.robot, self.active_joints)
        return [state[0] for state in joint_states]

    def follow_path(self, result):
        """Helper function to follow a path generated by the planner"""
        n_step = result["position"].shape[0]
        
        for i in range(n_step):
            # Set joint positions using position control
            p.setJointMotorControlArray(
                self.robot,
                self.active_joints,
                p.POSITION_CONTROL,
                targetPositions=result["position"][i],
                targetVelocities=result["velocity"][i],
                forces=[self.joint_stiffness] * len(self.active_joints),
                positionGains=[self.joint_damping] * len(self.active_joints)
            )
            
            # Simulation step
            p.stepSimulation()
            
            # Render every 4 simulation steps to make it faster
            if i % 4 == 0 and p.isConnected():
                pass  # PyBullet handles rendering automatically

    def set_gripper(self, pos):
        """
        Helper function to activate gripper joints
        Args:
            pos: position of the gripper joint in real number
        """
        # The following two lines are particular to the panda robot
        gripper_joints = self.active_joints[-2:]  # Assuming last two joints are gripper
        
        for i in range(100):
            p.setJointMotorControlArray(
                self.robot,
                gripper_joints,
                p.POSITION_CONTROL,
                targetPositions=[pos, pos],
                forces=[self.joint_stiffness] * 2
            )
            
            p.stepSimulation()
            
            if i % 4 == 0 and p.isConnected():
                pass  # PyBullet handles rendering automatically

    def open_gripper(self):
        self.set_gripper(0.4)

    def close_gripper(self):
        self.set_gripper(0)

    def move_to_pose_with_RRTConnect(self, pose: Pose):
        """
        Plan and follow a path to a pose using RRTConnect
        """
        result = self.planner.plan_pose(pose, self.get_qpos(), time_step=1 / 250)
        
        if result["status"] != "Success":
            print(result["status"])
            return -1
            
        self.follow_path(result)
        return 0

    def move_to_pose_with_screw(self, pose):
        """
        Interpolative planning with screw motion.
        Will not avoid collision and will fail if the path contains collision.
        """
        result = self.planner.plan_screw(
            pose,
            self.get_qpos(),
            time_step=1 / 250,
        )
        
        if result["status"] == "Success":
            self.follow_path(result)
            return 0
        else:
            return self.move_to_pose_with_RRTConnect(pose)

    def move_to_pose(self, pose, with_screw=True):
        """API to multiplex between the two planning methods"""
        if with_screw:
            return self.move_to_pose_with_screw(pose)
        else:
            return self.move_to_pose_with_RRTConnect(pose)

    def __del__(self):
        """Clean up PyBullet connection"""
        if hasattr(self, 'physics_client') and p.isConnected():
            p.disconnect()

主要修改内容：

​​仿真器替换​​：将Sapien替换为PyBullet

​​场景设置​​：使用PyBullet的GUI或DIRECT模式

​​机器人加载​​：使用p.loadURDF()代替Sapien的URDF加载器

​​关节控制​​：使用PyBullet的关节控制函数

​​渲染处理​​：PyBullet自动处理渲染，不需要手动更新

​​物理参数​​：调整了重力、时间步长等物理参数

​​相机设置​​：使用PyBullet的相机控制函数