From f81de1ab901de6f324540f2a7fcaef12bc5b9a9b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Yannis=20B=C3=BClter?= Date: Thu, 30 May 2024 10:57:03 +0200 Subject: [PATCH] added logic for all use cases. --- .../monitors/force_torque_monitor.py | 33 ++++++++++++++++--- test/Bowl_place_test.py | 14 ++++---- test/Cutlery_place_test.py | 14 ++++---- test/Plate_place_test.py | 14 ++++---- 4 files changed, 50 insertions(+), 25 deletions(-) diff --git a/src/giskardpy/monitors/force_torque_monitor.py b/src/giskardpy/monitors/force_torque_monitor.py index d8fee2fa43..0994c1a4fa 100644 --- a/src/giskardpy/monitors/force_torque_monitor.py +++ b/src/giskardpy/monitors/force_torque_monitor.py @@ -165,7 +165,7 @@ def __call__(self): abs(rob_torque.vector.y) >= torque_y_threshold): self.state = True - print(f'HIT PLACING: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') + print(f'HIT PLACING!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') else: self.state = False print(f'MISS PLACING!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') @@ -196,12 +196,37 @@ def __call__(self): # case for placing plates elif self.object_type == ObjectTypes.OT_Plate.value: # TODO: Add proper placing logic for Plate - print("IT JUST WORKS - Todd Howard, at some point") + + force_x_threshold = 2.34 + force_z_threshold = 1.0 + torque_y_threshold = 0.45 + + if (abs(rob_force.vector.x) >= force_x_threshold and + abs(rob_force.vector.z) >= force_z_threshold and + abs(rob_torque.vector.y) >= torque_y_threshold): + + self.state = True + print(f'HIT PLATE!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') + else: + self.state = False + print(f'MISS PLATE!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') # case for placing bowls elif self.object_type == ObjectTypes.OT_Bowl.value: - # TODO: Add proper placing logic for Bowl - print("IT JUST WORKS - Todd Howard, at some point") + + force_x_threshold = 2.34 + force_z_threshold = 1.0 + torque_y_threshold = 0.45 + + if (abs(rob_force.vector.x) >= force_x_threshold and + abs(rob_force.vector.z) >= force_z_threshold and + abs(rob_torque.vector.y) >= torque_y_threshold): + + self.state = True + print(f'HIT BOWL!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') + else: + self.state = False + print(f'MISS BOWL!: X:{rob_force.vector.x};Z:{rob_force.vector.z};Y:{rob_torque.vector.y}') # if no valid object_type has been declared in method parameters else: diff --git a/test/Bowl_place_test.py b/test/Bowl_place_test.py index d0213ffd60..57eb53295d 100644 --- a/test/Bowl_place_test.py +++ b/test/Bowl_place_test.py @@ -76,13 +76,13 @@ def pickup(self): self.gis.take_pose("pre_align_height") self.gis.execute(add_default=True) - align_pose = PoseStamped() - align_pose.header.frame_id = 'hand_gripper_tool_frame' - align_pose.pose.orientation.z = 1 - align_pose.pose.orientation.w = -0.04 - align_pose.pose.position.x = 1.70 - align_pose.pose.position.y = 1.75 - align_pose.pose.position.z = 1.2 + # align_pose = PoseStamped() + # align_pose.header.frame_id = 'hand_gripper_tool_frame' + # align_pose.pose.orientation.z = 1 + # align_pose.pose.orientation.w = -0.04 + # align_pose.pose.position.x = 1.70 + # align_pose.pose.position.y = 1.75 + # align_pose.pose.position.z = 1.2 action = ContextTypes.context_action.value(content=ContextActionModes.grasping.value) from_above = ContextTypes.context_from_above.value(content=True) diff --git a/test/Cutlery_place_test.py b/test/Cutlery_place_test.py index a8501cdf39..eaa3434bcd 100644 --- a/test/Cutlery_place_test.py +++ b/test/Cutlery_place_test.py @@ -76,13 +76,13 @@ def pickup(self): self.gis.take_pose("pre_align_height") self.gis.execute(add_default=True) - align_pose = PoseStamped() - align_pose.header.frame_id = 'hand_gripper_tool_frame' - align_pose.pose.orientation.z = 1 - align_pose.pose.orientation.w = -0.04 - align_pose.pose.position.x = 1.70 # (+0.14) - align_pose.pose.position.y = 1.75 - align_pose.pose.position.z = 1.2 + # align_pose = PoseStamped() + # align_pose.header.frame_id = 'hand_gripper_tool_frame' + # align_pose.pose.orientation.z = 1 + # align_pose.pose.orientation.w = -0.04 + # align_pose.pose.position.x = 1.70 # (+0.14) + # align_pose.pose.position.y = 1.75 + # align_pose.pose.position.z = 1.2 action = ContextTypes.context_action.value(content=ContextActionModes.grasping.value) from_above = ContextTypes.context_from_above.value(content=True) diff --git a/test/Plate_place_test.py b/test/Plate_place_test.py index 26944ff34e..64bf6cb1ea 100644 --- a/test/Plate_place_test.py +++ b/test/Plate_place_test.py @@ -76,13 +76,13 @@ def pickup(self): self.gis.take_pose("pre_align_height") self.gis.execute(add_default=True) - align_pose = PoseStamped() - align_pose.header.frame_id = 'hand_gripper_tool_frame' - align_pose.pose.orientation.z = 1 - align_pose.pose.orientation.w = -0.04 - align_pose.pose.position.x = 1.70 - align_pose.pose.position.y = 1.75 - align_pose.pose.position.z = 1.2 + # align_pose = PoseStamped() + # align_pose.header.frame_id = 'hand_gripper_tool_frame' + # align_pose.pose.orientation.z = 1 + # align_pose.pose.orientation.w = -0.04 + # align_pose.pose.position.x = 1.70 + # align_pose.pose.position.y = 1.75 + # align_pose.pose.position.z = 1.2 action = ContextTypes.context_action.value(content=ContextActionModes.grasping.value) from_above = ContextTypes.context_from_above.value(content=False)