Hi @flyingMango, thanks for reporting this. The wait_until_at_setpoint() method has some flaky behavior and I’d like to fix this. It would be helpful if you could follow my debugging procedure below and let me know what you find.
Running something similar to your snippet
input("Stowing the robot first. Proceed:")
r.stow()
input("Starting lift wait_until_at_setpoint at 0.5m. Proceed:")
r.lift.move_to(0.5)
r.push_command()
r.lift.wait_until_at_setpoint()
input("Done. Exit:")
I found that I had the opposite of the problem you’re seeing; wait_until_at_setpoint() returns instantly and I see the last “Done. Exit:” message before the lift has reached the 0.5m setpoint. As you can see, the wait_until_at_setpoint() method internally depends on a flag called “near_pos_setpoint” from the status dictionary:
So we’re going to use the Robot Trace Tool to debug what’s going on.
I enabled the “use_trace” flag in ~/stretch_user/<robot_serial_no>/stretch_user_params.yaml and added a trace callback to log the lift’s status dictionary. Copy the following into a file called “wait_setpoint.py” and run it using python3 wait_setpoint.py.
import sys
import time
import stretch_body.robot
def lift_trace_cb(robot, data):
data['timestamp'] = time.time()
data['lift.pos'] = robot.lift.status['pos']
data['lift.vel'] = robot.lift.status['vel']
data['lift.motor.pos'] = robot.lift.motor.status['pos']
data['lift.motor._command.pos'] = robot.lift.motor._command['x_des']
data['lift.motor.vel'] = robot.lift.motor.status['vel']
data['lift.motor.current'] = robot.lift.motor.status['current']
data['lift.motor.effort_pct'] = robot.lift.motor.status['effort_pct']
data['lift.motor.timestamp'] = robot.lift.motor.status['timestamp']
data['lift.motor.guarded_event'] = robot.lift.motor.status['guarded_event']
data['lift.motor.is_moving'] = robot.lift.motor.status['is_moving']
data['lift.motor.is_moving_filtered'] = robot.lift.motor.status['is_moving_filtered']
data['lift.motor.at_current_limit'] = robot.lift.motor.status['at_current_limit']
data['lift.motor.is_mg_accelerating'] = robot.lift.motor.status['is_mg_accelerating']
data['lift.motor.is_mg_moving'] = robot.lift.motor.status['is_mg_moving']
data['lift.motor.in_guarded_event'] = robot.lift.motor.status['in_guarded_event']
data['lift.motor.in_safety_event'] = robot.lift.motor.status['in_safety_event']
data['lift.motor.waiting_on_sync'] = robot.lift.motor.status['waiting_on_sync']
data['lift.motor.in_sync_mode'] = robot.lift.motor.status['in_sync_mode']
data['lift.motor.near_pos_setpoint'] = robot.lift.motor.status['near_pos_setpoint']
data['lift.motor.near_vel_setpoint'] = robot.lift.motor.status['near_vel_setpoint']
return data
r = stretch_body.robot.Robot()
r.trace.trace_cbs = []
r.trace.add_trace_callback(lift_trace_cb)
r.startup()
if not r.is_calibrated():
print('home the robot')
sys.exit(1)
input("Stowing the robot first. Proceed:")
r.stow()
input("Starting lift wait_until_at_setpoint at 0.5m. Proceed:")
r.lift.move_to(0.5)
r.push_command()
r.lift.wait_until_at_setpoint()
input("Done. Exit:")
r.stop()
Then using the REx_trace_robot.py Tool, we can plot the individual pieces of data. For example, I ran the snippet above twice, and the first time, I plotted the lift’s position side by side with the commands the lift received. We can see the lift received the command at ~0.11min (7 seconds in) and finished achieving the setpoint at ~0.17min (11 seconds in). We would expect the lift to take a few seconds to traverse 30cm so this makes sense.
Then, from the second run, I plotted the lift’s received commands side by side with the “near_pos_setpoint” flag. Here I found something weird. Although the command is received at 0.125min (7.5 seconds in), the “near_pos_setpoint” flag doesn’t let Stretch Body know that the robot hasn’t achieved the setpoint yet until 0.127min (7.62 seconds in).
By instantly calling wait_until_at_setpoint() after issuing the lift setpoint, the library doesn’t yet realize that the robot has begun moving and the method returns instantly. In your case, the method waits the maximum amount of time (30 seconds) before returning. Use the snippet above to save a trace and send it to me.