Path planning

Typically: "How do I... ", "How can I... " questions
Post Reply
Gopi Sumanth
Posts: 3
Joined: 27 Dec 2018, 08:01

Path planning

Post by Gopi Sumanth » 28 Dec 2018, 15:14

I am trying to simulate path planning. the code is

Code: Select all

function sysCall_init() 
    --usensors={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}
    --for i=1,16,1 do
      --  usensors[i]=sim.getObjectHandle("Pioneer_p3dx_ultrasonicSensor"..i)
    --end
    pos_on_path =0
    dis=0
    motorLeft=sim.getObjectHandle("Pioneer_p3dx_leftMotor")
    motorRight=sim.getObjectHandle("Pioneer_p3dx_rightMotor")
    path_plan_handle=simGetPathPlanningHandle("PathPlanningTask0")
    robot_handle=sim.getObjectHandle("Pioneer_p3dx")
    path_handle=sim.getObjectHandle("Path")
    planstate=simSearchPath(path_plan_handle,5)
    start_dummy_handle=sim.getObjectHandle("Start")
    --noDetectionDist=0.5
    --maxDetectionDist=0.2
    --detect={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
    --braitenbergL={-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
    --braitenbergR={-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
    --v0=2
end
-- This is a very simple EXAMPLE navigation program, which avoids obstacles using the Braitenberg algorithm


function sysCall_cleanup() 
 
end 

function sysCall_actuation() 
    robot_pos=sim.getObjectPosition(robot_handle,-1)
    path_pos=sim.getPositionOnPath(path_handle,pos_on_path)

    sim.setObjectPosition(start_dummy_handle,-1,path_pos)
    m=sim.getObjectMatrix(robot_handle,-1)
    m=sim.invertMatrix(m)
    path_pos=sim.multiplyVector(m,path_pos)
    
    dis=math.sqrt( (path_pos[1]^2) + (path_pos[2]^2) )
    phi=math.atan2(path_pos[2],path_pos[1])


    --Lua Code
    if(pos_on_path<1) then
        v_des=0.1
        om_des=0.8*phi
    else
        v_des=0
        om_des=0
    end
    d=0.06 --wheels separation
    v_r=(v_des+d*om_des)
    v_l=(v_des-d*om_des)

    r_w=0.0275 --wheel radius
    omega_right=v_r/r_w
    omega_left=v_l/r_w
    sim.setJointTargetVelocity(rm,-omega_right)
    sim.setJointTargetVelocity(lm,-omega_left)
    
    if (dis<0.1) then
        pos_on_path=pos_on_path + 0.01
    else
        
    end

    sim.wait(0.0025,true)

end 
my code is reporting error in
path_pos=sim.multiplyVector(m,path_pos)
line. Also my robot is not moving. can someone please help me where i am going wrong?

carolina_rutilima
Posts: 6
Joined: 23 Jul 2018, 10:50

Re: Path planning

Post by carolina_rutilima » 31 Jan 2019, 17:14

Did you find the problem?

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

Re: Path planning

Post by coppelia » 04 Feb 2019, 13:23

You need to check what arguments you are providing to that function and then debug

Cheers

Post Reply