execute_trajectory(trajectory) function

[adverley]

Describe the feature you'd like
It would be nice to bundle our experiences of executing trajectories (PathParameterizedTrajectory) on the real platform in a execute_trajectory because I have noticed we do not always run them straightforward as a servo_to_joint_config() on the real robot. See example below.

Use cases
Anyone running a PathParameterizedTrajectory...

Possible implementation
From existing demo's we have:

def execute_trajectory(joint_trajectory: PathParameterizedTrajectory, manipulator: PositionManipulator):
    assert joint_trajectory.start_time() == 0.0

    joint_trajectory.value(0).squeeze()

    period = 0.005  # Time per servo, approximately. This may be slightly changed because of rounding errors.
    # The period determines the times at which we sample the trajectory that was time-parameterized by TOPPRA.
    duration = (
        joint_trajectory.end_time() - joint_trajectory.start_time()
    )  # Should equal end_time(), but sanity check, or in case assertion is ever removed.

    n_servos = int(np.ceil(duration / period))
    period_adjusted = duration / n_servos  # can be slightly different from period due to rounding

    for t in np.linspace(0, duration, n_servos):
        joints = joint_trajectory.value(t).squeeze()
        logger.trace(
            f"Target joints: {np.rad2deg(joints)}, current joints: {np.rad2deg(manipulator.get_joint_configuration())} "
            f"(diff: {np.rad2deg(np.linalg.norm(joints - manipulator.get_joint_configuration())):0.02f} )"
        )
        manipulator.servo_to_joint_configuration(joints, period_adjusted).wait()

    # This avoids the abrupt stop and "thunk" sounds at the end of paths that end with non-zero velocity
    # However, I believe these functions are blocking, so right only stops after left has stopped.
    manipulator.rtde_control.servoStop(2.0)

    manipulator.move_to_joint_configuration(joints).wait()

[tlpss]

agreed that it makes sense to offer a higher level trajectory execution method.

Two notes on your implementation:

Using the ServoJ commands however, we can only control the positions directly (the velocity and acc command inputs are currently unused in the UR scripting language). The velocities and accelerations are thus implicitly determined by the servoJ interpolation I believe. I'm not sure if 1) this is a problem for us? If you sample the positions at high frequency, you kind of enfore the high-level velocity profile I believe. 2) ifthere even is a way to overcome this. options: look at the RTDE docs to see if other functions could help or take a look at the official C++ drivers for the UR that came out of the ROS UR drivers (https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/master)

second (minor). I would assert that the initial pose is close enough to the first waypoint, to avoid crazy accelerations. Optionally we could also iterate over the trajectory to validate it (max leading axis acceleration or something).

[tlpss]

linking some additional sources:

UR-scripting reference : https://s3-eu-west-1.amazonaws.com/ur-support-site/234367/script_directory_PolyScope5.pdf#page=53.28

UR-rtde servoJ docs: https://sdurobotics.gitlab.io/ur_rtde/api/api.html?highlight=servoj#_CPPv4N7ur_rtde20RTDEControlInterface6servoJERKNSt6vectorIdEEddddd

[Victorlouisdg]

Just as a reference, for the cloth competition, I also wrote an execute_dual_arm_drake_trajectory().

[Victorlouisdg]

An additional note for these trajectory execution functions is that they work with Drake Trajectory objects. The reason for this, is that they are continuous (i.e. interpolate by default), so you can sample them at any time joint_trajectory.value(t). This makes them easy to adapt to the servo period required by the UR controller.

The question then becomes, do we put these functions in airo-robot and make Drake a dependency?

Alternatively, it might be possible to use the discretized trajectory times from airo-typing in airo-robot instead. In airo-drake there is a discretize_drake_joint_trajectory() function that Drake users could use then.

[tlpss]

An additional note for these trajectory execution functions is that they work with Drake Trajectory objects. The reason for this, is that they are continuous (i.e. interpolate by default), so you can sample them at any time joint_trajectory.value(t). This makes them easy to adapt to the servo period required by the UR controller.

I hadn't noticed this. For me it makes more sense to discretize the trajectories and then update the servo with the nearest timestep (you can also interpolate it if needed, I think). This way it is independent of how the trajectory was created and if you sample the waypoints dense enough during discrezization, I don't think this causes issues?

[adverley]

I've done it both ways (like VL with Drake trajectories) and discretized trajs and both work (if densely sampled). I think if we want to avoid drake as a dependency that using discretized trajs makes more sense than adding the dependency.

I'll go over your suggestions later, thanks for the information! Might also push this to @m-decoster who also has his fair share of running trajs on the UR robots.

[Victorlouisdg]

Conclusion:

• Airo-mono will only provide execution functions for already discretized trajectories, e.g. SingleArmTrajectory.
• Consider joint arm and gripper trajectory execution
• Consider whether we want to make execute_trajectory non-blocking / awaitable
• It would be better to not await the servoJ in the loop. (This has the risk of lagging)
• Implementation: find two closest times and linearly interpolate
• Must have checks for start and end joint states (robot not too far)

[Victorlouisdg]

• Should be a member of PositionManipulator
• DualArm execution should be in sync
• Describe error accumulation

[Victorlouisdg]

• Sanity checks before execution e.g. veloctiy profile with np.diff
• Can non-zero end velocity sometimes be ok?
• Current consensus: trajectories should start and end with zero velocity.

[m-decoster]

We've implemented a first sanity check for barista (_configuration_nearby):

def execute_trajectory_ur(joint_trajectory: PathParameterizedTrajectory, manipulator: PositionManipulator):
    assert joint_trajectory.start_time() == 0.0
    assert _configuration_nearby(joint_trajectory, manipulator)
    # ...

def _configuration_nearby(joint_trajectory: PathParameterizedTrajectory, manipulator: PositionManipulator) -> bool:
    """Assert that the first configuration of the trajectory is close to the current configuration of the manipulator."""
    current_configuration = manipulator.get_joint_configuration()
    first_configuration = joint_trajectory.value(0).squeeze()
    return np.isclose(first_configuration, current_configuration, atol=np.radians(1.0), rtol=0.0).all()

Note that these asserts should probably be exceptions instead.