sim=require'sim'
simOMPL=require'simOMPL'
simUI=require'simUI'
function sysCall_thread()
[Link](true)
dt=[Link]()
TOL = 0.002
v0 = 0.2
k1 = 0.5
k2 = 5
p3dxL = 381e-3
p3dxR=195e-3/2
id={chkFB=5,btnSearch=10,btnFollow=20}
ui=[Link]([[<ui title="OMPL" closeable="false" placement="relative"
position="-10,100">
<checkbox id="]]..[Link]..[[" text="Forward/backward" />
<button id="]]..[Link]..[[" text="Search path" on-
click="searchClicked" />
<button id="]]..[Link]..[[" text="Follow path" on-
click="followClicked" enabled="false" />
</ui>]])
lockUI=function(b) for i,w in pairs(id) do [Link](ui,w,not b) end end
robotHandle=[Link]('/StartConfiguration')
targetHandle=[Link]('/GoalConfiguration')
leftMotor=[Link]('/PioneerP3DX/leftMotor')
rightMotor=[Link]('/PioneerP3DX/rightMotor')
pioneer = [Link]('/PioneerP3DX')
collVolumeHandle=[Link]('/Car')
initialPosition=[Link](robotHandle)
initialOrientation=[Link](robotHandle)
robotObstaclesCollection=[Link](0)
[Link](robotObstaclesCollection,sim.handle_all,-1,0)
[Link](robotObstaclesCollection,sim.handle_tree,pioneer,1)
collPairs={collVolumeHandle,robotObstaclesCollection}
lineContainer=[Link](sim.drawing_lines,3,0,-1,99999,
{0.2,0.2,0.2})
while true do
if task then task(); task=nil; end
[Link]()
end
end
function sysCall_cleanup()
if ui then [Link](ui) end
end
function search()
lockUI(true)
[Link](lineContainer,nil)
[Link]()
[Link](sim.verbosity_scriptinfos,"Path planning task underway...")
local t=[Link]('t')
local
ss={[Link]('2d',[Link],robotHandle,{0,0},
{10,10},1)}
[Link](ss[1],0.2,[Link](ui,[Link])>0)
[Link](t,0.002)
[Link](t,ss)
[Link](t,[Link])
[Link](t,collPairs)
--[Link](t,{pioneer, sim.handle_all})
[Link](t,readState(robotHandle))
[Link](t,readState(targetHandle))
[Link](t)
r,path=[Link](t,8,-1,800)
[Link](sim.verbosity_scriptinfos,"... done!")
if path then
for i=1,[Link](t,path)-1 do
local a=[Link](t,path,i)
local b=[Link](t,path,i+1)
local
lineDat={a[1],a[2],initialPosition[3],b[1],b[2],initialPosition[3]}
[Link](lineContainer,lineDat)
end
end
writeState(path,1)
[Link](t)
lockUI(false)
[Link](ui,[Link],path~=nil)
end
function follow()
lockUI(true)
sk = {}
--print(#path)
--print((#path-6)/3)
--print(path[1], path[4], path[2], path[5])
sk[1] = 0
for i=0, (#path-6)/3, 1 do
local taux = path[3*i+3+3] - path[3*i+3];
--print(i)
local dist = [Link]((path[3*i+3+1] - path[3*i+1])^2 + (path[3*i+3+2] -
path[3*i+2])^2);
sk[2*i+2] = (2*[Link](taux/2))/dist -- curvature
if sk[2*i+2] < TOL then
sk[2*i+3] = dist
else
sk[2*i+3] = taux/sk[2*i+2] -- distance
end
sk[2*i+3] = sk[2*i+3] + sk[2*i+1]
--print(sk[2*i+2], sk[2*i+3])
end
i = 0;
n = (#path/3);
local l = 0
local O = 0
local sR = 0
p3dxPos_pv = [Link](pioneer)
--print(sk[2*n - 1])
while sR < sk[2*n - 1] *(1- TOL) do
p3dxPos_at = [Link](pioneer)
p3dxOri = [Link](pioneer)
O = p3dxOri[3] - path[3*i + 3]
l = [Link]((p3dxPos_at[1] - path[3*i + 1])^2 + (p3dxPos_at[2] - path[3*i
+ 2])^2)
while i+1 < n and l > [Link]((p3dxPos_at[1] - path[3*(i+1) + 1])^2 +
(p3dxPos_at[2] - path[3*(i+1) + 2])^2) do
i = i+1
l = [Link]((p3dxPos_at[1] - path[3*i + 1])^2 + (p3dxPos_at[2] -
path[3*i + 2])^2)
end
while i-1 >= 0 and l > [Link]((p3dxPos_at[1] - path[3*(i-1) + 1])^2 +
(p3dxPos_at[2] - path[3*(i-1) + 2])^2) do
i = i-1
l = [Link]((p3dxPos_at[1] - path[3*i + 1])^2 + (p3dxPos_at[2] -
path[3*i + 2])^2)
end
if (p3dxPos_at[1] - path[3*i + 1])*[Link](path[3*i + 3]) > (p3dxPos_at[2]
- path[3*i + 2])*[Link](path[3*i + 3])
then l = -l end
sR = sk[2*i+1]
if sR >= sk[2*n - 1] *(1- TOL) then break end
if O <= TOL then
v = v0
else
v = -k1*l/[Link](O)
end
v = [Link](v, 0.5)
v = [Link](v, -0.5)
u = sk[2*i+2]*[Link](O)*v/(1-l*sk[2*i+2]);
w = u - k2*O;
w = [Link](w, 0.5)
w = [Link](w, -0.5)
phiR,phiL = drive_p3dx(v,w)
[Link](leftMotor,phiL)
[Link](rightMotor,phiR)
testeL = [Link](leftMotor)
testeR = [Link](rightMotor)
print(sR, sk[2*i+2], O, l, w, testeL, testeR)
p3dxPos_pv = p3dxPos_at
--l = l + v*[Link](O)*dt
--sR = sR + v*[Link](O)*dt
--O = O + (w - u)*dt
[Link]()
end
[Link](leftMotor,0)
[Link](rightMotor,0)
writeState(path,1)
lockUI(false)
end
function readState(handle)
local p=[Link](handle)
local o=[Link](handle)
return {p[1],p[2],o[3]}
end
function writeState(s,off)
off=off-1 or 0
local p={s[1+off],s[2+off],initialPosition[3]}
local o={initialOrientation[1],initialOrientation[2],s[3+off]}
[Link](robotHandle,p)
[Link](robotHandle,o)
[Link](pioneer,p)
[Link](pioneer,o)
end
function searchClicked(ui,id)
task=search
end
function followClicked(ui,id)
task=follow
end
--
function drive_p3dx(v,w)
--
wr=(2*v+w*p3dxL)/2/p3dxR
wl=(2*v-w*p3dxL)/2/p3dxR
--
return wr,wl
end