# Wait\_until\_at\_setpoint() behaviour

**URL:** <https://forum.hello-robot.com/t/wait-until-at-setpoint-behaviour/689>\
**Category:** Ask\
**Created:** [April 24, 2023, 11:02pm UTC](https://forum.hello-robot.com/t/wait-until-at-setpoint-behaviour/689 "2023-04-24T23:02:03Z")\
**Posts on this page:** 1\
**Showing post:** 4

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [April 26, 2023, 1:00am UTC](https://forum.hello-robot.com/t/wait-until-at-setpoint-behaviour/689/4 "2023-04-26T01:00:38Z")

</div>

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

```python
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:

> <https://github.com/hello-robot/stretch_body/blob/709ab300cd89cb092f3e50146f39a24a13e69e12/body/stretch_body/stepper.py#L439-L450>

So we’re going to use the Robot Trace Tool to debug what’s going on.

> [@Using the Robot Trace tool to debug Stretch Body](https://forum.hello-robot.com/t/using-the-robot-trace-tool-to-debug-stretch-body/629):
>
> Starting with Stretch Body 0.4.26 and Stretch Factory v0.3.15 we have introduced a new debugging tool for Stretch Body called Robot Trace. Note: This tool is recommended for advanced users only The Robot Trace tool periodically logs Stretch Body status data to disk in the background. This data can then be inspected and plotted offline. The Trace tool can be useful to capture and analyze events when debugging transient issues and behavior with the robot. For example, if the robot powers down…

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`.

```python
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.

 ![Screenshot from 2023-04-25 17-51-14](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/812e74292161a1c5f309e5f2a4a80053c8783dd1.png)

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).

 ![Screenshot from 2023-04-25 17-51-17](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/4d59ba8cfb3d178843d55c0718e6a6d6db191ff3.png)

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.

---

_[View the full topic](https://forum.hello-robot.com/t/wait-until-at-setpoint-behaviour/689)._