Configure ada_feeding for Articulable Fork
#211
Conversation
…sociated with the selected end_effector_tool, and use it when defining the list of joint_names in the MoveIt2 object
… Articulable Fork
…as a parameter to ada_feeding_action_servers
…nfigurations based on tool_joints associated with end_effector_tool
…sed on tool_joints defined by end_effector_tool
jjaime2
changed the title
ada_feeding with end_effector_toolada_feeding for Articulable Fork
| base_link_name=kinova.base_link_name(), | ||
| end_effector_name="forkTip", | ||
| group_name="jaco_arm", |
There was a problem hiding this comment.
Is there a specific reason for this change in group name? Is this because we are adding the articulable fork end effector to the JACO arm?
There was a problem hiding this comment.
My reasoning was to have the ada planning group encompass the kinematics from base to tool tip given that the AF joints are added. Specifically, I felt jaco_arm would be a misleading name with the added AF joints.
| @@ -454,3 +460,30 @@ def import_from_string(import_string: str) -> Any: | |||
| ) | |||
| except Exception as exc: | |||
| raise ImportError(f"Error importing {import_string}") from exc | |||
|
|
|||
|
|
|||
| def get_tool_joints(end_effector_tool: str) -> List[str]: | |||
There was a problem hiding this comment.
I'm unsure if this helper method is necessary given that the functionality is basic enough to implement everywhere else (which I believe is only 3 places) and we only have one end effector tool that has joints - the articulable fork. Perhaps it may be useful in the future if we have further end effectors with joints.
There was a problem hiding this comment.
Yeah, my intention was to have a common place that people could look for and add tool joints for different configurations. I'd prefer keeping the helper function since I don't like having to search the codebase to make updates for things like this.
| - -2.3609596843308234 # j2n6s200_joint_4 | ||
| - 4.43936623280362 # j2n6s200_joint_5 | ||
| - 3.06866544924739 # j2n6s200_joint_6 | ||
| - -2.4538579336877304 |
There was a problem hiding this comment.
Nit-pick: previously brief comments labeled each of the joint positions, this would improve readability if applied here too
| @@ -93,7 +93,7 @@ ada_planning_scene: | |||
| in_front_of_face_wall: # a wall in front of the user's face so robot motions don't unnecessarily move towards them. | |||
| primitive_type: 1 # shape_msgs/SolidPrimitive.BOX | |||
| primitive_dims: [0.65, 0.01, 0.4] | |||
| position: [0.37, 0.17, 0.85] | |||
| position: [0.37, 0.22, 0.85] | |||
There was a problem hiding this comment.
Is this change something we want to pull or was it for the sake of testing the articulable fork? If the latter, we want to stick to the original position.
There was a problem hiding this comment.
This is actually needed to fit the extra length of the AF during the staging configuration
| @@ -357,12 +360,15 @@ def get_moveit2_object( | |||
| blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) | |||
| # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. | |||
| callback_group = ReentrantCallbackGroup() | |||
| tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key)) | |||
There was a problem hiding this comment.
Add appropriate error handling around the blackboard.get if the key doesn't exist, doesn't have the right permissions, etc.
There was a problem hiding this comment.
Alternatively, you can pass blackboard to get_tool_joints and then handle the get from there (i.e., if any of the error cases arise, just set the key to none). That could help with centralization
| @@ -331,12 +331,15 @@ def get_moveit2_object( | |||
| # the same object. | |||
| moveit2_blackboard_key = "/moveit2" | |||
| moveit2_lock_blackboard_key = "/moveit2_lock" | |||
| end_effector_tool_blackboard_key = "/end_effector_tool" | |||
There was a problem hiding this comment.
In the earlier version of this function, if the BB keys don't exist, it wrote them. That is no longer the case for the EE tool key. Two requested changes:
- Consider having some default behavior where if the key isn't set, it writes
noneto the BB? - Irrespective of the above, add to the function docstring the expectation that the EE tool key is set, and what happens if/when it is not.
Description
This pull request modularizes the entire
ada_feedingstack to dynamically load the robot description, defines controllers and move groups, and updates static configurations for a specifiedend_effector_tool. When launching theada_feedingsoftware stack, setting theend_effector_toolto eitherforkorarticulable_forkwill require that the necessary hardware be assembled and connected, but will otherwise be able to complete the end-to-end feeding task in the same manner.A prerequisite to testing these changes is having your development machine setup according to the Software Installation section of the Articulable Fork Assembly Guide
Related PRs: personalrobotics/ada_ros2#52, personalrobotics/ada_ros2#53, personalrobotics/ada_ros2#54
To test on the real hardware, first connect the appropriate hardware to your development machine via USB, e.g. for
end_effector_tool = forkonly connect the Jaco to your machine, and forend_effector_tool = articulable_forkconnect both the Jaco and Articulable Fork to your machineTesting procedure
python3 src/ada_feeding/start.py --sim <real / mock> --end_effector_tool <tool>for each<tool>infork,articulable_fork.RViz2, observe that the appropriate robot description has been loaded visuallyMotionPlanningwindow inRViz2, open thePlanningGroupdrop-down menu and observe that the available options areada,articulable_fork,hand, andjaco_arm. Set thePlanningGrouptohandfor now, and toggleDisplays -> MotionPlanningon/off to avoid visual glitches inRViz2.Start Feedingto move ADA into theAbovePlateconfigurationMove to mouthbutton, and observe ADA's motion as it orients the fork towards the front of the user's faceContinueand observe ADA move the fork towards the user's mouth, but stop after achieving a set distanceMove above plateand observe ADA move the fork away from the user's mouth, and move into theAbovePlateconfigurationDone eatingand observe ADA moving into theStowconfigurationBefore opening a pull request
pre-commit run --all-filespylint --recursive=y --rcfile=.pylintrc .. All warnings butfixmemust be addressed.Before Merging
Squash & Merge