机械臂轨迹规划：MuJoCo仿真与TOPPRA最优时间规划教程

机械臂的运动通常分为两个阶段：路径规划和轨迹规划。路径规划的目标是在机器人的工作空间里，找出一条从起始点到目标点的无碰撞路径。该路径只关注机械臂末端执行器的几何位置和姿态，不涉及运动的时间因素。轨迹规划是在路径规划得出的路径基础上，为机械臂各关节分配随时间变化的运动参数，让机械臂按特定速度、加速度和时间完成运动。

本文将介绍一个用于轨迹规划的库TOPPRA（Time-Optimal Path Parameterization based on Reachability Analysis），它可以帮助实现最优时间轨迹规划，提升机械臂运动效率。TOPPRA库的GitHub地址是：https://github.com/hungpham2511/toppra。首先，可以通过以下命令安装TOPPRA：

pip3 install toppra

在MuJoCo仿真环境中，可以使用拖拽的方式生成关节路径点。例如，通过MuJoCo viewer记录的关键点数据可以表示为一系列关节位置坐标：

way_pts = [
    [-1.09146e-23, 0.00126288, -3.32926e-07, -0.0696243, -2.28695e-05, 0.192135, 0.00080101, -5.53841e-09, 2.91266e-07],
    [0.00296359, 0.0163993, 0.00368401, -0.0788281, 0.259307, 0.192303, -0.00312336, -4.3278e-08, 1.45579e-07],
    [0.00498913, 0.246686, 0.00381545, -0.0800148, 0.415234, 0.193705, -0.00425587, 3.30553e-07, -3.30357e-07],
    [0.00602759, 0.424817, 0.00377697, -0.0799391, 0.371875, 0.193076, -0.00368281, 1.0024e-06, -1.11103e-07],
    [0.00773196, 0.822049, 0.00373852, -0.0797594, 0.315405, 0.192995, -0.00319569, 1.50177e-06, -9.53418e-07],
    [0.00840017, 1.08506, 0.00374512, -0.0796342, 0.304862, 0.193412, -0.00331262, 2.46441e-06, 2.2996e-08]
]

参考TOPPRA官方示例，可以使用toppra读取URDF文件中的速度限制，指定加速度约束，并设置起始和结束速度为0，重新生成满足上述轨迹点的规划。以下是一个完整代码示例，展示了如何在MuJoCo中集成TOPPRA进行轨迹规划：

from scipy.optimize import minimize
import numpy as np
from numpy.linalg import norm, solve
import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import pinocchio
import time
import mujoco_viewer

class Test(mujoco_viewer.CustomViewer):
    def __init__(self, path):
        super().__init__(path, 3, azimuth=-45, elevation=-30)
    
    def runBefor(self):
        robot = pinocchio.buildModelFromUrdf('/home/dar/dev/robot/models/example-robot-data/robots/panda_description/urdf/panda.urdf')
        print('robot name: ' + robot.name)
        
        way_pts = [
            [-1.09146e-23, 0.00126288, -3.32926e-07, -0.0696243, -2.28695e-05, 0.192135, 0.00080101, -5.53841e-09, 2.91266e-07],
            [0.00296359, 0.0163993, 0.00368401, -0.0788281, 0.259307, 0.192303, -0.00312336, -4.3278e-08, 1.45579e-07],
            [0.00498913, 0.246686, 0.00381545, -0.0800148, 0.415234, 0.193705, -0.00425587, 3.30553e-07, -3.30357e-07],
            [0.00602759, 0.424817, 0.00377697, -0.0799391, 0.371875, 0.193076, -0.00368281, 1.0024e-06, -1.11103e-07],
            [0.00773196, 0.822049, 0.00373852, -0.0797594, 0.315405, 0.192995, -0.00319569, 1.50177e-06, -9.53418e-07],
            [0.00840017, 1.08506, 0.00374512, -0.0796342, 0.304862, 0.193412, -0.00331262, 2.46441e-06, 2.2996e-08]
        ]
           
        path_scalars = np.linspace(0, 1, len(way_pts))
        path = ta.SplineInterpolator(path_scalars, way_pts)
        vlim = np.vstack([-robot.velocityLimit, robot.velocityLimit]).T
        al = np.array([2,] * robot.nv)
        alim = np.vstack([-al, al]).T
        pc_vel = constraint.JointVelocityConstraint(vlim)
        pc_acc = constraint.JointAccelerationConstraint(
            alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
        
        instance = ta.algorithm.TOPPRA([pc_vel, pc_acc],path,solver_wrapper="seidel")
        jnt_traj = instance.compute_trajectory(0, 0)
        ts_sample = np.linspace(0, jnt_traj.get_duration(), 1000)
        self.qs_sample = jnt_traj.eval(ts_sample)
        self.index = 0
    
    def runFunc(self):
        if self.index < len(self.qs_sample):
            self.data.qpos[:7] = self.qs_sample[self.index][:7]
            self.index += 1
        else:
            self.data.qpos[:7] = self.qs_sample[-1][:7]
        time.sleep(0.01)

test = Test("/home/dar/dev/robot/mujoco/mujoco_menagerie/franka_emika_panda/scene.xml")
test.run_loop()

运行上述代码，可以在MuJoCo仿真中观察到机械臂按照规划轨迹进行加速、减速和到点的移动过程，实现时间最优的运动控制。

本文整理了机械臂运动规划的基本概念，并展示了如何使用TOPPRA库在MuJoCo仿真环境中进行最优时间轨迹规划。更多详细内容，包括视频讲解和完整代码仓库，请查看原帖子：MuJoCo 仿真 + TOPPRA 最优时间轨迹规划！机械臂运动效率拉满（附代码）。