【coppeliasim】6自由度路径规划

function sysCall_threadmain()
    robotHandle=sim.getObjectHandle('Start')--获取dummy:Start的句柄
    targetHandle=sim.getObjectHandle('End')--获取目标dummy：End的句柄
    t=simOMPL.createTask('t')--创建OMPL规划任务t
    ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}--创建状态空间：6d，状态空间类型-姿态3d，Start的句柄，边界{x,y,z}->{X,Y,Z},权重1
    simOMPL.setStateSpace(t,ss)--设置任务t的状态空间ss
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)--设置算法
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})--设置碰撞er  碰撞ee
    startpos=sim.getObjectPosition(robotHandle,-1)--取 起始位置
    startorient=sim.getObjectQuaternion(robotHandle,-1)--取起始方向
    startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}--起始位姿状态：位置和四元数
    simOMPL.setStartState(t,startpose)--设置起始位姿状态
    goalpos=sim.getObjectPosition(targetHandle,-1)--目标位置
    goalorient=sim.getObjectQuaternion(targetHandle,-1)--目标姿态
    goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}--目标位姿状态
    simOMPL.setGoalState(t,goalpose)--设置目标位姿状态
    r,path=simOMPL.compute(t,20,-1,200)--计算：20——>计算路径查找程序in seconds.最大时间，用于简化路径的最大时间-1表示默认时间，minimum number of states to be returned最少要返回的状态数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.路径状态集合
    while true do
        -- Simply jump through the path points, no interpolation here:
        for i=1,#path-7,7 do--每7状态 动一下
            pos={path[i],path[i+1],path[i+2]}--位置 
            orient={path[i+3],path[i+4],path[i+5],path[i+6]}--姿态四元数
            sim.setObjectPosition(robotHandle,-1,pos)--设置移动Start的位置
            sim.setObjectQuaternion(robotHandle,-1,orient)--设置姿态
            sim.switchThread()--切换线程
        end
    end
end