Quadricopter following path problems

Typically: "How do I... ", "How can I... " questions
Post Reply
ogyo
Posts: 8
Joined: 14 Feb 2017, 20:31

Quadricopter following path problems

Post by ogyo » 05 Nov 2017, 22:12

Hi there. I've asked similar questions regarding this topic earlier, but no progress since then. I'm currently working on a project involving a quadcopter moving autonomously in unknown environments with different algorithms.

I'm using Python remote API to generate a list of coordinates that the quad should follow. Then, I'm creating a path which I move the green sphere onto. The problem is this: When the first time the path is generated, the sphere moves along it, but when the path is regenerated, then the sphere just stands there doing nothing.

Video of the problem: https://youtu.be/ef4xdlZkD8w

Main quad script:

Code: Select all

-- QUADRICOPTER SCRIPT VREP --
-- CHILDSCRIPT INITIALIZATION CODE:
if (sim_call_type==sim_childscriptcall_initialization) then
    -- Make sure we have version 2.4.13 or above (the particles are not supported otherwise)
    simExtRemoteApiStart(19997) -- start RemoteAPI

    v=simGetInt32Parameter(sim_intparam_program_version)
    if (v<20413) then
        simDisplayDialog('Warning','The propeller model is only fully supported from V-REP version 2.4.13 and above.&&nThis simulation will not run as expected!',sim_dlgstyle_ok,false,'',nil,{0.8,0,0,0,0,0})
    end
    
-- HANDLE AND VREP-SIMULATION CODE:
    -- Detatch the manipulation sphere:
    targetObj=simGetObjectHandle('Quadricopter_target') -- get targetObj handle
    simSetObjectParent(targetObj, -1, true)
    --simSetObjectParent(targetObj,-1,true)

     -- get the targetObj parent
    shouldWeStopCounting = 0
    simulationStep = 0
    d=simGetObjectHandle('Quadricopter_base') -- get Quadricopter_base handle
    target = targetObj
   

    particlesAreVisible=simGetScriptSimulationParameter(sim_handle_self,'particlesAreVisible')
    simSetScriptSimulationParameter(sim_handle_tree,'particlesAreVisible',tostring(particlesAreVisible))
    simulateParticles=simGetScriptSimulationParameter(sim_handle_self,'simulateParticles')
    simSetScriptSimulationParameter(sim_handle_tree,'simulateParticles',tostring(simulateParticles))
    
    propellerScripts={-1,-1,-1,-1}
    for i=1,4,1 do
	propellerScripts[i]=simGetScriptHandle('Quadricopter_propeller_respondable'..i)
    end

    heli=simGetObjectAssociatedWithScript(sim_handle_self)
    
    fakeShadow=simGetScriptSimulationParameter(sim_handle_self,'fakeShadow')
    if (fakeShadow) then
        shadowCont=simAddDrawingObject(sim_drawing_discpoints+sim_drawing_cyclic+sim_drawing_25percenttransparency+sim_drawing_50percenttransparency+sim_drawing_itemsizes,0.2,0,-1,1)
    end
    
     -- CAMERA INITIALIZATION:
    qc_camera_init = function(inInts,inFloats,inStrings,inBuffer)
        -- Prepare 2 floating views with the camera views:
        floorCam=simGetObjectHandle('Quadricopter_floorCamera')
        frontCam=simGetObjectHandle('Quadricopter_frontCamera')
        floorView=simFloatingViewAdd(0.9,0.9,0.2,0.2,0)
        frontView=simFloatingViewAdd(0.7,0.9,0.2,0.2,0)
        simAdjustView(floorView,floorCam,64)
        simAdjustView(frontView,frontCam,64)
    end
end -- end initialization

-- CHILDSCRIPTCALL CLEANUP:
if (sim_call_type==sim_childscriptcall_cleanup) then 
    simRemoveDrawingObject(shadowCont)
    simFloatingViewRemove(floorView)
    simFloatingViewRemove(frontView)
end -- end cleanup
  
-- CHILDSCRIPTCALL ACTUATION:
if (sim_call_type==sim_childscriptcall_actuation) then
-- Handle and vrep-sim code:
    
    s=simGetObjectSizeFactor(d)
    
    pos=simGetObjectPosition(d,-1)
    if (fakeShadow) then
        itemData={pos[1],pos[2],0.002,0,0,1,0.2*s}
        simAddDrawingObjectItem(shadowCont,itemData)
    end

