Closed amalnanavati closed 9 months ago
(Note: I think squashing and merging would make the most sense given the many commits)
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.
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.
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 :)
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.