Joint precision changes based on the tool/object in the end-effector, but CommandGroup's acceptable_joint_error doesn't

[hello-amal]

Overview

For the wrist_roll joint (and perhaps others as well), how precisely it can reach a particular setpoint depends on the weight and center-of-rotation of the attached tool. This is a problem, because the command groups currently use one fixed acceptable_joint_error, irrespective of the end-effector tool.

I believe there are two fixes for this:

1. Make the acceptable joint error tool-dependant, and tune what it should for each tool.
2. Instead of waiting until target_position - curr_position is less than a threshold, instead wait until curr_position - prev_position is less than a threshold. In other words, keep looking until the joint stops moving.

I prefer the second fix for two reasons: (a) it generalizes better to other tools or different weight/shapes of objects the robot might pick up; and (b) it better aligns how stretch_body's wait_until_at_setpoint works.

Specific Example

Run the below script with 3 different end-effector tools: (a) none; (b) DexWrist; (c) tablet (with the mount at the top of the tablet in landscape orientation). On both 3030, I got the following results from the script:

1. No tool: abs error is <= 0.007
2. DexWrist: abs error is <= 0.005
3. Tablet: abs error is <= 0.027

3002 had results in the same order of magnitude.

This is significant because the WristRollCommandGroup uses the default acceptable_joint_error of 0.015, which means that when the tablet is attached, it is possible that trajectories that command the wrist roll joint to certain positions will run until timeout, simply because they are seeking a precision that the joint cannot provide. (In fact, this happens whenever the web app commands the tablet to portrait orientation).

import time
import numpy as np
from stretch_body.robot import Robot

if __name__ == "__main__":
    # Create the robot object
    rb = Robot()
    rb.startup()

    # Move the wrist roll joint
    for command in [
        0.0, np.pi/2.0, -np.pi/2.0
    ]:
        rb.end_of_arm.move_to("wrist_roll", command)
        rb.end_of_arm.wait_until_at_setpoint()
        time.sleep(1.0)
        err = command - rb.end_of_arm.get_joint_configuration()['joint_wrist_roll']
        print(f"Commanded: {command}, Error: {err}")

    # Stop the robot
    rb.stop()

[hello-amal]

@hello-binit @hello-fazil can you weigh in on the two fixes I outlined above? Which do you prefer / are there other options you can think of?

[hello-amal]

I have certified that #141 is caused by the same issue, and in that case the wrist pitch command has an error of 0.018 when there is a tennis ball in the gripper, greater than the fixed threshold of 0.015.

So we know this issue, of the joints being able to achieve less precision than is expected of them when an object/tool is in the gripper, affects at least the pitch and roll joints.