Skip to content

Commit 74dab3a

Browse files
amalnanavatiamalnanavati
authored
Add a collision wall between the forktip and user in MoveFromMouth (#100)
* Generalized toggle collision object to take in a list * MoveCollisionObject should add, not subtract, the offset * Made MoveCollisionObject use MoveIt2 * Generalized MoveCollisionObject to ModifyCollisionObject Tested the movement operation, yet to test add (prim & mesh) and remove * Add and remove collision wall in MoveFromMouth
1 parent bdfc4e3 commit 74dab3a

File tree

8 files changed

+339
-163
lines changed

8 files changed

+339
-163
lines changed

‎ada_feeding/README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ This code has been developed and tested with the Kinova JACO Gen2 Arm, on comput
88
- Install [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html)
99
- Install Python dependencies: `python3 -m pip install pyyaml py_trees pymongo tornado trimesh`
1010
- Install the code to command the real robot ([instructions here](https://github.com/personalrobotics/ada_ros2/blob/main/README.md))
11-
- Git clone the [PRL fork of pymoveit (branch: `amaln/cartesian_allow_collision`)](https://github.com/personalrobotics/pymoveit2/tree/amaln/cartesian_avoid_collision) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder.
11+
- Git clone the [PRL fork of pymoveit (branch: `amaln/move_collision`)](https://github.com/personalrobotics/pymoveit2/tree/amaln/move_collision) and the [PRL fork of py_trees_ros (branch: `amaln/service_client`)](https://github.com/personalrobotics/py_trees_ros/tree/amaln/service_client) into your ROS2 workspace's `src` folder.
1212
- Install additional dependencies: `sudo apt install ros-humble-py-trees-ros-interfaces`.
1313
- Install the web app into your workspace ([instructions here](https://github.com/personalrobotics/feeding_web_interface/tree/main/feedingwebapp)).
1414

‎ada_feeding/ada_feeding/behaviors/__init__.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,10 @@
22
This package contains custom py_tree behaviors for the Ada Feeding project.
33
"""
44
from .compute_move_to_mouth_position import ComputeMoveToMouthPosition
5-
from .move_collision_object import MoveCollisionObject
5+
from .modify_collision_object import (
6+
ModifyCollisionObject,
7+
ModifyCollisionObjectOperation,
8+
)
69
from .move_to import MoveTo
710
from .move_to_dummy import MoveToDummy
811
from .toggle_collision_object import ToggleCollisionObject

‎ada_feeding/ada_feeding/behaviors/modify_collision_object.py‎

Lines changed: 212 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,212 @@
1+
#!/usr/bin/env python3
2+
# -*- coding: utf-8 -*-
3+
"""
4+
This module defines the ModifyCollisionObject behavior, which adds, moves, or
5+
removes a collision object in MoveIt's planning scene.
6+
"""
7+
# Standard imports
8+
from enum import Enum
9+
from typing import List, Optional, Tuple
10+
11+
# Third-party imports
12+
import py_trees
13+
from rclpy.node import Node
14+
15+
# Local imports
16+
from ada_feeding.helpers import get_moveit2_object
17+
18+
19+
class ModifyCollisionObjectOperation(Enum):
20+
"""
21+
An enum for the operation to perform on the collision object.
22+
"""
23+
24+
ADD = 0
25+
REMOVE = 1
26+
MOVE = 2
27+
28+
29+
class ModifyCollisionObject(py_trees.behaviour.Behaviour):
30+
"""
31+
A behavior that adds, moves, or removes a collision object.
32+
"""
33+
34+
# pylint: disable=too-many-instance-attributes, too-many-arguments
35+
# A few over is fine. All are necessary.
36+
37+
def __init__(
38+
self,
39+
name: str,
40+
node: Node,
41+
operation: ModifyCollisionObjectOperation,
42+
collision_object_id: str,
43+
collision_object_position_input_key: Optional[str] = None,
44+
collision_object_orientation_input_key: Optional[str] = None,
45+
prim_type: Optional[int] = None,
46+
dims: Optional[List[float]] = None,
47+
mesh_filepath: Optional[str] = None,
48+
position_offset: Tuple[float, float, float] = (0.0, 0.0, 0.0),
49+
) -> None:
50+
"""
51+
Initializes the modify collision object behavior.
52+
53+
Parameters
54+
----------
55+
name: The name of the behavior.
56+
node: The ROS node to associate the publishes with.
57+
operation: The operation to perform on the collision object (ADD, REMOVE, MOVE).
58+
collision_object_id: The ID for the collision object in the MoveIt planning scene.
59+
collision_object_position_input_key: The key for the collision object pose input
60+
on the blackboard. Required for ADD and MOVE. This key should contain a list
61+
of size 3.
62+
collision_object_orientation_input_key: The key for the collision object orientation
63+
input on the blackboard. Required for ADD and MOVE. This key should contain a
64+
list of size 4.
65+
prim_type: The type of primitive to add. Either `prim_type` *and* `dims`, *or*
66+
`mesh_filepath` are required for ADD. For `prim_type` options, see:
67+
https://github.com/ros2/common_interfaces/blob/humble/shape_msgs/msg/SolidPrimitive.msg
68+
dims: The dimensions of the collision object. Either `prim_type` *and* `dims`, *or*
69+
`mesh_filepath` are required for ADD. For `dims` details, see:
70+
https://github.com/ros2/common_interfaces/blob/humble/shape_msgs/msg/SolidPrimitive.msg
71+
mesh_filepath: The filepath to the mesh to add. Either `prim_type` *and* `dims`, *or*
72+
`mesh_filepath` are required for ADD.
73+
position_offset: The offset to *add to* to the collision object position.
74+
"""
75+
# Initiatilize the behavior
76+
super().__init__(name=name)
77+
78+
# Store parameters
79+
self.node = node
80+
self.operation = operation
81+
self.collision_object_id = collision_object_id
82+
self.collision_object_position_input_key = collision_object_position_input_key
83+
self.collision_object_orientation_input_key = (
84+
collision_object_orientation_input_key
85+
)
86+
self.prim_type = prim_type
87+
self.dims = dims
88+
self.mesh_filepath = mesh_filepath
89+
self.position_offset = position_offset
90+
91+
# Check that the right parameters have been passed
92+
if self.operation in set(
93+
(ModifyCollisionObjectOperation.ADD, ModifyCollisionObjectOperation.MOVE)
94+
):
95+
if self.collision_object_position_input_key is None:
96+
raise ValueError(
97+
"The collision object position input key must be specified for ADD "
98+
"and MOVE operations."
99+
)
100+
if self.collision_object_orientation_input_key is None:
101+
raise ValueError(
102+
"The collision object orientation input key must be specified for "
103+
"ADD and MOVE operations."
104+
)
105+
if self.operation == ModifyCollisionObjectOperation.ADD:
106+
if self.mesh_filepath is None and (
107+
self.prim_type is None or self.dims is None
108+
):
109+
raise ValueError(
110+
"If `mesh_filepath` is None, then both `prim_type` and `dims` "
111+
"must be specified."
112+
)
113+
114+
# Initialize the blackboard for this behavior
115+
self.blackboard = self.attach_blackboard_client(
116+
name=name + " ModifyCollisionObject", namespace=name
117+
)
118+
# Read the position to move the collision object to
119+
if self.collision_object_position_input_key is not None:
120+
self.blackboard.register_key(
121+
key=self.collision_object_position_input_key,
122+
access=py_trees.common.Access.READ,
123+
)
124+
# Read the orientation to move the collision object to
125+
if self.collision_object_orientation_input_key is not None:
126+
self.blackboard.register_key(
127+
key=self.collision_object_orientation_input_key,
128+
access=py_trees.common.Access.READ,
129+
)
130+
# Read the frame ID for the pose to move the collision object to
131+
if self.collision_object_position_input_key is not None:
132+
self.blackboard.register_key(
133+
key="frame_id",
134+
access=py_trees.common.Access.READ,
135+
)
136+
137+
# Get the MoveIt2 interface
138+
self.moveit2, self.moveit2_lock = get_moveit2_object(
139+
self.blackboard,
140+
self.node,
141+
)
142+
143+
def update(self) -> py_trees.common.Status:
144+
"""
145+
Adds, moves, or removes the collision object, as specified by the
146+
operation. If the operation is to add or move, it gets the latest
147+
pose from the blackboard. This behavior does not wait for the message
148+
to be processed by MoveIt before returning success.
149+
"""
150+
self.logger.info(f"{self.name} [ModifyCollisionObject::update()]")
151+
152+
# Remove the collision object
153+
if self.operation == ModifyCollisionObjectOperation.REMOVE:
154+
with self.moveit2_lock:
155+
self.moveit2.remove_collision(self.collision_object_id)
156+
return py_trees.common.Status.SUCCESS
157+
158+
# If we are adding/moving, first get the pose to move the collision object to
159+
try:
160+
collision_object_position = self.blackboard.get(
161+
self.collision_object_position_input_key
162+
)
163+
collision_object_orientation = self.blackboard.get(
164+
self.collision_object_orientation_input_key
165+
)
166+
frame_id = self.blackboard.get("frame_id")
167+
except KeyError:
168+
# If the collision object pose is not on the blackboard, return failure
169+
self.logger.error(
170+
"The collision object pose is not on the blackboard. "
171+
"Returning failure."
172+
)
173+
return py_trees.common.Status.FAILURE
174+
175+
# Add the position offset
176+
collision_object_position = [
177+
collision_object_position[0] + self.position_offset[0],
178+
collision_object_position[1] + self.position_offset[1],
179+
collision_object_position[2] + self.position_offset[2],
180+
]
181+
182+
# Move the collision object
183+
with self.moveit2_lock:
184+
if self.operation == ModifyCollisionObjectOperation.ADD:
185+
# Add the collision object
186+
if self.mesh_filepath is not None:
187+
self.moveit2.add_collision_mesh(
188+
self.mesh_filepath,
189+
self.collision_object_id,
190+
collision_object_position,
191+
collision_object_orientation,
192+
frame_id=frame_id,
193+
)
194+
else:
195+
self.moveit2.add_collision_primitive(
196+
self.collision_object_id,
197+
self.dims,
198+
collision_object_position,
199+
collision_object_orientation,
200+
self.prim_type,
201+
frame_id=frame_id,
202+
)
203+
elif self.operation == ModifyCollisionObjectOperation.MOVE:
204+
self.moveit2.move_collision(
205+
self.collision_object_id,
206+
collision_object_position,
207+
collision_object_orientation,
208+
frame_id,
209+
)
210+
211+
# Return success
212+
return py_trees.common.Status.SUCCESS

‎ada_feeding/ada_feeding/behaviors/move_collision_object.py‎

Lines changed: 0 additions & 140 deletions
This file was deleted.

0 commit comments

Comments
 (0)