Code: Select all
function sim.generateTimeOptimalTrajectory(...)
local path, pathLengths, minMaxVel, minMaxAccel, trajPtSamples, boundaryCondition, timeout =
checkargs({
{type = 'table', item_type = 'float', size = '2..*'},
{type = 'table', item_type = 'float', size = '2..*'},
{type = 'table', item_type = 'float', size = '2..*'},
{type = 'table', item_type = 'float', size = '2..*'},
{type = 'int', default = 1000},
{type = 'string', default = 'not-a-knot'},
{type = 'float', default = 5},
}, ...)
local confCnt = #pathLengths
local dof = math.floor(#path / confCnt)
if (dof * confCnt ~= #path) or dof < 1 or confCnt < 2 or dof ~= #minMaxVel / 2 or
dof ~= #minMaxAccel / 2 then error("Bad table size.") end
local lb = sim.setStepping(true)
local pM = Matrix(confCnt, dof, path)
local mmvM = Matrix(dof, 2, minMaxVel)
local mmaM = Matrix(dof, 2, minMaxAccel)
local code = [=[import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np
ta.setup_logging("INFO")
def sysCall_init():
pass
def sysCall_cleanup():
pass
def cb(req):
try:
resp = cbb(req)
resp['success'] = True
except Exception as e:
resp = {'success': False, 'error': str(e)}
return resp
def cbb(req):
coefficients = ta.SplineInterpolator(req['ss_waypoints'], req['waypoints'], req.get('bc_type', 'not-a-knot'))
pc_vel = constraint.JointVelocityConstraint(req['velocity_limits'])
pc_acc = constraint.JointAccelerationConstraint(req['acceleration_limits'], discretization_scheme=constraint.DiscretizationType.Interpolation)
instance = algo.TOPPRA([pc_vel, pc_acc], coefficients, solver_wrapper='seidel')
jnt_traj = instance.compute_trajectory(0, 0)
duration = jnt_traj.duration
#print("Found optimal trajectory with duration {:f} sec".format(duration))
n = coefficients.dof
resp = dict(qs=[[]]*n, qds=[[]]*n, qdds=[[]]*n)
ts = np.linspace(0, duration, req.get('samples', 100))
for i in range(n):
resp['qs'][i] = jnt_traj.eval(ts).tolist()
resp['qds'][i] = jnt_traj.evald(ts).tolist()
resp['qdds'][i] = jnt_traj.evaldd(ts).tolist()
resp['ts'] = ts.tolist()
return resp
]=]
local script = sim.createScript(sim.scripttype_customization, code, 0, 'python')
sim.setObjectAlias(script, 'toppraPythonScript')
sim.initScript(script)
local toSend = {
samples = trajPtSamples,
ss_waypoints = pathLengths,
waypoints = pM:totable(),
velocity_limits = mmvM:totable(),
acceleration_limits = mmaM:totable(),
bc_type = boundaryCondition,
}
local s, r = pcall(sim.callScriptFunction, 'cb', script, toSend)
sim.removeObjects({script})
sim.setStepping(lb)
if s ~= true then
error('Failed calling TOPPRA via the generated Python script. Make sure Python is configured for CoppeliaSim, and toppra as well as numpy are installed.')
end
if not r.success then
error('toppra failed with following message: ' .. r.error)
end
return Matrix:fromtable(r.qs[1]):data(), r.ts
end