【coppeliasim】6自由度路径规划

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
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