动力学Drake框架详解
1. 概述
Drake是一个用于机器人、机械系统和其他动力学系统建模、分析和控制的C++库。它提供了丰富的工具，支持从简单的刚体系统到复杂的多体动力学系统的建模与仿真。
2. 核心功能
- 多体动力学：支持刚体和柔性体的建模。
- 控制系统：提供控制器设计和系统分析工具。
- 优化：支持约束优化和非线性优化。
- 可视化：集成MeshCat和Matplotlib等工具，用于系统状态和轨迹的可视化。
3. 主要组件
- MultibodyPlant：用于多体动力学系统的建模。
- Diagram：用于将多个系统组件连接成一个整体。
- Simulator：用于系统的仿真。
- Controllers：提供多种控制器实现，如PID、LQR等。
4. 使用案例代码
以下是一个简单的倒立摆系统建模与仿真示例。
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/visualization/visualization_config_functions.h"
namespace drake {
namespace examples {
namespace pendulum {
namespace {
int do_main() {
// 创建DiagramBuilder
systems::DiagramBuilder<double> builder;
// 创建MultibodyPlant
auto [plant, scene_graph] = multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
// 从URDF文件加载倒立摆模型
multibody::Parser parser(&plant);
parser.AddModelFromFile("path/to/pendulum.urdf");
// 完成MultibodyPlant的构建
plant.Finalize();
// 添加可视化
visualization::AddDefaultVisualization(&builder);
// 构建Diagram
auto diagram = builder.Build();
// 创建Simulator
systems::Simulator<double> simulator(*diagram);
// 设置初始状态
auto context = simulator.get_mutable_context();
auto& plant_context = diagram->GetMutableSubsystemContext(plant, context);
plant.SetPositions(&plant_context, Eigen::VectorXd::Constant(1, 0.1)); // 初始角度
plant.SetVelocities(&plant_context, Eigen::VectorXd::Zero(1)); // 初始角速度
// 运行仿真
simulator.AdvanceTo(10.0); // 仿真10秒
return 0;
}
} // namespace
} // namespace pendulum
} // namespace examples
} // namespace drake
int main() {
return drake::examples::pendulum::do_main();
}
5. 代码说明
- DiagramBuilder：用于构建系统图。
- MultibodyPlant：用于多体动力学系统的建模。
- Parser：用于从URDF文件加载模型。
- Simulator：用于系统的仿真。
- Visualization：用于系统的可视化。
6. 运行步骤
6.1. 安装Drake库
通过 pip 安装 Drake 是最简单的方式，适合不需要修改 Drake 源码或进行 C++ 开发的用户。以下是详细步骤：
6.1.1. 系统要求
- 操作系统：Ubuntu 20.04 LTS（Focal Fossa）。
- Python 版本：Python 3.8 或更高版本。
- 内存：建议至少 4GB RAM。
- 磁盘空间：建议至少 2GB 可用空间。
6.1.2. 安装依赖项
1. 更新系统
sudo apt update
sudo apt upgrade -y
2. 安装 Python 和 pip
确保已安装 Python 3 和 pip：
sudo apt install -y python3 python3-pip
3. 安装其他依赖项
Drake 的 Python 绑定需要一些额外的系统库：
sudo apt install -y \
libglib2.0-dev \
libgl1-mesa-dev \
libglu1-mesa-dev \
libpng-dev \
libxt-dev \
libosmesa6-dev \
zlib1g-dev
6.1.3. 安装 Drake
1. 使用 pip 安装 Drake
运行以下命令安装 Drake：
python3 -m pip install --upgrade drake
2. 验证安装
安装完成后，验证是否成功：
python3 -c "import pydrake; print(pydrake.__version__)"
如果输出了 Drake 的版本号（例如 1.19.0），说明安装成功。
6.1.4. 运行示例
1. 运行 Python 示例
Drake 提供了一些内置的 Python 示例。例如，运行倒立摆示例：
python3 -m pydrake.examples.pendulum
2. 使用可视化工具
Drake 支持 MeshCat 可视化工具。确保已安装 MeshCat：
python3 -m pip install meshcat
运行可视化示例：
python3 -m pydrake.examples.pendulum --visualization=meshcat
这将打开一个浏览器窗口，显示倒立摆的实时仿真。
6.1.5. 常见问题
1. 导入错误
如果运行 import pydrake 时出现错误，请检查：
- 是否安装了正确的 Python 版本（3.8 或更高）。
- 是否在虚拟环境中运行（如果是，请确保虚拟环境已激活）。
2. 缺少依赖项
如果运行时提示缺少某些库，请根据错误信息安装相应的依赖项。例如：
sudo apt install -y libgl1-mesa-glx
3. 可视化工具无法打开
如果 MeshCat 无法打开浏览器窗口，请确保：
- 已安装
meshcatPython 包。 - 系统支持浏览器访问（如果是远程服务器，请确保已配置 X11 转发或使用 VNC）。
6.1.6. 卸载 Drake
如果需要卸载 Drake，可以运行：
python3 -m pip uninstall pydrake
6.1.7. 参考文档
- Drake 官方文档：https://drake.mit.edu/
- MeshCat 文档：https://github.com/rdeits/meshcat
通过pip安装 Drake 是最快捷的方式，适合大多数用户。
6.2. 准备倒立摆的URDF文件
倒立摆的URDF模型
以下是一个简单的倒立摆URDF模型，保存为 pendulum.urdf 文件：
<?xml version="1.0"?>
<robot name="pendulum">
<!-- Base Link -->
<link name="base">
<inertial>
<mass value="0.1"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
</link>
<!-- Pendulum Link -->
<link name="pendulum">
<inertial>
<mass value="1.0"/>
<origin xyz="0 0 -0.5"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
<visual>
<origin xyz="0 0 -0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1.0" radius="0.02"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1.0" radius="0.02"/>
</geometry>
</collision>
</link>
<!-- Joint between Base and Pendulum -->
<joint name="revolute_joint" type="revolute">
<parent link="base"/>
<child link="pendulum"/>
<axis xyz="0 1 0"/>
<limit effort="10.0" velocity="10.0" lower="-3.14" upper="3.14"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
</robot>
URDF 说明：
base：底座，是一个小立方体。pendulum：摆杆，是一个细长的圆柱体。revolute_joint：底座和摆杆之间的旋转关节，允许摆杆在垂直平面内摆动。
将上述内容保存为 pendulum.urdf 文件。
6.3. 代码详解、编译并运行代码
6.3.1.完整的仿真代码详解
以下是完整的C++代码，使用Drake框架加载上述URDF模型并进行仿真：
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/visualization/visualization_config_functions.h"
#include "drake/systems/primitives/constant_vector_source.h"
namespace drake {
namespace examples {
namespace pendulum {
namespace {
int do_main() {
// 创建DiagramBuilder
systems::DiagramBuilder<double> builder;
// 创建MultibodyPlant和SceneGraph
auto [plant, scene_graph] = multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);
// 从URDF文件加载倒立摆模型
multibody::Parser parser(&plant);
parser.AddModelFromFile("pendulum.urdf"); // 确保URDF文件路径正确
// 完成MultibodyPlant的构建
plant.Finalize();
// 添加可视化
visualization::AddDefaultVisualization(&builder);
// 构建Diagram
auto diagram = builder.Build();
// 创建Simulator
systems::Simulator<double> simulator(*diagram);
// 设置初始状态
auto context = simulator.get_mutable_context();
auto& plant_context = diagram->GetMutableSubsystemContext(plant, context);
plant.SetPositions(&plant_context, Eigen::VectorXd::Constant(1, 0.1)); // 初始角度
plant.SetVelocities(&plant_context, Eigen::VectorXd::Zero(1)); // 初始角速度
// 运行仿真
simulator.AdvanceTo(10.0); // 仿真10秒
return 0;
}
} // namespace
} // namespace pendulum
} // namespace examples
} // namespace drake
int main() {
return drake::examples::pendulum::do_main();
}
6.3.2.倒立摆实时仿真示例的逻辑与运行
1. 示例逻辑
倒立摆（Pendulum）是一个经典的动力学系统，由一个固定在底座上的摆杆组成。摆杆可以在重力作用下自由摆动。Drake 的倒立摆示例展示了以下内容：
- 动力学建模：使用 Drake 的
MultibodyPlant对倒立摆进行建模。 - 仿真：通过
Simulator对系统进行仿真。 - 可视化：使用 MeshCat 可视化工具实时显示倒立摆的运动。
2. 示例代码逻辑
以下是 pydrake.examples.pendulum 示例的主要逻辑：
- 创建系统：
- 使用
MultibodyPlant创建倒立摆的动力学模型。 - 从 URDF 文件加载倒立摆的几何和质量属性。
- 使用
- 设置仿真：
- 定义初始状态（摆杆的角度和角速度）。
- 使用
Simulator对系统进行仿真。
- 可视化：
- 使用 MeshCat 可视化工具实时显示倒立摆的运动。
3. 示例运行效果
以下是倒立摆仿真的示意图：
初始状态：
|
O (摆杆初始角度为 0.1 弧度)
|
=== (底座)
仿真过程中：
\
O (摆杆在重力作用下摆动)
/
===
4. 示例代码详解
以下是 pydrake.examples.pendulum 示例的核心代码逻辑：
from pydrake.all import (
DiagramBuilder,
MultibodyPlant,
Parser,
Simulator,
MeshcatVisualizer,
SceneGraph,
)
def run_pendulum():
# 创建 DiagramBuilder
builder = DiagramBuilder()
# 创建 MultibodyPlant 和 SceneGraph
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
# 从 URDF 文件加载倒立摆模型
Parser(plant).AddModelFromFile("pendulum.urdf")
# 完成 MultibodyPlant 的构建
plant.Finalize()
# 添加 MeshCat 可视化
meshcat = MeshcatVisualizer.AddToBuilder(builder, scene_graph)
# 构建 Diagram
diagram = builder.Build()
# 创建 Simulator
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# 设置初始状态
plant_context = diagram.GetMutableSubsystemContext(plant, context)
plant.SetPositions(plant_context, [0.1]) # 初始角度为 0.1 弧度
plant.SetVelocities(plant_context, [0.0]) # 初始角速度为 0
# 运行仿真
simulator.AdvanceTo(10.0) # 仿真 10 秒
if __name__ == "__main__":
run_pendulum()
5. 关键点说明
- MultibodyPlant：
- 用于建模多体动力学系统。
- 通过
Parser从 URDF 文件加载模型。
- SceneGraph：
- 用于管理系统的几何和可视化。
- MeshcatVisualizer：
- 提供基于 Web 的实时可视化。
- Simulator：
- 对系统进行仿真，并支持设置初始状态和仿真时长。
6. 可视化效果
运行示例后，MeshCat 会打开一个浏览器窗口，显示倒立摆的实时仿真。你可以看到：
- 摆杆从初始角度（0.1 弧度）开始摆动。
- 摆杆在重力作用下摆动，直到达到稳定状态（垂直向下）。
7. 如何运行示例
-
确保已安装 Drake 和 MeshCat：
python3 -m pip install drake meshcat -
运行示例：
python3 -m pydrake.examples.pendulum --visualization=meshcat -
打开浏览器，查看实时仿真效果。
8. 结果分析
- 摆杆会从初始角度（0.1弧度）开始摆动。
- 由于没有控制器，摆杆会在重力作用下自由摆动，直到达到稳定状态（垂直向下）。
- 你可以通过调整初始状态或添加控制器来改变系统的行为。
9. 扩展
- 添加控制器（如PID控制器）来稳定倒立摆。
- 修改URDF模型，增加更多关节或复杂结构。
- 使用Drake的优化工具进行轨迹优化或参数优化。
7. 总结
Drake框架提供了强大的工具，适用于机器人、机械系统等动力学系统的建模、仿真和控制。通过上述案例，可以快速上手Drake的基本功能。
扫一扫
2195
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
