robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

Update BT for full process with graver

Browse source 
- Adding new fixed joint links (`right_contact_panel` and `left_contact_panel`) to Link8 and Link7.
 - Defining visual representations with semi-transparent boxes for these panels.
 - Setting up collision models to enable accurate contact detection.
 - Updating Gazebo references to maintain fixed joint properties in simulations.
 - Moving sensors from the main links to the new contact panels for better interaction sensing.
 - Correcting and updating collision references in sensors.
 - Making minor XML formatting adjustments for consistency.
This commit is contained in:
Ilya Uraev 2025-03-03 16:36:35 +03:00
parent f5b9924b79
commit 2156e9a0c9
17 changed files with 543 additions and 121 deletions
Download patch file Download diff file Expand all files Collapse all files

55
rbs_mill_assist/scripts/grasping_service.py
View file

@ -42,10 +42,11 @@ class GetGraspPickPoses(Node):
def get_grasp_pick_poses(
self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
) -> GetPickPlacePoses.Response:
if request.action_name == "pick":
pregrasp_pose = self.grasping.get("pregrasp_pose", None)
grasp_pose = self.grasping.get("grasp_pose", None)
postgrasp_pose = self.grasping.get("postgrasp_pose", None)
if request.action_name == "to_graver.pick":
to_graver = self.grasping.get("to_graver", None)
pregrasp_pose = to_graver.get("pregrasp_pose", None)
grasp_pose = to_graver.get("grasp_pose", None)
postgrasp_pose = to_graver.get("postgrasp_pose", None)
if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
response.ok = False
@ -60,11 +61,49 @@ class GetGraspPickPoses(Node):
self.create_pose(grasp_pose),
self.create_pose(postgrasp_pose),
]
elif request.action_name == "to_graver.place":
to_graver = self.grasping.get("to_graver", None)
preplace_pose = to_graver.get("preplace_pose", None)
place_pose = to_graver.get("place_pose", None)
postplace_pose = to_graver.get("postplace_pose", None)
elif request.action_name == "place":
preplace_pose = self.grasping.get("preplace_pose", None)
place_pose = self.grasping.get("place_pose", None)
postplace_pose = self.grasping.get("postplace_pose", None)
if None in [preplace_pose, place_pose, postplace_pose]:
response.ok = False
self.get_logger().error(
"Missing one or more of the preplace, place, or postplace poses."
)
return response
# Create Pose messages for placing
response.place = [
self.create_pose(preplace_pose),
self.create_pose(place_pose),
self.create_pose(postplace_pose),
]
elif request.action_name == "from_graver.pick":
from_graver = self.grasping.get("from_graver", None)
pregrasp_pose = from_graver.get("pregrasp_pose", None)
grasp_pose = from_graver.get("grasp_pose", None)
postgrasp_pose = from_graver.get("postgrasp_pose", None)
if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
response.ok = False
self.get_logger().error(
"Missing one or more of the pregrasp, grasp, or postgrasp poses."
)
return response
# Create Pose messages
response.grasp = [
self.create_pose(pregrasp_pose),
self.create_pose(grasp_pose),
self.create_pose(postgrasp_pose),
]
elif request.action_name == "from_graver.place":
from_graver = self.grasping.get("from_graver", None)
preplace_pose = from_graver.get("preplace_pose", None)
place_pose = from_graver.get("place_pose", None)
postplace_pose = from_graver.get("postplace_pose", None)
if None in [preplace_pose, place_pose, postplace_pose]:
response.ok = False