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