-- Functions:
    qc_Get_vx = function(inInts, inFloats,  inStrings, inBuffer)
        --sp=simGetObjectPosition(targetObj,d)
        m=simGetObjectMatrix(d,-1)
        vx={1,0,0}
        vx=simMultiplyVector(m,vx)
        vy={0,1,0}
        vy=simMultiplyVector(m,vy)

        return {}, vx, {}, ''
    end

    qc_Get_vy = function(inInts, inFloats,  inStrings, inBuffer)
   
        --sp=simGetObjectPosition(targetObj,d)
        m=simGetObjectMatrix(d,-1)
        vx={1,0,0}
        vx=simMultiplyVector(m,vx)
        vy={0,1,0}
        vy=simMultiplyVector(m,vy)

        return {}, vy, {}, ' '
    end

    qc_Get_Object_Matrix = function(inInts, inFloats,  inStrings, inBuffer)
        --sp=simGetObjectPosition(targetObj,d)
        d=simGetObjectHandle('Quadricopter_base')
        m=simGetObjectMatrix(d,-1)
        local m_string = simPackFloatTable(m)
        simSetStringSignal('mtable',m_string)
    return {}, vx, {}, ''
    end

    qc_get_absolute_coordinates_sensor = function( inInts, inFloats, inStrings, inBuffer)

        local sensorHandle = 0
        if (inInts[1] == 0) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_back')
        end
        if (inInts[1] == 1) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_front')
        end
        if (inInts[1] == 2) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_left')
        end
        if (inInts[1] == 3) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_right')
        end
        if (inInts[1] == 4) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_up')
        end
        if (inInts[1] == 5) then
            sensorHandle = simGetObjectHandle('Proximity_sensor_down')
        end
        
        local position = inFloats
        local matrix = simGetObjectMatrix(sensorHandle, -1)
        local absPosition = {1,2,3,0}
        absPosition = simMultiplyVector(matrix, position)
        return inInts ,absPosition, {}, ' '
    end

   qc_get_object_size = function( inInts, inFloats, inStrings, inBuffer)
    local min_x = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_min_x)
    local max_x = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_max_x)
    local min_y = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_min_y)
    local max_y = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_max_y)
    local min_z = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_min_z)
    local max_z = simGetObjectFloatParameter(inInts[1], sim_objfloatparam_modelbbox_max_z)

    a = simGetObjectSizeValues(inInts[1])    
    return {min_x, max_x, min_y, max_y, min_z, max_z}, {}, {}, ''
    end

    qc_get_position_on_path = function( inInts, inFloats, inStrings, inBuffer)
    
        inFloats = simGetPositionOnPath(simGetObjectHandle('Path'), 0.95)
        return inInts, inFloats, {}, ''
    end
        

   -- Send the desired motor velocities to the 4 rotors:
   qc_propeller_v = function(inInts,particlesTargetVelocities,inStrings,inBuffer)
	for i=1,4,1 do
	    simSetScriptSimulationParameter(propellerScripts[i],'particleVelocity',particlesTargetVelocities[i])
    end
    return {}, {}, {}, ''
   end
	
end -- end actuation
Green sphere child script:

Code: Select all

threadFunction=function()
target=simGetObjectAssociatedWithScript(sim_handle_self)
simulationStep = 0
shouldWeStopCounting = 0 
console = simAuxiliaryConsoleOpen('Debug title', 30, 1)
path = simCreatePath(-1)
generate_path = function(x,y,z, g)
              
              simCutPathCtrlPoints(path, -1, 0)
              file = io.open("C:\\Users\\path.txt", "r")          --reads csv file
              ctrlPointsBufferTable = {}                          --creates empty buffer table
              i = 0;                                              --csv line counter
              for line in io.lines("C:\\Users\\path.txt") do
              i = i + 1
              end
              numOfdata = i * 16                              --Number of data in csv file(each line contains 11 data)

              for n =1,numOfdata do                               --stores ctrl points' data into the buffer table
              ctrlPointsBufferTable[n] =file:read('*n'); file:read(1)
              end
                simAuxiliaryConsolePrint(console,'\n created path ')
               simInsertPathCtrlPoints(path, 2, 0, i, ctrlPointsBufferTable)
                simAuxiliaryConsolePrint(console,simGetPathLength(path))
              return x,y,z,g
    end
while (simGetSimulationState()~=sim_simulation_advancing_abouttostop) do
        simFollowPath(target, path, 1, 0 , 10, 0.1)
        
    end


    -- Put your main code here
     
end

