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

Add asynchronous planning and execution #40

Merged
merged 18 commits into from
Dec 22, 2023

Conversation

amalnanavati
Copy link
Contributor

@amalnanavati amalnanavati commented Dec 20, 2023

Description

The primary goal of this PR is to enable users of pymoveit2 to plan and/or execute asynchronously. This is useful to allow users to cancel planning/execution calls, and to allow the main thread to continue executing during planning/execution (e.g., we use pymoveit2 within a behavior tree framework, so the tree must be able to continue ticking as the robot is moving).

Changes 1-2 below tackle that primary goal. As part of tackling the primary goal, some additional changes also made sense; those are Changes 3-4, which are justified below.

Detailed Changes

  1. Make action execution (both MoveGroup and controller execution) asynchronous. Track the state of the action with a new enum, MoveIt2State, and allow users to query the state and cancel execution.
  2. Carve out the asynchronous planning logic into another function, plan_async, that users of this API can call directly. Create an affiliated get_trajectory function that processes the future and returns a trajectory.
  3. Make all invocations of plan/plan_async use the planning service (earlier users could decide whether to use the service or the MoveGroup action with plan_only=true).
    1. The reason for this is that the way of interacting with asynchronous actions call is very different from asynch services, since action calls first return a goal handle, whereas service calls directly return a future for the result. This would make the return value of plan_async, and how users interact with that return value, quite different depending on whether they invoke the action or the service.
    2. Since this code already has a way to asynchronously call, query, and cancel a MoveGroup action (via Change 1 above), I think a better way to re-enable planning-only via MoveGroup would be to allow users to set the plan_only property of the MoveGroup goal, and then allow them to invoke _send_goal_async_move_action (which is invoked anyway if they use move_to_pose or move_to_configuration). This is unimplemented — let me know if you’d like it.
  4. Replace the FollowJointTrajectory action exposed by the controller with the ExecuteTrajectory action exposed by MoveIt2.
    1. This is necessary to allow us to cancel the goal via MoveIt2’s /trajectory_execution_event topic. Although we could alternatively cancel the action through the goal handle, the action server is able to reject the cancellation request. This topic provides us a way to more directly stop execution, but it can only be done if we execute using MoveIt’s ExecuteTrajectory action.
    2. I don’t think any features are lost with this switch, since MoveIt’s ExecuteTrajectory internally calls the controller’s FollowJointTrajectory action.

Testing

  • Launch the simulated panda arm: ros2 launch panda_moveit_config ex_fake_control.launch.py
  • Test move_to_configuration:
    • Test the synchronous execution still works: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]"
    • Restore to initial joint positions: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[0.0, -0.7853981633974483, 0.0, -2.356194490192345, 0.0, 1.5707963267948966, 0.7853981633974483]"
    • Test the asynchronous execution can be successfully canceled: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=1.0
    • Restore to initial joint positions
    • Test that asynchronous execution succeeds if not canceled: ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57, 0.7854]" -p synchronous:=False -p cancel_after_secs:=-1.0
  • Test move_to_pose:
    • Restore to initial joint positions
    • Test the synchronous execution still works: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
    • Restore to initial joint positions
    • Test the asynchronous execution can be successfully canceled: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=1.0
    • Restore to initial joint positions
    • Test that asynchronous execution succeeds if not canceled: ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False -p synchronous:=False -p cancel_after_secs:=-1.0
  • Test gripper:
    • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="open"
    • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="close"
    • ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle"

Further, for our application we have developed behavior tree behaviors that use this PR’s changes to asynchronously plan and execute (with use_move_group_action=False). We’ve been using those behaviors for months and they have worked reliably.

@amalnanavati
Copy link
Contributor Author

(Note: I think squashing and merging would make the most sense given the many commits)

Copy link
Owner

@AndrejOrsula AndrejOrsula left a comment

Choose a reason for hiding this comment

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

Thank you very much for your contribution. It looks great!

I have two comments. One of them discusses the breaking changes of this PR. If it would be possible to hide/remove these breaking changes, it would be much simpler to merge this PR into the primary branch. Regardless, it is probably still better to merge it into devel as a beginning to give some time for testing.

examples/ex_joint_goal.py Outdated Show resolved Hide resolved
pymoveit2/moveit2.py Outdated Show resolved Hide resolved
@AndrejOrsula
Copy link
Owner

Also, I added CI action for pre-commit that might run automatically on the next commit. If not, could you please run your PR through pre-commit? There is a setup script that should simplify installing pre-commit and automatically running it for this repo. Thank you.

@AndrejOrsula AndrejOrsula changed the base branch from master to devel December 21, 2023 17:38
egordon and others added 16 commits December 21, 2023 11:26
Consider two cases, one where action server (either for execute or MoveGroup) is not available, and another where the action succeeds very fast. Both cases are currently indistinguishable from the client perspective, because they will request a goal, and then when they query the state it will be IDLE. This commit resolves that, because if the error code is set that means the action completed very fast, whereas if it is None that means the action did not complete.
@amalnanavati
Copy link
Contributor Author

amalnanavati commented Dec 21, 2023

Thanks for the prompt reply! I addressed all the comments, rebased onto and ran the pre-commit hook, and re-tested it. I think there is slight further discussion to be had on breaking changes (see comment).

Just to give you a heads-up, there will be more PRs coming over the coming days/weeks that build off of this. My labmates and I have been making multiple feature additions in our fork over the last few months, including expanding the API to allow for path constraints, modify the allowed collision matrix, move collision objects, scale collision meshes, etc. I'll create PRs for them one-by-one, and target devel like this PR does.

Copy link
Owner

@AndrejOrsula AndrejOrsula left a comment

Choose a reason for hiding this comment

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

Thank you for addressing those changes!


I just realized that this PR breaks MoveIt2Gripper and in turn GripperInterface because MoveIt2Gripper inherits from MoveIt2 and reuses some of its internals (including the replaced FollowJointTrajectory action client).

E.g. running ex_gripper.py:

'GripperInterface' object has no attribute '_MoveIt2__follow_joint_trajectory_action_client'

Would you be interested in updating this interface accordingly? Alternatively, we can merge this into devel now and then address grippers in some upcoming PR.

examples/ex_joint_goal.py Outdated Show resolved Hide resolved
examples/ex_pose_goal.py Outdated Show resolved Hide resolved
pymoveit2/moveit2.py Outdated Show resolved Hide resolved
@amalnanavati
Copy link
Contributor Author

Fixed the gripper interface, and updated the tests accordingly. Note that because I don't actively use the gripper I probably didn't test it as thoroughly as can be -- I just ran the three commands at the top ex_gripper.py. Feel free to test more if there are further edge cases you often encounter when using the Gripper Interface :)

Copy link
Owner

@AndrejOrsula AndrejOrsula left a comment

Choose a reason for hiding this comment

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

It works well from my initial tests. Thank you very much for your contribution! 😄

@AndrejOrsula AndrejOrsula merged commit ae4c8bc into AndrejOrsula:devel Dec 22, 2023
1 check passed
@AndrejOrsula AndrejOrsula mentioned this pull request Dec 22, 2023
AndrejOrsula added a commit that referenced this pull request Mar 6, 2024
* Add asynchronous planning and execution (#40)

* Add joint goal example for Kinova JACO2

* [WIP] Added execution cancellation and polling

* Switch to ExecuteTrajectory action

* [WIP] Goal cancellation is broken

* Added cancellation via topic publication

* Full cancellation example

* Need option for both move action and direct planning

* Created get_trajectory, so users of plan_async can easily get the result

* Reset last error code before action execution

Consider two cases, one where action server (either for execute or MoveGroup) is not available, and another where the action succeeds very fast. Both cases are currently indistinguishable from the client perspective, because they will request a goal, and then when they query the state it will be IDLE. This commit resolves that, because if the error code is set that means the action completed very fast, whereas if it is None that means the action did not complete.

* Reverted to original example

* Small fixes from rebase

* Update examples

* Update docstrings

* Update docstrings

* Addressed PR comments

* Ran pre-commit hook

* Addressed PR comments

* Fixed gripper interface

---------

Co-authored-by: Ethan K. Gordon <[email protected]>

* Add Path Constraints (#42)

* [WIP] Add ability to do path constraints

* [WIP] Change API to be more intelligible

* Allowed different orientation tolerances per axes

* Make change not breaking by adding float option

* Added parameterization option

* Updated set_pose_goal

* Rearranged parameterization to not be a breaking change

* Formatting changes form pre-commit

* Add orientation path constraint example

* Reused constraint creation code from goal constraints

* Pre-commit formatting fix

---------

Co-authored-by: Ethan Gordon <[email protected]>

* Added Async Forward/Inverse Kinematics (#43)

* [WIP] Add async service call for FK/IK analogous to planning

* Compute FK returns a list of post_stampeds

* Pre-commit formatting

* Added FK example

* Comment changes to orientation path constraint examples

* Added IK example

* Update examples/ex_fk.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update examples/ex_ik.py

Co-authored-by: Andrej Orsula <[email protected]>

* Update moveit2.py

---------

Co-authored-by: Ethan Gordon <[email protected]>
Co-authored-by: Andrej Orsula <[email protected]>

* Allow users to set `planner_id` and `pipeline_id` (#48)

* Added set_planner_id

* Make planner_id and pipeline_id properties

* Add planner_id param to example files

* Formatting

---------

Co-authored-by: Amal Nanavati <[email protected]>
Co-authored-by: Ethan K. Gordon <[email protected]>
@amalnanavati amalnanavati deleted the egordon/async branch April 23, 2024 17:13
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

3 participants