reBot Arm B601-DM 的 Pinocchio 与 MeshCat 入门指南

6-DOF Robotic Arm · Multi-Motor Support · Kinematics Solver · Trajectory Planning · Fully Open Source

traj_sim_geodesic

tip

本案例示例代码可用于进行机械臂的电机或姿态的调控，包含单个电机调控、正/逆运动学调控与测试、机械臂原点设置及电机角度读取、MeshCat可视化系统等。

Pinocchio 是一个用于机器人动力学分析和优化的开源库。它提供了高效的正向/逆向运动学、动力学计算和轨迹规划功能。MeshCat 是一个基于 Web 的 3D 可视化工具，可以实时显示机器人状态和运动轨迹。

本项目结合了 Pinocchio 的强大计算能力和 MeshCat 的直观可视化，为 reBot Arm B601-DM 提供了一套完整的运动学分析和调试工具。

项目特点

完整的运动学分析
支持正向运动学 (FK) 和逆向运动学 (IK) 计算，可实时解算机械臂末端位姿。
实时 3D 可视化
通过 MeshCat 在浏览器中实时显示机械臂状态和运动轨迹，无需额外软件。
轨迹规划与跟踪
实现 SE(3) 测地线轨迹规划，支持 CLIK (Closed-Loop Inverse Kinematics) 跟踪控制。
重力补偿控制
基于 Pinocchio 动力学模型计算关节重力矩，实现机械臂的"漂浮"效果。
开源 & 可扩展
所有代码开源，支持用户根据需求自定义控制算法和可视化效果。

规格参数

本教程硬件由矽递科技 Seeed Studio 提供

参数	规格
机械臂型号	reBot Arm B601-DM
自由度	6-DOF + 夹爪
电机型号	Damiao DM4340 / DM4310
通信方式	CAN Bus via USB-CAN 适配器
工作电压	24V DC
控制方式	PC
推荐工作温度范围	0°C ～ 40°C

材料清单（BOM）

部件	数量	是否包含
reBot Arm B601-DM 机械臂	1	✅
USB2CAN 串口桥	1	✅
电源适配器 (24V)	1	✅
USB-C 线缆	1	✅
夹爪	1	✅

环境要求

项目	要求
Python	3.10+
操作系统	Ubuntu 22.04+
通信接口	USB2CAN 串口桥或 CAN 接口

安装步骤

步骤 1. 安装 uv（如未安装）

curl -LsSf https://astral.sh/uv/install.sh | sh

步骤 2. 同步环境（安装所有依赖）

git clone https://github.com/vectorBH6/reBotArm_control_py.git
cd reBotArm_control_py
uv sync

tip

uv sync 会自动创建虚拟环境（如不存在）并根据 pyproject.toml 和 uv.lock 安装所有依赖。

调试工具介绍

权限设置

运行实机控制示例前，需要设置设备权限：

# 设置串口设备权限（达妙 USB2CAN）
sudo chmod 666 /dev/ttyACM0

# 或设置 CAN 设备权限（如 can0）
sudo chmod 666 /dev/can0

单电机控制台 (`0x01damiao_test.py`)

直接使用 motorbridge SDK 进行单电机测试。

运行方式：

uv run python example/0x01damiao_test.py

交互命令：

命令	说明
`enable` / `disable`	使能/失能
`set_zero`	设置零位
`state`	查看状态
`mode mit`	MIT 模式
`mode posvel`	位置速度模式，并可追加 PID 参数
`mode vel`	纯速度模式

零点校准与角度监控 (`2_zero_and_read.py`)

自动设置所有关节零点，实时显示关节角度。

运行方式：

uv run python example/2_zero_and_read.py

# 输出实例
-0.12  +0.23  -6.42  +41.74  -0.45  -0.01  -0.01

MIT 控制模式 (`3_mit_control.py`)

输入所有关节的目标角度，将MIT控制模式下完成各电机的控制，通常用于力控、阻抗控制或需要高动态响应的场景。

运行方式：

uv run python example/3_mit_control.py
> 30 0 0 0 0 0 # 控制1号电机正转30度
> state
  pos (deg): ['+29.99', '+0.00', '-45.00', '+0.00', '+0.00', '+0.00']
> q # 退出系统

danger

注意，在MIT控制模式下，机械臂的速度会很快，需要保证人或其他设备远离机械臂的工作半径。

位置-速度控制模式 (`4_pos_vel_control.py`)