-- Put some initialization code here:
simSetThreadSwitchTiming(2) -- Default timing for automatic thread switching

   

-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
	simAddStatusbarMessage('Lua runtime error: '..err)
end

-- Put some clean-up code here:
Python code that activates the path regeneration:

Code: Select all

    def write_path_to_file(self,coordinates):
        #reset quad target and path
        print "Started writing file"
        [returnCode, targetObj] = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target',
                                                           vrep.simx_opmode_blocking)
        [returnCode, qc_handle] = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_blocking)
        [returnCode, qc_position] = vrep.simxGetObjectPosition(self.clientID, qc_handle, -1, vrep.simx_opmode_blocking)
        # vrep.simxSetObjectPosition(self.clientID,targetObj,-1,qc_position, vrep.simx_opmode_oneshot)


        open('path.txt', 'w').close()
        file = open('path.txt', 'w')

        for coordinate in coordinates:
            file.write(str(coordinate[0]) + ',' + str(coordinate[1]) + ',' + str(coordinate[2])
                       + ',0.000000,90.000003,90.000003,1.000000,15,0.500000,0.500000,0.000000,0,0.000000,0.000000,0.000000,0.000000\n')
        file.close()

        vrep.simxSetStringSignal(self.clientID, 'newTrajectory', "true", vrep.simx_opmode_blocking)
        vrep.simxSetStringSignal(self.clientID, 'newTrajectoryResetValues', "true", vrep.simx_opmode_blocking)
        [returnCode, self.pathHandle] = vrep.simxGetObjectHandle(self.clientID, 'Path', vrep.simx_opmode_blocking)
        vrep.simxCallScriptFunction(self.clientID,'Quadricopter_target', vrep.sim_scripttype_childscript,'generate_path',[],[],[],bytearray(), vrep.simx_opmode_blocking)

        print "Finished writing file"
        return True
Could you point me where I'm making the mistake? :)
Thanks upfront :)

coppelia
Site Admin
Posts: 7267
Joined: 14 Dec 2012, 00:25

Re: Quadricopter following path problems

Post by coppelia » 07 Nov 2017, 09:01

Hello,

not sure where the mistake is, but I can't help you with algorithms unfortunately, since this is very personal how an algorithm is written, and difficult/time-consuming for another person to look into it. I'm happy to help with V-REP functionality and directed questions.

Cheers

kemz
Posts: 6
Joined: 07 Jul 2019, 15:48

Re: Quadricopter following path problems

Post by kemz » 10 Jul 2019, 21:15

Hello admin,
How do I send forces to Vrep via python using simxsetstringsignal? Please kindly help with this

coppelia
Site Admin
Posts: 7267
Joined: 14 Dec 2012, 00:25

Re: Quadricopter following path problems

Post by coppelia » 15 Jul 2019, 06:39

Hello,

if you have a non-threaded child script, then something like:

Code: Select all

function sysCall_actuation()
    local f=sim.getStringSignal('myForce')
    if f then
        local sim.unpackFloatTable(f)
        sim.addForceAndTorque(shapeHandle,force,nil)
    end
end
You can of course also call a special function in your script via simxCallScriptFunction.

Cheers

kemz
Posts: 6
Joined: 07 Jul 2019, 15:48

Re: Quadricopter following path problems

Post by kemz » 17 Jul 2019, 10:05

Thank you for your help Coppelia, I applied the force, however the quadcopter just shoots right up . How do I make it just hover? I tried applying less force, however this just makes the quadcopter fall. Thank you again for your help

coppelia
Site Admin
Posts: 7267
Joined: 14 Dec 2012, 00:25

Re: Quadricopter following path problems

Post by coppelia » 18 Jul 2019, 14:55

Try to practice first with a cube that you make float over the floor. You will have to:
  • add a force that compensates for the weight of the cube
  • add a force that corrects for positional errors (e.g. if you want your cube to be able to move to different heights). A P-controller is probably not going to make it. You'll probably need a PD-controller (where the D-component represents the drag)
Cheers

kemz
Posts: 6
Joined: 07 Jul 2019, 15:48

Re: Quadricopter following path problems

Post by kemz » 25 Jul 2019, 14:53

Hello,
I got the cube and then the quadcopter to hover by applying forces that compensate for the weight. I can move the quadcopter forward using sim.setObjectFloatParameter(16,3000,velocity[1]). Do I still need to add the PID? And do you have an example code of how to implement PID on the quadcopter?

Thanks

Post Reply