Stretch_body Python SDK and Python Multithreading

Discuss
10

You might try using the is_mg_moving flag. This flag indicates if the underlying trapezoidal motion generator is commanding motion to the PID controller. When the trapezoidal motion generators arrives at its setpoint this flag will be zero.

This flag can be preferable as it isn’t dependent on the motor encoder which inherently has sensor noise. The PID controller tracks the commanded motion accurately so it serves as a reasonable approximation of the true physical motion. The exception being that an exception to the motion (say, a disturbance causes the wheel to stall) will not be caught.

The following diagram helps illustrate:

Screenshot from 2021-11-09 10-33-20
Screenshot from 2021-11-09 10-33-202517×1341 123 KB

Here’s a code snippet to test this use case:

#!/usr/bin/env python
import stretch_body.robot

def wait_until_not_moving(robot):
    import time
    # Note, this is necessary to give time for the trajectory to begin executing before polling for completion
    time.sleep(0.1)
    while robot.base.left_wheel.status['is_mg_moving'] or robot.base.right_wheel.status['is_mg_moving']:
        time.sleep(0.1)

robot = stretch_body.robot.Robot()
robot.startup()


robot.base.translate_by(.005)
robot.push_command()
wait_until_not_moving(robot)

robot.base.translate_by(-.005)
robot.push_command()
wait_until_not_moving(robot)

robot.stop()

This will cause the robot to move forward 5mm, then back 5mm.

1 Like