输入所有关节的目标角度，将在POS_VEL（位置-速度）混合控制模式下完成各电机的控制，在到达指定角度时运动得更加平稳、可控，减少震动。

运行方式：

uv run python example/4_pos_vel_control.py
> 30 0 0 0 0 0 # 控制1号电机正转30度
> state
  pos (deg): ['+29.99', '+0.00', '-45.00', '+0.00', '+0.00', '+0.00']
> q # 退出系统

运动学测试

正运动学测试 (`5_fk_test.py`)

根据关节角度计算末端位姿。

输入：6 个关节角度（度）

输出：

末端位置 (X, Y, Z) — 单位：米
旋转矩阵 (3×3)
欧拉角 (横滚/俯仰/偏航) — 单位：度

示例：

uv run python example/5_fk_test.py
> 0 0 0 0 0 0
====================================================
  结果 / Result
====================================================
  关节角度 (度) / Joint angles (deg): [0. 0. 0. 0. 0. 0.]
  末端位置 (m) / End-effector position (m):
    X = +0.260306
    Y = +0.000000
    Z = +0.191701
  旋转矩阵 (R_world^end) / Rotation matrix (R_world^end):
    [+1.000000  +0.000000  -0.000007]
    [+0.000000  +1.000000  +0.000100]
    [+0.000007  -0.000100  +1.000000]
  欧拉角 XYZ (横滚, 俯仰, 偏航) [度] / Euler XYZ (roll, pitch, yaw) [deg]:
    横滚/roll  = -0.0057
    俯仰/pitch = -0.0004
    偏航/yaw   = +0.0000

逆运动学测试 (`6_ik_test.py`)

根据期望末端位姿求解关节角度。

输入格式：

仅位置：<x> <y> <z>（米）
位置 + 姿态：<x> <y> <z> <roll> <pitch> <yaw>（度）

示例：

uv run python example/6_ik_test.py

# 用法A
> 0.28 0 0.3  # 仅位置
====================================================
  结果 / Result
====================================================
  目标末端位置 / Target position : [+0.2800, +0.0000, +0.3000] m
  收敛 / Converged : 是 / Yes
  迭代次数 / Iterations: 2000
  位置误差 / Position error: 5.62e-17 m
  关节角度 (度) [前 6 个控制关节] / Joint angles (deg) [first 6 control joints]:
    joint1     =  -0.0003 deg  (-0.0000 rad)
    joint2     = -22.9687 deg  (-0.4009 rad)
    joint3     = -24.2191 deg  (-0.4227 rad)
    joint4     =  +1.2508 deg  (+0.0218 rad)
    joint5     =  -0.0003 deg  (-0.0000 rad)
    joint6     =  +0.0057 deg  (+0.0001 rad)

# 用法B
> 0.28 0 0.3 0 1 0       # 位置 + 姿态
====================================================
  结果 / Result
====================================================
  目标末端位置 / Target position   : [+0.2800, +0.0000, +0.3000] m
  目标末端姿态 / Target orientation: [+0.00, +1.00, +0.00] deg
  收敛 / Converged  : 是 / Yes
  迭代次数 / Iterations: 2000
  位置误差 / Position error: 6.28e-17 m
  关节角度 (度) [前 6 个控制关节] / Joint angles (deg) [first 6 control joints]:
    joint1     =  -0.0003 deg  (-0.0000 rad)
    joint2     = -23.3968 deg  (-0.4084 rad)
    joint3     = -25.3018 deg  (-0.4416 rad)
    joint4     =  +2.9054 deg  (+0.0507 rad)
    joint5     =  -0.0003 deg  (-0.0000 rad)
    joint6     =  +0.0057 deg  (+0.0001 rad)

MIT 模式下的逆运动学控制 (`7_arm_ik_control.py`)

在 MIT 模式下使用逆运动学（IK）指定机械臂末端想去的三维坐标（X, Y, Z）和姿态（欧拉角）

输入格式：

仅位置：<x> <y> <z>（米）
位置 + 姿态：<x> <y> <z> <roll> <pitch> <yaw>（度）
输入 state ：查看当前各个关节的实际弧度值。
输入 end_state ：查看当前末端在空间中的实际坐标 (m) 和欧拉角 (rad)。

运行方式：

uv run python example/7_arm_ik_control.py

#用法A
> 0.3 0.0 0.4 # 仅控制位置（姿态默认为0），让机械臂末端走到前方 0.3 米，上方 0.4 米的位置。

