Can you help me check if my dynamic constraints are correct？ #236
Description
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.