YouBot Movement

[razer200033]

Hi!

I'm trying to make my YouBot travel a certain distance and then stop.

I am importing a csv file which contains x y coordinates. After that, I want the robot to start and then stop once the required distance is achieved. In the code which I'm using, the robot doesn't stop, it keeps on going.

The code is as follows:

Code: Select all

    b=ctrlPointsBufferTable[k+6]-ctrlPointsBufferTable[k-1]             --distance to travel from csv file
    function sysCall_sensing()
    local p=sim.getObjectPosition(self,-1)
    local d=0; for i=1,3 do d=d+math.pow(p[i]-lastPosition[i],2) end; d=math.sqrt(d)
    lastPosition=p
    distance=distance+d                                               --distance travelled by robot
    print (distance)
        if b <0 then
        b=-b
        end
        if distance<b then setMovement(-1,0,0)         --if distance travelled is less than required keep going
        elseif b==distance then 
        setMovement(0,0,0)             --if distance travelled is equal to required then stop
        end
    end

Thanks in advance!

[coppelia]

Hello,

have a look at the demo scene scenes/pathPlanning/mobileRobotPathPlanning.ttt, where you can see how the robot is following a calculated path. The basic idea is to have a target object move along the path, and have your robot follow the target object, similar to the purple sphere in the demo scene I mention.

Cheers

[razer200033]

Thanks for getting back.

Is there any simpler way to achieve this? I can't quite understand the script you mentioned above.

Why isn't the code that I attached working? The robot should stop once the distance travelled equals the distance it should travel(from table).

[coppelia]

Here a very simple way to travel from current position to another. Orientation of the robot is not taken into account, you'll have to add that yourself in a similar way:

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
    wheelJoints={-1,-1,-1,-1} -- front left, rear left, rear right, front right
    wheelJoints[1]=sim.getObject('./rollingJoint_fl')
    wheelJoints[2]=sim.getObject('./rollingJoint_rl')
    wheelJoints[3]=sim.getObject('./rollingJoint_rr')
    wheelJoints[4]=sim.getObject('./rollingJoint_fr')
    ref=sim.getObject('./youBot_ref')
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

function setMovement(forwBackVel,leftRightVel,rotVel)
    -- Apply the desired wheel velocities:
    sim.setJointTargetVelocity(wheelJoints[1],-forwBackVel-leftRightVel-rotVel)
    sim.setJointTargetVelocity(wheelJoints[2],-forwBackVel+leftRightVel-rotVel)
    sim.setJointTargetVelocity(wheelJoints[3],-forwBackVel-leftRightVel+rotVel)
    sim.setJointTargetVelocity(wheelJoints[4],-forwBackVel+leftRightVel+rotVel)
end

function coroutineMain()
    travelTo({1,2})
    travelTo({0,0})
end

function limitSpeed(s)
    for i=1,2,1 do
        if s[i]~=0 then
            local sign=s[i]/math.abs(s[i])
            s[i]=sign*math.min(math.max(math.abs(s[i]),0.3),1)
        end
    end
end

function travelTo(target)
    local t=Vector3({target[1],target[2],0})
    while true do
        local p=Vector3(sim.getObjectPosition(ref,-1))
        local dx=t:sub(p)
        local d=dx:norm()
        if d<0.01 then
            setMovement(0,0,0)
            break
        end
        local m=sim.getObjectMatrix(ref,-1)
        sim.invertMatrix(m)
        local relPos=sim.multiplyVector(m,t:data())
        limitSpeed(relPos)
        setMovement(relPos[2],relPos[1],0)
        sim.switchThread()
    end
end

Cheers