Path Tracking with pioneer P3-DX with vision sensor

[ImDike134@]

Hello,
I'm working on a simulation project with Coppeliasim Edu.

The project involves simulating a Path Tracking with the Pioneer P3-DX robot equipped with a vision sensor. I started by drawing the path in black in a circular shape. I imported the Pioneer P3-DX robot and added some code, but after the simulation, the robot does not follow the path correctly; it follows its own route. I am stuck at this point.

I wanted to ask for your help if you have ever worked on a similar project or have some knowl

[coppelia]

Hello,

when you say you are stuck, what does it mean exactly? Is there any error message? Can you share your scene?

There is a similar tutorial available.

Cheers

[ImDike134@]

[youtube]https://youtu.be/VIhEUS-MOY0[/youtube]
here is my code for the simulation

function sysCall_init()

-- Motor and sensor management

motGauche = sim.getObjectHandle("Pioneer_p3dx_leftMotor")
motDroit = sim.getObjectHandle("Pioneer_p3dx_rightMotor")
ir = sim.getObjectHandle("Vision_sensor")

-- Initialization of parameters

v = 0.2 -- linear speed of the robot
k = 0.6 --controller proportionality factor
b = 0.33 / 2 --half the distance between the two wheels
R = 0.1 --robot wheel radius

-- Graph Initialization

graph = sim.getObjectHandle("/Graph")
objectPosX = sim.addGraphStream(graph, 'object pos x', 'm', 1)
objectPosY = sim.addGraphStream(graph, 'object pos y', 'm', 1)
sim.addGraphCurve(graph, 'object pos x/y', 2, {objectPosX, objectPosY}, {0, 0}, 'm by m')

end

function sysCall_actuation()

--Capturing the vision sensor image

--ground gray level measurement
Ng = sim.getVisionSensorImage(ir + sim.handleflag_greyscale)

-- Calculation of motor errors and speeds

--calculation of robot rotation speed
w = -k * (Ng[1] - 0.4)

--calculation of the linear speeds of the left and right wheels
vg = v - (b * w)
vd = v + (b * w)

--calculation of the angular velocities of the left and right wheels
wg = vg / R
wd = vd / R

-- Updating motor speeds
sim.setJointTargetVelocity(motGauche, wg)
sim.setJointTargetVelocity(motDroit, wd)

-- Recording robot coordinates
pos = sim.getObjectPosition(sim.getObjectHandle('Pioneer_p3dx'), -1)

-- Chart update
sim.setGraphStreamValue(graph, objectPosX, pos[1])
sim.setGraphStreamValue(graph, objectPosY, pos[2])
end

function sysCall_cleanup()

end

[ImDike134@]

Based on the reference you provided, I was able to improve my code, but after execution, I am getting an error saying:
string "/Pioneer_p3dx@simulationScript"]:30: attempt to compare number with table
stack traceback:
[string "/Pioneer_p3dx@simulationScript"]:30: in function 'sysCall_actuation
here is my new code :

Code: Select all

function sysCall_init()
    -- Initialize motors and the vision sensor
    motGauche = sim.getObjectHandle("Pioneer_p3dx_leftMotor")
    motDroit = sim.getObjectHandle("Pioneer_p3dx_rightMotor")
    ir = sim.getObjectHandle("Vision_sensor")
    
    -- Initialize drawing object for tracking the robot's path
    robotTrace = sim.addDrawingObject(sim.drawing_linestrip + sim.drawing_cyclic, 2, 0, -1, 200, {1, 1, 0}, nil, nil, {1, 1, 0})

    -- Initialize parameters
    v = 0.2 -- Linear velocity
    k = 0.6 -- Proportional gain for the controller
    b = 0.33 / 2 -- Coefficient for the speed adjustment
    R = 0.1 -- Wheel radius
    
    -- Initialize graph for visualizing the robot's position
    graph = sim.getObjectHandle("/Graph")
    objectPosX = sim.addGraphStream(graph, 'object pos x', 'm', 1)
    objectPosY = sim.addGraphStream(graph, 'object pos y', 'm', 1)
    sim.addGraphCurve(graph, 'pioneer position x/y', 2, {objectPosX, objectPosY}, {0, 0}, 'm by m')
end

function sysCall_sensing()
    -- Mark the robot's path and track the actual path it follows
    local p = sim.getObjectPosition(sim.getObjectHandle('Pioneer_p3dx'), -1)
    sim.addDrawingObjectItem(robotTrace, p)
end

function sysCall_actuation()
    -- Capture the image from the vision sensor
    local result, data = sim.getVisionSensorImage(ir + sim.handleflag_greyscale)
    if result >= 0 then
        -- Check the structure of 'data'
        if type(data) == "table" and #data >= 3 then
            local width = data[1] -- Image width
            local height = data[2] -- Image height
            local imageData = data[3] -- Image data starts from index 3

            -- Verify that 'imageData' is a table
            if type(imageData) == "table" then
                -- Calculate the average gray level of the image
                local averageGray = 0
                local numPixels = width * height
                
                for i = 1, numPixels do
                    averageGray = averageGray + imageData[i]
                end
                averageGray = averageGray / numPixels

                -- Calculate error and motor speeds
                local error = (averageGray - 0.5) * 2 -- Normalize around 0
                local w = -k * error
                local vg = v - (b * w)
                local vd = v + (b * w)
                local wg = vg / R
                local wd = vd / R

                -- Update motor velocities
                sim.setJointTargetVelocity(motGauche, wg)
                sim.setJointTargetVelocity(motDroit, wd)

                -- Record the robot's coordinates
                local pos = sim.getObjectPosition(sim.getObjectHandle('Pioneer_p3dx'), -1)
                
                -- Update the graph
                sim.setGraphStreamValue(graph, objectPosX, pos[1])
                sim.setGraphStreamValue(graph, objectPosY, pos[2])
            else
                print("Error: imageData is not a table.")
            end
        else
            print("Error: The data returned by getVisionSensorImage is incorrect.")
        end
    else
        -- If reading fails, stop the robot or adjust safety behaviors
        sim.setJointTargetVelocity(motGauche, 0)
        sim.setJointTargetVelocity(motDroit, 0)
    end
end

function sysCall_cleanup()
    -- Cleanup if necessary
end

[coppelia]

You haven't posted all the code: on line 30 I can't find the incriminating code. My guess it is the line:

Code: Select all

if result >= 0 then

Noticed however that sim.getVisionSensorImage returns a table.

By the way, do not use the deprecated sim.getVisionSensorImage, use instead sim.getVisionSensorImg

Cheers

[ImDike134@]

Thank You for all, i soleved the problem, everything IS okay