Question

我创建了一个路径规划机器人，并在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

如何使我的机器人（先锋）沿着VREP中的路径移动

0 个答案: