MuJoCo 仿真 + TOPPRA 最优时间轨迹规划！机械臂运动效率拉满（附代码）

视频讲解：MuJoCo 仿真 + TOPPRA 最优时间轨迹规划！机械臂运动效率拉满（附代码）_哔哩哔哩_bilibili

MuJoCo 仿真 + TOPPRA 最优时间轨迹规划！机械臂运动效率拉满（附代码）

代码仓库：GitHub - LitchiCheng/mujoco-learning

机械臂的运动一般分成两个阶段，路径规划、轨迹规划

路径规划的目标是在机器人的工作空间里，找出一条从起始点到目标点的无碰撞路径。该路径只关注机械臂末端执行器的几何位置和姿态，不涉及运动的时间因素

轨迹规划是在路径规划得出的路径基础上，为机械臂各关节分配随时间变化的运动参数，让机械臂按特定速度、加速度和时间完成运动

今天介绍一个轨迹规划的库toppra，https://github.com/hungpham2511/toppra，先安装toppra

pip3 install toppra

在mujoco viewer中使用拖拽的方式生成几个关节路径

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

参考https://github.com/hungpham2511/toppra/blob/develop/examples/plot_straight_line.py中，使用toppra读取urdf的速度限制，以及指定加速度，指定起始和结束的速度均为0，重新生成满足如上的轨迹点。

完整代码如下

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)

        # <key qpos='-1.09146e-23 0.00126288 -3.32926e-07 -0.0696243 -2.28695e-05 0.192135 0.00080101 -5.53841e-09 2.91266e-07'/>
        # <key qpos='0.00296359 0.0163993 0.00368401 -0.0788281 0.259307 0.192303 -0.00312336 -4.3278e-08 1.45579e-07'/>
        # <key qpos='0.00498913 0.246686 0.00381545 -0.0800148 0.415234 0.193705 -0.00425587 3.30553e-07 -3.30357e-07'/>
        # <key qpos='0.00602759 0.424817 0.00377697 -0.0799391 0.371875 0.193076 -0.00368281 1.0024e-06 -1.11103e-07'/>
        # <key qpos='0.00773196 0.822049 0.00373852 -0.0797594 0.315405 0.192995 -0.00319569 1.50177e-06 -9.53418e-07'/>
        # <key qpos='0.00840017 1.08506 0.00374512 -0.0796342 0.304862 0.193412 -0.00331262 2.46441e-06 2.2996e-08'/>
        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()

预期可以看到，加速-减速-到点的移动过程