[coppeliasim] 6-DOF path planning

function sysCall_threadmain()
    robotHandle=sim.getObjectHandle('Start')-- obtain dummy:Start The handle of 
    targetHandle=sim.getObjectHandle('End')-- Get the target dummy：End The handle of 
    t=simOMPL.createTask('t')-- establish OMPL Planning tasks t
    ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}-- Create state space ：6d, State space type - Posture 3d,Start The handle of , The border {x,y,z}->{X,Y,Z}, The weight 1
    simOMPL.setStateSpace(t,ss)-- Set up tasks t State space of ss
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)-- Set algorithm 
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})-- Set up collision er   Collision ee
    startpos=sim.getObjectPosition(robotHandle,-1)-- take   The starting position 
    startorient=sim.getObjectQuaternion(robotHandle,-1)-- Take the starting direction 
    startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}-- Initial pose state ： Position and quaternion 
    simOMPL.setStartState(t,startpose)-- Set the initial pose state 
    goalpos=sim.getObjectPosition(targetHandle,-1)-- Target location 
    goalorient=sim.getObjectQuaternion(targetHandle,-1)-- Target attitude 
    goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}-- Target pose status 
    simOMPL.setGoalState(t,goalpose)-- Set the target pose state 
    r,path=simOMPL.compute(t,20,-1,200)-- Calculation ：20——> Calculate the path finder in seconds. Maximum time , Maximum time to simplify the path -1 Indicates the default time ,minimum number of states to be returned The minimum number of States to return 200
--r=true: true if a solution has been found
--path: a table of states, representing the solution, from start to goal. States are specified linearly. Path state set 
    while true do
        -- Simply jump through the path points, no interpolation here:
        for i=1,#path-7,7 do-- Every time 7 state   Move 
            pos={path[i],path[i+1],path[i+2]}-- Location  
            orient={path[i+3],path[i+4],path[i+5],path[i+6]}-- Attitude quaternion 
            sim.setObjectPosition(robotHandle,-1,pos)-- Set up mobile Start The location of 
            sim.setObjectQuaternion(robotHandle,-1,orient)-- Set posture 
            sim.switchThread()-- Switching thread 
        end
    end
end