Re: Import file to CoppeliaSim
Posted: 14 Jul 2024, 03:00
by clucas69@hotmail.com
I have a motion planning TTT already open. it has the following code
function fromCSV (s)
s = s .. ',' -- ending comma
local t = {} -- table to collect fields
local fieldstart = 1
repeat
-- next field is quoted? (start with `"'?)
if string.find(s, '^"', fieldstart) then
local a, c
local i = fieldstart
repeat
-- find closing quote
a, i, c = string.find(s, '"("?)', i+1)
until c ~= '"' -- quote not followed by quote?
if not i then error('unmatched "') end
local f = string.sub(s, fieldstart+1, i-1)
table.insert(t, (string.gsub(f, '""', '"')))
fieldstart = string.find(s, ',', i) + 1
else -- unquoted; find next comma
local nexti = string.find(s, ',', fieldstart)
table.insert(t, string.sub(s, fieldstart, nexti-1))
fieldstart = nexti + 1
end
until fieldstart > string.len(s)
return t
end
function file_exists(name)
local f=io.open(name,"r")
if f~=nil then io.close(f) return true else return false end
end
--- Check if a file or directory exists in this path
function exists(file)
local ok, err, code = os.rename(file, file)
if not ok then
if code == 13 then
-- Permission denied, but it exists
return true
end
end
return ok, err
end
function parseCSVFile(u, id)
-- Remove all previous objects
for i=numberofobj-1,1,-1 do
sim.removeObject(objhandles)
end
numberofobj = 1
local nodes={}
local data={}
local out=""
fname=simUI.getEditValue(ui, 1001)
if not exists(fname) then
out=string.format("Failed to find path.")
simUI.setLabelText(ui, 1003, out)
return nil
end
-- Import files.
local paramf = fname .. "obstacles.csv"
local dataf = fname .. "path.csv"
local nodesf = fname .. "nodes.csv"
local edgesf = fname .. "edges.csv"
-- Check if files exist.
--if not file_exists(dataf) then
--out=string.format("Failed to parse!<br>Could not find path:<br>%s",dataf)
--return nil
--end
--simUI.setLabelText(ui, 1003, out)
-- Now parse all nodes.
local i=1
local parsedLine={}
if file_exists(nodesf) then
for line in io.lines(nodesf) do
if not string.find(line, '#') and line:sub(1,1) ~= '' then
local parsedLine=fromCSV(line)
if (#parsedLine < 3) then
out=string.format("Error Parsing nodes %s: Data line %d only has %d length)<br>Raw:<br>%s",nodesf,i,#parsedLine,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
local q={}
for j=1,#parsedLine,1 do
table.insert(q, tonumber(parsedLine[j]))
if (q[j]==nil) then
out=string.format("Failed to parse nodes %s!<br>Could not convert data line %d!<br> Raw Value=%s",nodesf,i,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
end
nodes=q
i=i+1
-- Draw all nodes.
objhandles[numberofobj] = sim.createPureShape(2,16,{0.05,0,0},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{q[2],q[3],0.0023})
sim.setShapeColor(objhandles[numberofobj],NULL,sim.colorcomponent_ambient_diffuse,{0,0,1})
sim.setObjectParent(objhandles[numberofobj],graphHandle,false)
numberofobj=numberofobj+1
end
end
end
-- Now parse all edges.
if file_exists(edgesf) then
for line in io.lines(edgesf) do
if not string.find(line, '#') and line:sub(1,1) ~= '' then
local parsedLine=fromCSV(line)
if (#parsedLine < 3) then
out=string.format("Error Parsing edges %s: Data line %d only has %d length)<br>Raw:<br>%s",edgesf,i,#parsedLine,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
local q={}
for j=1,#parsedLine,1 do
table.insert(q, tonumber(parsedLine[j]))
if (q[j]==nil) then
out=string.format("Failed to parse edges %s!<br>Could not convert data line %d!<br> Raw Value=%s",edgesf,i,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
end
-- Draw all edges.
if #nodes < q[1] or #nodes < q[2] then
out=string.format("Some nodes missing")
simUI.setLabelText(ui, 1003, out)
return nil
end
local difx = nodes[q[1]][2] - nodes[q[2]][2]
local dify = nodes[q[1]][3] - nodes[q[2]][3]
local midx = (nodes[q[1]][2] + nodes[q[2]][2]) / 2.0
local midy = (nodes[q[1]][3] + nodes[q[2]][3]) / 2.0
objhandles[numberofobj] = sim.createPureShape(0,16,{math.sqrt(difx ^ 2 + dify ^ 2),0.01,0},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{midx,midy,0.0021})
sim.setObjectOrientation(objhandles[numberofobj],-1,{0,0,math.atan(dify / difx)})
sim.setShapeColor(objhandles[numberofobj],NULL,sim.colorcomponent_ambient_diffuse,{1,1,0})
sim.setObjectParent(objhandles[numberofobj],graphHandle,false)
numberofobj=numberofobj+1
end
end
end
-- Now parse all obstacles.
if file_exists(paramf) then
for line in io.lines(paramf) do
if not string.find(line, '#') and line:sub(1,1) ~= '' then
local parsedLine=fromCSV(line)
if (#parsedLine < 3) then
out=string.format("Error Parsing obstacles %s: Data line %d only has %d length)<br>Raw:<br>%s",paramf,i,#parsedLine,line)
return nil
end
local q={}
for j=1,#parsedLine,1 do
table.insert(q, tonumber(parsedLine[j]))
if (q[j]==nil) then
out=string.format("Failed to parse obstacles %s!<br>Could not convert data line %d!<br> Raw Value=%s",paramf,i,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
end
-- Draw all obstacles.
objhandles[numberofobj] = sim.createPureShape(2,26,{q[3],0,0.03},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{q[1],q[2],0.015})
sim.setObjectParent(objhandles[numberofobj],envHandle,false)
numberofobj=numberofobj+1
end
end
end
-- Now parse the path.
if file_exists(dataf) then
for line in io.lines(dataf) do
if not string.find(line, '#') and line:sub(1,1) ~= '' then
local parsedLine=fromCSV(line)
local q={}
for j=1,#parsedLine,1 do
table.insert(q, tonumber(parsedLine[j]))
if (q[j]==nil) then
out=string.format("Failed to parse path %s!<br>Could not convert data line %d!<br> Raw Value=%s",dataf,i,line)
simUI.setLabelText(ui, 1003, out)
return nil
end
if #nodes < q[j] then
out=string.format("Some nodes missing")
simUI.setLabelText(ui, 1003, out)
return nil
end
data[j] = {nodes[q[j]][2], nodes[q[j]][3]}
-- Draw path.
if j > 1 then
pathEdgeL[j-1] = math.sqrt((data[j][1] - data[j-1][1]) ^ 2 + (data[j][2] - data[j-1][2]) ^ 2)
local difx = nodes[q[j]][2] - nodes[q[j-1]][2]
local dify = nodes[q[j]][3] - nodes[q[j-1]][3]
local midx = (nodes[q[j]][2] + nodes[q[j-1]][2]) / 2.0
local midy = (nodes[q[j]][3] + nodes[q[j-1]][3]) / 2.0
objhandles[numberofobj] = sim.createPureShape(0,16,{pathEdgeL[j-1],0.02,0},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{midx,midy,0.0022})
sim.setObjectOrientation(objhandles[numberofobj],-1,{0,0,math.atan(dify / difx)})
sim.setShapeColor(objhandles[numberofobj],NULL,sim.colorcomponent_ambient_diffuse,{0,0.5,0})
sim.setObjectParent(objhandles[numberofobj],graphHandle,false)
numberofobj=numberofobj+1
end
if j == 2 then pathPassedL[1] = pathEdgeL[1] end
if j > 2 then pathPassedL[j-1] = pathPassedL[j-2] + pathEdgeL[j-1] end
end
end
end
totalLength = pathPassedL[#pathEdgeL]
jointData=data
if #jointData <= 1 then
totalLength = 0.0
out=string.format("No available path")
else
out=string.format("Successfully parsed file!<br>File path = %s<br>Data count = %d",fname,i-1)
end
simUI.setLabelText(ui, 1003, out)
-- Begin and end nodes.
if #jointData > 0 then
objhandles[numberofobj] = sim.createPureShape(2,16,{0.05,0,0},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{jointData[1][1],jointData[1][2],0.0024})
sim.setShapeColor(objhandles[numberofobj],NULL,sim.colorcomponent_ambient_diffuse,{0,0.5,0})
sim.setObjectParent(objhandles[numberofobj],graphHandle,false)
numberofobj=numberofobj+1
end
if #jointData > 1 then
objhandles[numberofobj] = sim.createPureShape(2,16,{0.05,0,0},10,NULL)
sim.setObjectPosition(objhandles[numberofobj],-1,{jointData[#jointData][1],jointData[#jointData][2],0.0024})
sim.setShapeColor(objhandles[numberofobj],NULL,sim.colorcomponent_ambient_diffuse,{1,0,0})
sim.setObjectParent(objhandles[numberofobj],graphHandle,false)
numberofobj=numberofobj+1
end
-- Initial position.
if #jointData > 0 then
applyJoints(jh, jointData[1])
end
end
end
function interpConfig(tinterp)
local t=totalLength/vel
local q={0,0}
local reset=nil
if totalLength == 0 then
q = jointData[1]
thetaangle = 0.0
elseif (t<tinterp) then
tinterp = t
reset = true
else
for n,leng in ipairs(pathPassedL) do
if tinterp*vel <= leng then
for i=1,#q,1 do
if n == 1 then
q=jointData[n] + ((jointData[n+1]-jointData[n])/pathEdgeL[n])*(tinterp*vel)
else
q=jointData[n] + ((jointData[n+1]-jointData[n])/pathEdgeL[n])*(tinterp*vel-pathPassedL[n-1])
end
end
thetaangle = math.atan((jointData[n+1][2]-jointData[n][2])/(jointData[n+1][1]-jointData[n][1]))
if jointData[n+1][1] < jointData[n][1] then
thetaangle = thetaangle - math.pi
end
break
end
end
end
return reset,q,thetaangle
end
function setTimeSliderValue(ui,t)
local fraction=t/totalLength*vel
simUI.setSliderValue(ui,4000,10000*fraction,true)
return
end
function playPressed(ui,id)
local out=nil
if not jointData then
out=string.format("WARN: No file loaded, cannot play!")
simUI.setLabelText(ui, 1003, out)
return
end
if (playbackStatus==status.stop) then
startTime=sim.getSimulationTime()
out=string.format("Playback started from beginning!")
simUI.setLabelText(ui, 1003, out)
playbackStatus=status.play
elseif (playbackStatus==status.pause) then
local tfrac=simUI.getSliderValue(ui,4000)/10000
startTime=sim.getSimulationTime()-tfrac*totalLength/vel
out=string.format("Playback resuming from pause!")
simUI.setLabelText(ui, 1003, out)
playbackStatus=status.play
end
end
function pausePressed(ui,id)
if (playbackStatus==status.play) then
out=string.format("Playback paused!")
simUI.setLabelText(ui, 1003, out)
playbackStatus=status.pause
end
end
function stopPressed(ui,id)
out=string.format("Playback stopped!")
simUI.setLabelText(ui, 1003, out)
playbackStatus=status.stop
setTimeSliderValue(ui, 0.0)
if jointData then
local reset,q,thetaangle = interpConfig(0.0)
applyJoints(jh,q)
sim.setJointPosition(jhx,thetaangle)
end
end
function stepPressed(ui,id)
local reset=nil
local q=nil
local tfrac=nil
local t=nil
local stepSize=nil
playbackStatus=status.pause
out=string.format("Step!")
simUI.setLabelText(ui, 1003, out)
tfrac=simUI.getSliderValue(ui,4000)/10000
--if (interpFlag) then
tfrac=tfrac+0.005
if (tfrac>=1.0) then tfrac=1.0 end
t=tfrac*totalLength/vel
reset,q,thetaangle = interpConfig(t)
--else
--stepSize=1./#jointData
--tfrac=(math.floor(tfrac/stepSize)+1)*stepSize + stepSize/2.
--if (tfrac>=1.0) then tfrac=0.0 end
--t=tfrac*(#jointData*nominalDT*(1/timeMult()))
--reset,q = interpConfig(jointData, t, (1/timeMult())*nominalDT)
--end
if jointData then
applyJoints(jh, q)
sim.setJointPosition(jhx,thetaangle)
end
setTimeSliderValue(ui, t)
end
function timeSliderChange(ui,id,newVal)
local t=nil
local reset=nil
local q=nil
local resumePlay=false
local out=""
if (jointData == nil) then
out=string.format("Need to open CSV file!")
simUI.setLabelText(ui, 1003, out)
simUI.setSliderValue(ui,4000,0,true)
return
end
if (playbackStatus==status.play) then
resumePlay=true
playbackStatus=status.pause
end
t=(newVal/10000)*totalLength/vel
reset,q,thetaangle = interpConfig(t)
if jointData then
applyJoints(jh, q)
sim.setJointPosition(jhx,thetaangle)
end
if (resumePlay) then
playPressed(ui,1006)
else
playbackStatus=status.pause
end
out=string.format("Time slider dragged to %3.2f percent",newVal/10000)
simUI.setLabelText(ui, 1003, out)
return
end
function decreaseSpeed(ui,id)
local resumePlay=false
if (playbackStatus==status.play) then
resumePlay=true
playbackStatus=status.pause
end
vel=vel-multStep
if (vel <= multStep) then vel=multStep end
if (resumePlay) then playPressed(ui,1006) end
local out=string.format("%6.2f",vel)
simUI.setEditValue(ui, 1006, out)
end
function increaseSpeed(ui,id)
local resumePlay=false
if (playbackStatus==status.play) then
resumePlay=true
playbackStatus=status.pause
end
vel=vel+multStep
if (resumePlay) then playPressed(ui,1006) end
local out=string.format("%6.2f",vel)
simUI.setEditValue(ui, 1006, out)
end
function parseEnteredSpeed(ui,id,newVal)
local q=tonumber(newVal)
local out=nil
local resumePlay=false
if (q==nil) then
out=string.format("Could not parse entered number!")
simUI.setLabelText(ui, 1003, out)
return
elseif (q<multStep) then
q=multStep
out=string.format("Minimum multiplier value is %3.2f!",multStep)
simUI.setLabelText(ui, 1003, out)
end
if (playbackStatus==status.play) then
resumePlay=true
playbackStatus=status.pause
end
vel=q
if (resumePlay) then playPressed(ui,1006) end
out=string.format("%6.2f",vel)
simUI.setEditValue(ui, 1006, out)
end
applyJoints=function(jointHandles, joints)
for i=1,#jointHandles,1 do
sim.setJointPosition(jointHandles[i],joints[i])
end
end
getJoints=function(jointHandles)
local angles={0,0}
for i=1,#jointHandles,1 do
angles[i]=sim.getJointPosition(jointHandles[i])
end
return angles
end
function closeEventHandler(h)
sim.addStatusbarMessage('Window '..h..' is closing...')
simUI.hide(h)
end
if (sim_call_type==sim.syscb_init) then
-- Check if we have a controller in the scene:
xml = [[<ui closeable="false" on-close="closeEventHandler" resizable="true">
<tabs>
<tab title="CSV Playback">
<group layout="vbox">
<label text="<big> Enter the folder path, ending with a slash/backslash:</big>" id="1000" wordwrap="false" style="font-weight: bold;"/>
<group layout="hbox">
<edit value="" id="1001" />
<button text="Open Files" on-click="parseCSVFile" id="2000" />
</group>
<label text="<big> Controls</big>" id="1005" wordwrap="false" style="font-weight: bold;"/>
<group layout="grid">
<button text="Play" on-click="playPressed" />
<button text="Pause" on-click="pausePressed" />
<button text="Stop" on-click="stopPressed" />
<button text="Step" on-click="stepPressed" />
<br/>
<label text="Time Multiplier" />
<button text="Increase" on-click="increaseSpeed" />
<button text="Decrease" on-click="decreaseSpeed" />
<edit id="1006" value="0.1" oneditingfinished="parseEnteredSpeed" />
</group>
<label text="Time" />
<hslider id="4000" tick-position="below" tick-interval="500" minimum="0" maximum="10000" on-change="timeSliderChange"/>
<label text="<big> Messages:</big>" id="1002" wordwrap="false" style="font-weight: bold;"/>
<group layout="vbox">
<label value="" id="1003" wordwrap="true" />
</group>
</group>
<stretch />
</tab>
</tabs>
</ui>]]
ui=simUI.create(xml)
-- get joints:
jh={-1,-1}
jh[1] = sim.getObjectHandle('World_x_joint')
jh[2] = sim.getObjectHandle('World_y_joint')
jhx = sim.getObjectHandle('World_theta_joint')
envHandle=sim.getObjectHandle('env')
graphHandle=sim.getObjectHandle('graph')
kilobotHandle=sim.getObjectHandle('KilobotD')
multStep=0.02
jointData={}
vel = 0.1
pathEdgeL={}
pathPassedL = {}
totalLength = 0.0
objhandles={}
numberofobj=1
fname=""
startTime=0.0
-- flags
status={stop=0, play=1, pause=2}
playbackStatus=status.stop
delayCount=0
delayRefCount=60
end
if (sim_call_type==sim.syscb_actuation) then
local t=0.0
local reset=nil
local q=nil
local thetaangle=0.0
if (playbackStatus==status.play) then
t=(sim.getSimulationTime()-startTime)
if (jointData) then
reset,q,thetaangle = interpConfig(t)
else
out=string.format("WARN: No file loaded, cannot play!")
simUI.setLabelText(ui, 1003, out)
end
if reset and #jointData > 0 then
if (delayCount>=delayRefCount) then
delayCount=delayCount+1
applyJoints(jh, jointData[1])
out=string.format("Animation reset!")
simUI.setLabelText(ui, 1003, out)
if (delayCount>=2*delayRefCount) then
delayCount=0
startTime=sim.getSimulationTime()
end
else
applyJoints(jh, jointData[#jointData])
out=string.format("CSV end!")
simUI.setLabelText(ui, 1003, out)
delayCount=delayCount+1
end
elseif (q) then
local pre = getJoints(jh)
applyJoints(jh, q)
sim.setJointPosition(jhx,thetaangle)
setTimeSliderValue(ui,t)
end
end
end
if (sim_call_type==sim.syscb_cleanup) then
simUI.destroy(ui)
end
I need to add three excel csv files to make the simulation work
file one edges
# edges.csv file for V-REP kilobot motion planning scene.
# All lines beginning with a # are treated as a comment and ignored.
# Each line below is of the form
# ID1 ID2 cost
# where ID1 and ID2 are the IDs of the nodes connected by the edge and
# cost is the cost of traversing that edge (in either direction).
12 11 0.214
12 10 0.3135
12 8 0.5014
11 10 0.1778
11 9 0.4454
10 7 0.2586
9 8 0.2005
9 6 0.2796
9 5 0.5994
8 4 0.48
7 4 0.3417
5 2 0.179
4 3 0.3517
4 2 0.2289
3 2 0.2169
3 1 0.2903
2 1 0.422
7 5 0.4402
5 4 0.11
File two nodes
# All lines beginning with a # are treated as a comment and ignored.
# Each line below has the form
# ID x y heuristic-cost-to-go
# where ID is the unique integer ID number of the node (1 through N)
# (x y) is the location of the node in the plane and heuristic-cost-to-go
# is an optimistic estimate of the path length from that node to the
# goal node as needed by A* search.
1 -0.5 -0.5 1.4142
2 -0.09 -0.4 1.0762
3 -0.285 -0.305 1.1244
4 0.0575 -0.225 0.8494
5 -0.0525 -0.0175 0.7604
6 -0.37 0.3 0.8927
7 0.3525 -0.0525 0.5719
8 0.0625 0.255 0.5014
9 -0.1 0.3725 0.6134
10 0.4275 0.195 0.3135
11 0.345 0.3525 0.214
12 0.5 0.5 0
File three obstacles
# obstacles.csv file for V-REP kilobot motion planning scene.
# All lines beginning with a # are treated as a comment and ignored.
# Below is a specification of a set of cylindrical obstacles in the form
# x y diameter
# where (x y) is the center of the cylinder and diameter is its diameter.
-0.285 -0.075 0.33
0.365 -0.295 0.27
0.205 0.155 0.15
file four path
# path.csv file for V-REP kilobot motion planning scene.
# All lines beginning with a # are treated as a comment and ignored.
# Below is a solution path represented as a sequence of nodes of the graph.
1 3 4 7 10 12
How do I get these files into coppeliasimedu do simulate the required simulation