Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Execute trajectory #154

Draft
wants to merge 5 commits into
base: main
Choose a base branch
from
Draft

Execute trajectory #154

wants to merge 5 commits into from

Conversation

m-decoster
Copy link
Contributor

@m-decoster m-decoster commented Jan 20, 2025

Describe your changes

This pull request (WIP) monitors the progress on implementing issue #150.

It includes a new execute_trajectory method for the PositionManipulator and DualArmPositionManipulator classes.

Current progress

Currently, there is an implementation of the execute_trajectory method for the PositionManipulator and DualArmPositionManipulator classes.
No implementation for execute_trajectory for the MobileRobot is (currently) provided, as this is not in scope of issue #150, but more on this below.

The implementation is currently blocking. We hadn't decided yet if we want it to be non-blocking.

Considerations

Code duplication

To ensure that the trajectories of both manipulators in the DualArmPositionManipulator are executed in a synchronized manner, the implementation of execute_trajectory in this class does not delegate to the _left_manipulator (resp. _right)'s implementation, but is its own implementation with some code duplication. An alternative would be to make the PositionManipulator's implementation non-blocking (running it in a separate thread), and await both arm's executions. However, this could lead to synchronization issues based on thread scheduling (also, take into account the GIL's impact on multithreaded code), and trajectory execution for both grippers would still need to be implemented in the current way.

Sleeping in trajectory execution

The current implementation does not sleep, but sends servo commands to the robot as fast as it can. This is done to avoid execution lag. This should be fine, as the control box will only execute the latest servo command. Is this the behaviour we want?

Testing

These methods need to be tested extensively on different set-ups. Currently, the DualArmPositionManipulator implementation has not been tested on real hardware. The gripper trajectory execution has also not yet been tested.

Trajectory types and implementation effort

airo-typing defines two main types:

class SingleArmTrajectory:
    times: TimesType  # time (seconds) from start of trajectory
    path: JointPathContainer
    gripper_path: Optional[JointPathContainer] = None

class DualArmTrajectory:
    times: TimesType  # time (seconds) from start of trajectory
    path_left: JointPathContainer
    path_right: JointPathContainer
    gripper_path_left: Optional[JointPathContainer] = None
    gripper_path_right: Optional[JointPathContainer] = None

This makes the difference between a trajectory for one arm and for two arms explicit and allows for the execution of trajectories for two arms simultaneously.
However, when we add the mobile platform to the equation, we would need to consider all combinations of arms and the mobile platform (SingleArmOnMobilePlatformTrajectory, DualArmOnMobilePlatformTrajectory) if we want to move the arm(s) and platform concurrently.
This results in a large amount of duplicated code to execute trajectories.
Can we avoid it somehow, perhaps by making trajectory execution non-blocking? (This would require testing if multithreaded execution results in unexpected behaviour.)

Creating trajectories

The trajectories we obtain from airo-planner are instances of Drake's PathParameterizedTrajectory class.
These can be converted to airo-typing's SingleArmTrajectory class with this function or the following code block:

import numpy as np
from airo_typing import SingleArmTrajectory, JointPathContainer

def _convert_drake_trajectory(drake_trajectory, period=0.005):
    duration = drake_trajectory.end_time() - drake_trajectory.start_time()
    n_servos = int(np.ceil(duration / period))

    times = []
    positions = []
    for t in np.linspace(0, duration, n_servos):
        times.append(t)
        positions.append(drake_trajectory.value(t).squeeze())

    times = np.array(times)
    positions = np.array(positions)
    path = JointPathContainer(positions=positions)

    trajectory = SingleArmTrajectory(times, path, None)
    return trajectory

Checklist

  • Have you modified the changelog?
  • If you made changes to the hardware interfaces, have you tested them using the manual tests?

Sorry, something went wrong.

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
@m-decoster m-decoster self-assigned this Jan 20, 2025
@m-decoster
Copy link
Contributor Author

Thought to keep in mind: what if communication with the robot lags: will we get large jumps with ServoJ?


self._assert_joint_configuration_nearby(joint_trajectory.path.positions[0])

if joint_trajectory.path.velocities is not None:
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

might want to calculate the leading axis velocities and check if they are not too high?

Is the velocity profile used currently? Bc the servoJ does not implement it iirc?

# )

# This avoids the abrupt stop and "thunk" sounds at the end of paths that end with non-zero velocity
# TODO: This is not an attribute of PositionManipulator, but is specific to URrtde. How do we implement this correctly?
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

could add this function to the interface? But do we want to have this as default behavior? shouldn't the user ensure the trajectory is well-behaved?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we do not add the function to the interface (and use it here) I believe a warning or at the very least a comment in the method's docstring would be appropriate, to avoid mistakes when new users execute trajectories that end with non-zero velocities.


# Servo can overshoot. Do a final move to the last configuration.
# manipulator._assert_joint_configuration_nearby(joint_trajectory.path.positions[-1])
self.move_to_joint_configuration(joint_trajectory.path.positions[-1]).wait()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do we always want this? I can't immediately think of a scenario though.

joint_trajectory: the joint trajectory to execute."""
self._assert_joint_trajectory_is_executable(joint_trajectory)

period = 0.005 # Time per servo, approximately. This may be slightly changed because of rounding errors.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

how did you determine this value?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I took it from the cloth competition codebase, I suppose 0.002 would also be a fair choice (500Hz)

@tlpss
Copy link
Contributor

tlpss commented Feb 4, 2025

Seems like a very good first version!

some thoughts on the considerations:

code duplication

I think this is valid. Maybe add comment to explain?

sleeping
Although older commands are indeed pre-empted, i might make more sense to only send commands at at most 500Hz? In the current implementation, the CPU will be 100% busy. Not sure if that matters currently though? would be important for async execution (waypoint scheduling, concurrent mobile navigation,...)

additional questions:

  • have the timings been investigated? what is the overhead of the index search? might be computationally cheaper to do the bookkeeping manually? bc 500Hz -> 2ms budget, which is not much.
  • The gripper certainly does not operate at 500Hz, I don't know how it will behave when you update it so fast. Has this been tested?
  • should we create some 'hw tests' for this? e.g. execute a few trajectories to quickly check if things are working as expected?

@tlpss
Copy link
Contributor

tlpss commented Feb 4, 2025

Thought to keep in mind: what if communication with the robot lags: will we get large jumps with ServoJ?

yes. One way to avoid this would be to get the current joint config and verify it is not too far from the target config, but that might be too expensive to run at 500Hz?

@tlpss
Copy link
Contributor

tlpss commented Feb 4, 2025

for reference, this is the ros2_control "trajectory execution controller": https://control.ros.org/rolling/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html
https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Way more generic and advanced thatn what we are building, but useful as reference.
They verify if the robot is within certain tolerances for example.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

2 participants