#用法B
> 0.3 0.0 0.4 0.0 0.0 0.5 #同时控制位置和姿态：走到指定位置，同时手腕偏航角旋转 0.5 弧度。

> ctrl + c # 退出系统

danger

注意，在该实例代码下机械臂的速度会很快，需要保证人或其他设备远离机械臂的工作半径。

平滑轨迹的逆运动学控制 (`8_arm_traj_control.py`)

在 MIT 模式下使用逆运动学（IK），在目标时间内自动规划出一条匀速或带平滑加减速的运动轨迹，避免了关节剧烈抖动。

输入格式：

仅位置：<x> <y> <z>（米）
位置 + 姿态：<x> <y> <z> <roll> <pitch> <yaw>（度）
位置 + 姿态 + 时间（默认为 2.0 ）：<x> <y> <z> <roll> <pitch> <yaw> <time>（度）
输入 state ：查看当前各个关节的实际弧度值。
输入 end_state ：查看当前末端在空间中的实际坐标 (m) 和欧拉角 (rad)。

运行方式：

uv run python example/8_arm_traj_control.py

#用法A
> 0.3 0.0 0.4 #仅指定位置，姿态默认为 0，移动时间默认为 2.0 秒

#用法B
> 0.3 0.0 0.4 0.0 0.0 0.5 #同时控制位置和姿态：走到指定位置，同时手腕偏航角旋转 0.5 弧度，移动时间默认为 2.0 秒

#用法C
> 0.3 0.0 0.4 0.0 0.0 0.0 5.0 #让机械臂走到特定位置，并指定用 5.0 秒 的时间慢慢挪过去。(注意：如果要输时间，前方的姿态参数 0 0 0 不能省略)

> ctrl + c # 退出系统

重力补偿测试

重力补偿控制 (`9_gravity_compensation.py`)

使用 Pinocchio 动力学模型补偿关节重力。

控制律：

tau = g(q)          — 重力前馈
pos = 当前电机位置   — 关节位置跟随当前位置
kp = 2, kd = 1     — 所有关节统一刚度/阻尼

预期行为：

机械臂可以在任意姿态下"漂浮"
松开后不会因自重坠落
可以手动掰动到任意位置

运行方式：

uv run python example/9_gravity_compensation.py

输出：

实时显示各关节期望力矩（N·m）
按 Ctrl+C 停止并断开连接

高阻尼重力补偿控制 (`10_gravity_compensation_lock.py`)

抵抗轻微外力的重力补偿控制。

控制律：

注：当人用力推机械臂，末端线速度 > 0.04 m/s 或角速度 > 0.08 rad/s 时，解锁并实时更新目标角度

tau = g(q) + 积分项  — 重力前馈，并引入积分累计消除静摩擦力和残余重力死区
pos = 目标锁定角度   — 当末端移动速度低于阈值时，目标角度锁死在当前点
kp = 8.0, kd = 1.0  — 锁定状态下刚度提升至 8.0，提供更坚固的抗干扰和定位约束

预期行为：

松手即锁死：将机械臂用手掰到某个位置一旦松手，会立刻在原地死死锁住，能完美解决缓缓下坠和微小漂移的问题。
轻推不发生位移：轻微的晃动、风吹或外力触碰无法让机械臂移位。
用力即可拖动：只有用一定的力气推它、打破速度阈值后，才会解锁并顺从地滑动。

运行方式：

uv run python example/10_gravity_compensation_lock.py

输出：

实时显示当前的状态（锁定状态显示 LOCKED，解锁拖动状态显示 UPDATE）。
同步打印末端实时的线速度（m/s）、角速度（rad/s）以及各关节的重力矩（N·m）。
按 Ctrl+C 停止并断开连接

仿真环境

正运动学仿真 (`sim/fk_sim.py`)

交互式正运动学仿真，通过输入关节角度在 MeshCat 中可视化机械臂位姿。

运行方式：

uv run python example/sim/fk_sim.py

交互命令：

输入 6 个关节角度（度），空格分隔
示例：0 0 0 0 0 0
示例：45 -30 15 -60 90 -180
q/quit/exit：退出

功能特点：

实时显示末端位置和姿态
支持连续输入测试不同位姿
输出格式化的位姿信息

逆运动学仿真 (`sim/ik_sim.py`)

交互式逆运动学仿真，输入目标位姿自动求解关节角度并可视化。

运行方式：

uv run python example/sim/ik_sim.py

输入格式：

