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)?
How to make sure the joint has reached the desired angle before the code continues to the next operation?
-
- Posts: 9
- Joined: 28 Aug 2022, 16:25
Re: How to make sure the joint has reached the desired angle before the code continues to the next operation?
I think you need to useshirellem72 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))
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
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 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.