CoppeliaSim forums

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

Code: Select all

local simSubprocess = require('simSubprocess')
local pyth = sim.getStringParam(sim.stringparam_defaultpython)
simSubprocess.execAsync(pyth, {'path/To/ZmqClient.py', toppraArgs}, {useSearchPath = true, openNewConsole = true})