How to make sure the joint has reached the desired angle before the code continues to the next operation?

Typically: "How do I... ", "How can I... " questions
Post Reply
shirellem72
Posts: 9
Joined: 28 Aug 2022, 16:25

How to make sure the joint has reached the desired angle before the code continues to the next operation?

Post by shirellem72 »

I am trying to make my quadruped robot walk. I have written a move_COB function the moves the center of body to a stable position and a move_leg function that move a leg throw a path.
I want my robot to finish the move_COB and only after the joints reach the target position I want the robot to move the leg using the move_leg function.
Both functions work perfectly but when I write them in my python script one after the other they happen at the same time (the COB and the leg both move together).
When I add a while loop that waits for the robot to reach the target position (using: delta = sim.getjointpositon()-target_position and a while loop: while(delta<0.01)) the loop is running but while the loop runs the robot doesn't move in the simulation so the while loop never ends- the joint position doesn't change.
I have also tried a for loop instead of the while loop and i saw that the robot COB moves only after the for loop ends (I printed the i iteration and I saw that the print happens before the robot start moving even though it is written below in the script code) .
Any ideas why is this happening? and how can I make those two functions run one after the other and not parallel (let the second function wait that the first one has ended)?

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: How to make sure the joint has reached the desired angle before the code continues to the next operation?

Post by fferri »

shirellem72 wrote: 04 Dec 2022, 11:43 When I add a while loop that waits for the robot to reach the target position (using: delta = sim.getjointpositon()-target_position and a while loop: while(delta<0.01))
I think you need to use while math.abs(delta)<0.01 to be correct.

Anyways, if you want to do such busy wait cycle in an embedded script, you need to do it in a Lua coroutine (a.k.a. "threaded" script), otherwise the simulation can't advance and you created a deadlock.

Example:

Code: Select all

function setTargetPositionAndWait(joint,pos)
    sim.setJointTargetPosition(joint,pos)
    while math.abs(sim.getJointPosition(joint)-pos)>tolerance do
        sim.switchThread()
    end
end

function sysCall_init()
    joint=sim.getObject'.'
    tolerance=0.25*math.pi/180
    corout=coroutine.create(function()
        setTargetPositionAndWait(joint,math.pi/2)
        sim.wait(1)
        setTargetPositionAndWait(joint,-math.pi/2)
        sim.wait(1)
        setTargetPositionAndWait(joint,0)
        print'done'
    end)
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
or you can do it without coroutines, but then you should not have a busy-wait cycle in the simulation step. Instead you should check if the target has reached or not, and if it hasn't reached yet, simply do nothing, plus some additional variable to track the status between simulation steps. Example:

Code: Select all

function sysCall_init()
    joint=sim.getObject'.'
    i=1
    targetPos={math.pi/2,-math.pi/2,0}
    sim.setJointTargetPosition(joint,targetPos[i])
    waitUntil=0
    tolerance=0.25*math.pi/180
end

function sysCall_actuation()
    if waitUntil>sim.getSimulationTime() then return end
    
    reachedTargetPrev=reachedTarget
    reachedTarget=math.abs(sim.getJointPosition(joint)-targetPos[i])<=tolerance
    if not reachedTarget then
        -- target not reached: do nothing
    elseif not reachedTargetPrev then
        -- target just reached, wait 1s
        waitUntil=sim.getSimulationTime()+1
    else
        -- target reached, wait done, select next target
        if i<#targetPos then
            i=i+1
            sim.setJointTargetPosition(joint,targetPos[i])
        end
    end
end
as you can see the version with coroutines is simple and straightforward.

As a final remark, keep in mind that your check for reaching target position is overly simplified: a joint with load and position control can overshoot or vibrate around the target, hopefully with an oscillation that dampens over time. Then a better condition for the joint reaching the target position is that the position is within tolerance but also velocity is (almost) zero.

Post Reply