-
Notifications
You must be signed in to change notification settings - Fork 60
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
Add asynchronous planning and execution #40
Conversation
(Note: I think squashing and merging would make the most sense given the many commits) |
There was a problem hiding this 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.
Also, I added CI action for |
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.
741a248
to
f560d0c
Compare
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 |
There was a problem hiding this 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.
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 |
There was a problem hiding this 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! 😄
* 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]>
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 usepymoveit2
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
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.plan_async
, that users of this API can call directly. Create an affiliatedget_trajectory
function that processes the future and returns a trajectory.plan
/plan_async
use the planning service (earlier users could decide whether to use the service or the MoveGroup action withplan_only=true
).plan_async
, and how users interact with that return value, quite different depending on whether they invoke the action or the service.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 theplan_only
property of the MoveGroup goal, and then allow them to invoke_send_goal_async_move_action
(which is invoked anyway if they usemove_to_pose
ormove_to_configuration
). This is unimplemented — let me know if you’d like it.FollowJointTrajectory
action exposed by the controller with theExecuteTrajectory
action exposed by MoveIt2./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’sExecuteTrajectory
action.ExecuteTrajectory
internally calls the controller’sFollowJointTrajectory
action.Testing
ros2 launch panda_moveit_config ex_fake_control.launch.py
move_to_configuration
: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]"
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]"
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
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
move_to_pose
: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
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
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
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.