Scripting Joint Motion

Overview

Individual joint motion can be set from the RobotClient API — although if ROS2 is being used then typically motion will be set from stretch_core API.

The RobotClient API abstracts away the many configuration settings for joint motion — and the default YAML params are generally sufficient for most users.

Design Pattern

Commanding joint motion typically follows a standard design pattern of

1. Create RobotClient
2. Startup RobotClient
3. Start control loop <100hz
4. For each loop
 - Do a RobotClient pull_status
 - Compute motion setpoints
 - Command RobotClient motion
 - Do a RobotClient push_command
5. Exit loop and stop RobotClient

See the Example: Reactive Control Loop for more information.

What can I control?

The available subsystems that support motion commands are:

robot_client.lift
robot_client.arm
robot_client.end_of_arm
robot_client.omnibase

See the Robot Client API for the availabe motion API for each.

MOVE_BY

All joints support a move_by which create motion relative to the current position. For example, here is the API for the Lift and Arm:

https://github.com/hello-robot/stretch4_body/blob/4be724431ea21c87d41bdf03f3cce36cd4ba4562/stretch4_body/robot/robot_client.py#L748-L768

    def move_by(self, x_m, v_m=None, a_m=None, stiffness=None, req_calibration=True, contact_sensitivity_pos=None, contact_sensitivity_neg=None):
        """
        Move the joint by a relative amount.
        
        Parameters
        ----------
        x_m : float
            Relative motion in meters.
        v_m : float, optional
            Velocity limit (m/s).
        a_m : float, optional
            Acceleration limit (m/s^2).
        stiffness : float, optional
            Stiffness setting (0.0 to 1.0) or None to leave unchanged.
        req_calibration : bool, optional
            If True, requires the joint to be calibrated, by default True.
        contact_sensitivity_pos : float, optional
            Contact sensitivity in positive direction (0-1).
        contact_sensitivity_neg : float, optional
            Contact threshold in negative direction (0-1).
        """

For example, to move the Lift up by 0.1m:

from stretch4_body.robot.robot_client import RobotClient
r = RobotClient()
r.startup()
r.lift.move_by(x_m=0.1)
r.push_command()
r.wait_on_motion_finish()
r.stop()

This example uses the default velocity, acceleration, and other motion settings — as define in by the parameter system.

MOVE_TO

In the same way, the move_to methods for joints allow moving to absolution positions. For example, to move the Lift to 0.5m:

from stretch4_body.robot.robot_client import RobotClient
r = RobotClient()
r.startup()
r.lift.move_to(x_m=0.5)
r.push_command()
r.wait_on_motion_finish()
r.stop()

SET_VELOCITY

Joints support a set_velocity command as well. This method should be used with care — as a runaway velocity can cause harm. While the Stretch Body Server enforces joint limits to prevent runaways, it is still advised to only use set_velocity in a carefully constructed control-loop with a watchdog.

The nominal API for the Lift and Arm look like:

https://github.com/hello-robot/stretch4_body/blob/main/stretch4_body/robot/robot_client.py#L775

Working with the EndOfArm

The End-Of-Arm (EOA) supports a chain of Feetech servos that individually support motion through the RobotClient API. While similar to the Lift and Arm, the notable difference is that the joint name must be provided.

https://github.com/hello-robot/stretch4_body/blob/4be724431ea21c87d41bdf03f3cce36cd4ba4562/stretch4_body/robot/robot_client.py#L748-L768

    def move_by(self, x_m, v_m=None, a_m=None, stiffness=None, req_calibration=True, contact_sensitivity_pos=None, contact_sensitivity_neg=None):
        """
        Move the joint by a relative amount.
        
        Parameters
        ----------
        x_m : float
            Relative motion in meters.
        v_m : float, optional
            Velocity limit (m/s).
        a_m : float, optional
            Acceleration limit (m/s^2).
        stiffness : float, optional
            Stiffness setting (0.0 to 1.0) or None to leave unchanged.
        req_calibration : bool, optional
            If True, requires the joint to be calibrated, by default True.
        contact_sensitivity_pos : float, optional
            Contact sensitivity in positive direction (0-1).
        contact_sensitivity_neg : float, optional
            Contact threshold in negative direction (0-1).
        """