Can you help me check if my dynamic constraints are correct？

[littlefiveRobot]

I use Pinocchio to compute inverse dynamic of UR5 robot. My code is as follows.

import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np
import matplotlib.pyplot as plt
import argparse
import pinocchio
from sys import argv
from os.path import dirname, join, abspath

pinocchio_model_dir = join(dirname(dirname(str(abspath(file)))), "models")
urdf_filename = pinocchio_model_dir + '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
model = pinocchio.buildModelFromUrdf(urdf_filename)
print('model name: ' + model.name)
data = model.createData()

def myInverseDynamic(q,v,a):
tau = pinocchio.rnea(model,data,q,v,a)
return tau

N_samples = 4 # 路径点的数量
dof = 6 # 机械臂自由度

way_pts = np.zeros([4,6])
way_pts[0] = [-1.62219,0.0430026,0.51164 ,0.814931 ,0.88985 ,-1.98603]
way_pts[1] = [0, 0, 0, 0, 0.523599, 0]

vlim_ = np.array([3.15,3.15,3.15,3.2,3.2,3.2])
vlim = np.vstack((-vlim_, vlim_)).T # 对每个自由度设置速度 最小最大限制
alim_ = np.array([6,6,6,6,6,6])
alim = np.vstack((-alim_, alim_)).T
taulim_ = np.array([150,150,150,28,28,28])
taulim = np.vstack((-taulim_, taulim_)).T
fs_coef = list([0] * dof)

path = ta.SplineInterpolator(np.linspace(0, 1, N_samples), way_pts)

pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim, discretization_scheme=constraint.DiscretizationType.Collocation)
pc_tau = constraint.JointTorqueConstraint(myInverseDynamic, taulim, fs_coef, discretization_scheme=constraint.DiscretizationType.Collocation
)
instance = algo.TOPPRA([pc_vel,pc_acc,pc_tau],path,gridpoints=np.linspace(0, 1, 1000),solver_wrapper='ecos',)

result

Version
I use 'pip install toppra' to install the algorithm.

[littlefiveRobot]

Since you didn't provide an example of dynamics, I'm not sure if my code is correct. I would be very grateful if you would reply to me

[littlefiveRobot]

And I wonder why there is a big fluctuation in blue line when I set the "gridpoints=np.linspace(0, 1, 300)". But when I set "gridpoints=np.linspace(0, 1, 1000)",the blue line is more smooth.

[hungpham2511]

Hi, you can try using Interpolation discretization scheme.

[hungpham2511]

See https://hungpham2511.github.io/toppra/python_api.html?highlight=collocation#toppra.constraint.DiscretizationType.Interpolation