Hi!
The wait_until_at_setpoint() seems to wait for way too long before completing its command, even after the robot reaches its setpoint. I’m not sure what the problem is here, can anyone guide me?
Thanks
Praveen
Hi @flyingMango, the wait for setpoint method depends on a threshold value within which the motor has to achieve the goal position. For the wheel motors, this threshold value might need to be tweaked if the robot is on surfaces like carpet. On which motor are you calling the wait_until_at_setpoint() method? It would be helpful if you could share a code snippet that I could run.
Hi @bshah , im using the wait for setpoint on arm and lift joints,
even something as simple as the following makes the robot wait for quite a while.
robot.lift.move_to(0.5);
robot.push_command();
robot.lift.wait_until_at_setpoint();
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.