In service of #96 . Earlier, sometimes the kinematic motion of MoveFromMouth would move closer to the user, which seems scary and unintuitive. This PR addresses that by:
Generalizing MoveCollisionObject to ModifyCollisionObject, which now has the ability to add, move, and remove a collision object.
Adding a collision wall (see below) in front of the user's face in MoveFromMouth during the kinematic motion.
Allows ToggleCollisionObject to take a list of collision IDs. This ended up being unused in this PR, but is a strict generalization so we decided to merge it in.
Design Decisions
Originally, #96 asked for adding this collision wall for all actions but MoveToMouth/MoveFromMouth's cartesian motions. That is still an option. However, such a collision wall might end up unnecessary restricting where the "elbow" (joint 3) of the robot can go, e.g., for an acquisition motion, maybe the elbow needs to slightly enter the wall. Because the main observed problem that this is solving occurs in MoveFromMouth, I opted to only add/remove the wall in MoveFromMouth. However, if further problems arise, we can make this wall be permanently there except for the final motion into/out of the mouth.
Testing procedure
Run the code as documented in the README. Then, run the below sequence of actions and verify (a) that they succeed; and (b) that the collision wall appears and disappears as expected.
[x] Format your code using black formatterpython3 -m black .
[x] Run your code through pylint and address all warnings/errors. The only warnings that are acceptable to not address is TODOs that should be addressed in a future PR. From the top-level ada_feeding directory, run: pylint --recursive=y --rcfile=.pylintrc ..
Description
In service of #96 . Earlier, sometimes the kinematic motion of
MoveFromMouthwould move closer to the user, which seems scary and unintuitive. This PR addresses that by:MoveCollisionObjecttoModifyCollisionObject, which now has the ability to add, move, and remove a collision object.MoveFromMouthduring the kinematic motion.ToggleCollisionObjectto take a list of collision IDs. This ended up being unused in this PR, but is a strict generalization so we decided to merge it in.Design Decisions
Originally, #96 asked for adding this collision wall for all actions but MoveToMouth/MoveFromMouth's cartesian motions. That is still an option. However, such a collision wall might end up unnecessary restricting where the "elbow" (joint 3) of the robot can go, e.g., for an acquisition motion, maybe the elbow needs to slightly enter the wall. Because the main observed problem that this is solving occurs in MoveFromMouth, I opted to only add/remove the wall in MoveFromMouth. However, if further problems arise, we can make this wall be permanently there except for the final motion into/out of the mouth.
Testing procedure
Run the code as documented in the README. Then, run the below sequence of actions and verify (a) that they succeed; and (b) that the collision wall appears and disappears as expected.
ros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveFromMouthToRestingPosition ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveToMouth ada_feeding_msgs/action/MoveTo "{}" --feedbackros2 action send_goal /MoveFromMouthToAbovePlate ada_feeding_msgs/action/MoveTo "{}" --feedbackBefore opening a pull request
python3 -m black .ada_feedingdirectory, run:pylint --recursive=y --rcfile=.pylintrc ..Before Merging
Squash & Merge