Как заставить моего робота (пионера) двигаться по пути в VREP - PullRequest
Новая
       91

Как заставить моего робота (пионера) двигаться по пути в VREP

0 голосов
/ 26 декабря 2018

Я создаю робота для планирования пути и закодировал его в дочернем скрипте Vrep, но робот не двигается.Я прилагаю код.пожалуйста, исправьте это и помогите мне.Это кодируется на языке сценариев LUA.Используемый робот является пионером.Симулятор Vrep Язык кодирования - LUa. 

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")
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=sqrt((path_pos[1]^2 + (path_pos[2]^2))
    phi=atan(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
    simSetJointTargetVelocity(rm,-omega_right)
    simSetJointTargetVelocity(lm,-omega_left)

    if (dis<0.1) then
        pos_on_path=pos_on_path + 0.01 
    end
    sim.wait(0.0025,true)
end
...