仅位置：x y z（米）
位置+姿态：x y z roll pitch yaw（弧度）

示例：

> 0.25 0.0 0.25              # 仅位置
> 0.25 0.0 0.25 0 0 0        # 位置+姿态

功能特点：

自动判断 IK 是否收敛
显示迭代次数和误差
实时更新机器人位姿

轨迹规划仿真 (`sim/traj_sim.py`)

基于 SE(3) 测地线的轨迹规划仿真，包含 CLIK 跟踪和 MeshCat 动画回放。

运行方式：

uv run python example/sim/traj_sim.py

交互命令：

输入：x y z [roll pitch yaw]（米/弧度）
直接回车使用默认配置
q：退出

功能特点：

从当前位置规划到目标位姿
使用最小加加速度轨迹 profile
实时显示轨迹统计信息
MeshCat 中回放完整轨迹动画
显示参考路径（灰色）和实际路径（绿色）

可视化工具 (`sim/visualizer.py`)

MeshCat 可视化器封装，提供统一的机器人显示接口。

主要功能：

加载 URDF 模型并显示机器人
绘制 3D 折线路径（参考/实际）
显示 IK 目标位姿（三色轴+球体）
支持关节轨迹动画播放

使用示例：

from example.sim.visualizer import Visualizer
viz = Visualizer()
viz.update(q)  # 更新机器人位姿
viz.draw_path(points, "path_name", color)  # 绘制路径

FAQ

遇到 Permission denied 错误
确保已执行 sudo chmod 666 /dev/ttyACM0 或 sudo chmod 666 /dev/can0 设置设备权限。
IK 解算失败或结果异常
检查目标位姿是否在机械臂工作空间内，确保关节限位配置正确。
重力补偿效果不佳
这可能是结构件误差和加工精度引起的，本项目的重力补偿依赖urdf和pinocchio，您可以先尝试修正urdf变为您实测下来的参数（这一步可以询问ai）。

联系方式

技术支持: 提交 Issue
项目仓库: GitHub
论坛: Seeed Studio Forum

参考文档

如果本项目对你有帮助，请给个 Star 支持一下！

项目特点​

规格参数​

材料清单（BOM）​

环境要求​

安装步骤​

步骤 1. 安装 uv（如未安装）​

步骤 2. 同步环境（安装所有依赖）​

调试工具介绍​

单电机控制台 (0x01damiao_test.py)​

零点校准与角度监控 (2_zero_and_read.py)​

MIT 控制模式 (3_mit_control.py)​

位置-速度控制模式 (4_pos_vel_control.py)​

运动学测试​

正运动学测试 (5_fk_test.py)​

逆运动学测试 (6_ik_test.py)​

MIT 模式下的逆运动学控制 (7_arm_ik_control.py)​

平滑轨迹的逆运动学控制 (8_arm_traj_control.py)​

重力补偿测试​

重力补偿控制 (9_gravity_compensation.py)​

高阻尼重力补偿控制 (10_gravity_compensation_lock.py)​

仿真环境​

正运动学仿真 (sim/fk_sim.py)​

逆运动学仿真 (sim/ik_sim.py)​

轨迹规划仿真 (sim/traj_sim.py)​

可视化工具 (sim/visualizer.py)​

FAQ​

联系方式​

参考文档​

项目特点

规格参数

材料清单（BOM）

环境要求

安装步骤

步骤 1. 安装 uv（如未安装）

步骤 2. 同步环境（安装所有依赖）

调试工具介绍

单电机控制台 (`0x01damiao_test.py`)

零点校准与角度监控 (`2_zero_and_read.py`)

MIT 控制模式 (`3_mit_control.py`)

位置-速度控制模式 (`4_pos_vel_control.py`)

运动学测试

正运动学测试 (`5_fk_test.py`)

逆运动学测试 (`6_ik_test.py`)

MIT 模式下的逆运动学控制 (`7_arm_ik_control.py`)

平滑轨迹的逆运动学控制 (`8_arm_traj_control.py`)

重力补偿测试

重力补偿控制 (`9_gravity_compensation.py`)

高阻尼重力补偿控制 (`10_gravity_compensation_lock.py`)

仿真环境

正运动学仿真 (`sim/fk_sim.py`)

逆运动学仿真 (`sim/ik_sim.py`)

轨迹规划仿真 (`sim/traj_sim.py`)

可视化工具 (`sim/visualizer.py`)

FAQ

联系方式

参考文档