Quadcopter following the path

[ArthurS]

Hello, I need to create a function that will allow the drone to move along points in space.
[There is an array of points in space. The function alternately takes points from it and builds a path from the drone to the point along them and the drone flies and stops at the end of the path and so on again in a loop until the points run out]

Now to the problems of my scripts:
I am using the simple path sim.createPath: local path=sim.createPath(pathArray,0,100,0,0,{0,0,1})
But there are 2 problems: the path for a large number of points is built even with smoothing = 0 is very crooked and does not go into some points. And the second problem: it is not clear how to detect the drone hitting the right point on the way. Therefore, I need a function that builds a path only by 2 points (to avoid curvature) and I need to somehow stop the drone at certain points for some time on the path and then resume moving (exactly coordinatewise).
My drone move code:

Code: Select all

if(signal=='true' and Start==false) then
        println('Connection')
         local pointscript=sim.getScriptAssociatedWithObject(sim.getObjectHandle('Scripts'))
         ClastersTable=sim.callScriptFunction("returnClastersTable@"..sim.getScriptName(pointscript),pointscript,ClastersTable)
        ObjectToFollowPath=sim.getObjectHandle(sim.handle_self)
        path=sim.getObjectHandle('Path1')
        pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(path,'PATH'))
        local m=Matrix(#pathData//7,7,pathData)
        
        pathPositions=m:slice(1,1,m:rows(),3):data()
      
        pathQuaternions=m:slice(1,4,m:rows(),7):data()
        pathLengths,totalLength=sim.getPathLengths(pathPositions,3)
        Start=true
    end
    
    if(Start) then

        local t=sim.getSimulationTime()
      --  print(Time)
        --print(t)
        if(IsGo)then
    
        
        posAlongPath=posAlongPath+velocity*(t-previousSimulationTime)
        posAlongPath=posAlongPath % totalLength
        local pos=sim.getPathInterpolatedConfig(pathPositions,pathLengths,posAlongPath)
        local quat=sim.getPathInterpolatedConfig(pathQuaternions,pathLengths,posAlongPath,nil,{2,2,2,2})
        for i=1,#ClastersTable,1 do
       local po= sim.getObjectPosition(sim.getObjectHandle(ClastersTable[1][i]),-1) 
        sim.setObjectPosition(ObjectToFollowPath,path,pos)
        sim.setObjectQuaternion(ObjectToFollowPath,path,quat)
        previousSimulationTime=t
        end
        if(t-Time>=10 and IsGo==false)then
        --print('T')
        IsGo=true
        Time=t
        else
        previousSimulationTime=t
        end 
    else
        previousSimulationTime=sim.getSimulationTime()
    end

I really appreciate your help

[coppelia]

Hello,

if you need a non-interpolated/smoothed path, then specify the exact same number of points for the path subdivision as for the path points. E.g. in case of 4 path points, do something like:

Code: Select all

function sysCall_init()
    local pts={0,0,0,0,0,0,1,1,0,0,0,0,0,1,1,1,0,0,0,0,1,0,1,0,0,0,0,1}
    sim.createPath(pts,0,4,0,0,{0,0,1})
end

Otherwise, you can always create your own path data, and render it yourself with sim.addDrawingObject, since path objects are more meant to be used when interaction with the user is desired (e.g. manually add/remove a path point, move a path point, etc.). Following a simple example using sim.addDrawingObject:

Code: Select all

function sysCall_init()
    local pts={0,0,0,1,0,0,1,1,0,0,1,0}
    local cont=sim.addDrawingObject(sim.drawing_lines,2,0,-1,1000,{1,0,0})
    for i=0,#pts//3-2,1 do
        sim.addDrawingObjectItem(cont,{pts[3*i+1],pts[3*i+2],pts[3*i+3],pts[3*(i+1)+1],pts[3*(i+1)+2],pts[3*(i+1)+3]})
    end
end

Cheers

[ArthurS]

Thank you for the answer . And how to make the drone now move along this line and along the points? Can you give us some sample code?

[coppelia]

It is almost the same. In the example I gave, we create a path data structure that only contains positions. So you can immediately write pathPositions=pts and use your own code (but remove occurences of the orientation part).

If you also need orientation, then simply create another path data structure that contains the orientation, as quaternions, e.g.:

Code: Select all

function sysCall_init()
    pathPositions={0,0,0,1,0,0,1,1,0,0,1,0} -- 4 positions
    pathQuaternions={0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1} -- 4 quaternions
    local cont=sim.addDrawingObject(sim.drawing_lines,2,0,-1,1000,{1,0,0})
    for i=0,#pts//3-2,1 do
        sim.addDrawingObjectItem(cont,{pts[3*i+1],pts[3*i+2],pts[3*i+3],pts[3*(i+1)+1],pts[3*(i+1)+2],pts[3*(i+1)+3]})
    end

    pathLengths,totalLength=sim.getPathLengths(pathPositions,3)
    -- etc.
end

Cheers

[ArthurS]

thank you for your help. as I understood sim.drawingObject is just for the perception of the path with the eyes. and the very logic of the movement does not change. this is good. And how then to make the drone stop at the point of the way. This is not yet possible because he skips, for example, the last point and immediately goes to the beginning of the path. I hacked the code and stop it when pathposition> pathlength and put it at the extreme point. on the simulation looks quite crooked ((

[coppelia]

That entirely depends on how you implement things. The simplest would be to see the path as succession of segments. Then simply have your drone follow each segment at a time, i.e. you can simply break-down the path into individual paths for instance.

Cheers

[ArthurS]

yes I do. multithreaded I run the function which creates a path to 1 point from the array and iterates over all the points in this way.
By the way, can you tell me how matters stand with the privileged methods in coppeliasim? I have 3 drones constantly reading 1 and the same array and changing it. Maybe there is some kind of logic like lock () unlock () like in c ++?

[coppelia]

I have 3 drones constantly reading 1 and the same array and changing it. Maybe there is some kind of logic like lock () unlock () like in c ++?

How do you do that? My guess is you have 3 separate scripts, and all of them are reading a same array. But where is that array located?

You can easily share data in various ways. The most common two are:

1. use a packed string signal which holds your data. Data will be volatile, i.e. not permanently stored. Here a simple example:

Code: Select all

function readCommonTable()
    local data=sim.getStringSignal('myCommonTable')
    if data then
        data=sim.unpackTable(data)
    else
        data={}
    end
    return data
end

function writeCommonTable(tableData)
    sim.setStringSignal('myCommonTable',sim.packTable(tableData))
end

2. use a packed custom data block which holds your data inside of a given scene object. If the scene/object is saved, then the data will also be saved, i.e. permanently stored. Here a simple example:

Code: Select all

function readCommonTable(objectHandle)
    local data=sim.readCustomDataBlock(objectHandle,'myCommonTable')
    if data then
        data=sim.unpackTable(data)
    else
        data={}
    end
    return data
end

function writeCommonTable(objectHandle,tableData)
    sim.writeCustomDataBlock(objectHandle,'myCommonTable',sim.packTable(tableData))
end

So your 3 drones would read/write a common data structure with above code. If your code runs non-threaded, then you have not to worry about locking/unlocking access to that data. If your code runs threaded, then you can temporarily forbid thread switched, e.g. as in following example:

Code: Select all

    local sl=sim.setThreadAutomaticSwitch(false)
    local data=readCommonTable()
    ... modify that table here
    writeCommonTable(data)
    sim.setThreadAutomaticSwitch(sl)

Cheers

[ArthurS]

my drones go to a dummy called "Scripts". there is an array of points (handles).
each drong has a multi-threaded child script.

[ArthurS]

when more than 3 drones are added, the studio starts to lag. even with multithreading. The computer does not even overclock. how can this be solved?