From ce44a087939ed79f2e61c6e997ea6246387afca4 Mon Sep 17 00:00:00 2001
From: uninh <uninh@student.kit.edu>
Date: Fri, 17 Jan 2025 01:46:31 +0000
Subject: [PATCH] adapted evaluations
---
.../learn_environment/converter/waypoints.py | 6 +-
.../activate_controller_eval.py | 19 +-
.../position_controller_eval.py | 8 +-
.../controller_task/stop_controller_eval.py | 5 +-
.../move_it_basics/move_it_basics_3_eval.py | 12 +-
.../move_it_intermediate_eval_1.py | 18 +-
.../move_it_objects/move_it_objects_1_eval.py | 36 ++--
.../move_it_objects/move_it_objects_2_eval.py | 132 +++++++++++---
.../move_it_objects/move_it_objects_3_eval.py | 163 ++++--------------
.../task_pool/reset_robot.py | 53 ++++--
.../move_it_objects/move_it_objects_1.ipynb | 2 +
.../move_it_objects/move_it_objects_2.ipynb | 2 +
.../move_it_objects/move_it_objects_3.ipynb | 4 +-
.../task_pool/task_definitions.json | 10 +-
.../move_it_objects/move_it_objects_3.ipynb | 22 +++
15 files changed, 270 insertions(+), 222 deletions(-)
diff --git a/catkin_ws/src/learn_environment/converter/waypoints.py b/catkin_ws/src/learn_environment/converter/waypoints.py
index d5e2678..b5657a8 100644
--- a/catkin_ws/src/learn_environment/converter/waypoints.py
+++ b/catkin_ws/src/learn_environment/converter/waypoints.py
@@ -1,7 +1,7 @@
from geometry_msgs.msg import Pose, Point, Quaternion
waypoints = [
- Pose(position=Point(x=0.4069354889438582, y=-6.141990041987915e-05, z=0.5904324844709694), orientation=Quaternion(x=-0.923826637648789, y=0.3828108985974372, z=-0.0003682582188372592, w=0.00015450385093557074)),
- Pose(position=Point(x=0.4069354889438582, y=0.19993858009958013, z=0.5904324844709694), orientation=Quaternion(x=-0.923826637648789, y=0.3828108985974372, z=-0.0003682582188372592, w=0.00015450385093557074)),
- Pose(position=Point(x=0.4069354889438582, y=0.19993858009958013, z=0.6904324844709694), orientation=Quaternion(x=-0.923826637648789, y=0.3828108985974372, z=-0.0003682582188372592, w=0.00015450385093557074)),
+ Pose(position=Point(x=0.4069321064888144, y=5.241540340455708e-05, z=0.5904369797681918), orientation=Quaternion(x=-0.9239236763900451, y=0.38257664636403016, z=-0.00035301110327771995, w=0.00015888963427430322)),
+ Pose(position=Point(x=0.4069321064888144, y=0.20005241540340457, z=0.5904369797681918), orientation=Quaternion(x=-0.9239236763900451, y=0.38257664636403016, z=-0.00035301110327771995, w=0.00015888963427430322)),
+ Pose(position=Point(x=0.4069321064888144, y=0.20005241540340457, z=0.6904369797681917), orientation=Quaternion(x=-0.9239236763900451, y=0.38257664636403016, z=-0.00035301110327771995, w=0.00015888963427430322)),
]
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/activate_controller_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/activate_controller_eval.py
index 02bbf37..5946d90 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/activate_controller_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/activate_controller_eval.py
@@ -11,11 +11,20 @@ def evaluate_controller_activation():
# Filter the running controllers
running_controllers = [controller.name for controller in controllers if controller.state == 'running']
- # Check if exactly one controller is running
- assert len(running_controllers) == 1, f"Expected 1 running controller, but found {len(running_controllers)}"
- # Check if the running controller is 'position_joint_trajectory_controller'
- assert running_controllers[0] == 'position_joint_trajectory_controller', \
- f"Expected 'position_joint_trajectory_controller' to be running, but found {running_controllers[0]}"
+ try:
+ # Check if exactly one controller is running
+ if len(running_controllers) != 1:
+ print(f"Expected 1 running controller, but found {len(running_controllers)}")
+ raise AssertionError(f"Expected 1 running controller, but found {len(running_controllers)}")
+ # Check if the running controller is 'position_joint_trajectory_controller'
+ if running_controllers[0] != 'position_joint_trajectory_controller':
+ print(f"Expected 'position_joint_trajectory_controller' to be running, but found {running_controllers[0]}")
+ raise AssertionError(f"Expected 'position_joint_trajectory_controller' to be running, but found {running_controllers[0]}")
+ print(True)
+ except AssertionError as e:
+ print(e)
+ print(False)
+
if __name__ == "__main__":
evaluate_controller_activation()
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller_eval.py
index e42c5ab..36c3fea 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller_eval.py
@@ -25,11 +25,13 @@ rospy.sleep(5)
# Verify joint positions
for i in range(len(target_positions)):
if abs(current_positions[i] - target_positions[i]) > tolerance:
- rospy.loginfo(f"Joint {i+1} is not in the target position.")
- rospy.loginfo(current_positions)
+ print(f"Joint {i+1} is not in the target position.")
+ print(current_positions)
+ print(False)
break
else:
- rospy.loginfo("All joints are in the correct positions.")
+ print("All joints are in the correct positions.")
+ print(True)
# Shutdown the ROS node
rospy.signal_shutdown("Evaluation completed")
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/stop_controller_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/stop_controller_eval.py
index 2f85a28..3076f1b 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/stop_controller_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/stop_controller_eval.py
@@ -10,10 +10,13 @@ def evaluate_stop_controllers():
if not active_controllers:
print("Evaluation passed: No active controllers.")
+ print(True)
else:
print("Evaluation failed: There are still active controllers:", active_controllers)
+ print(False)
except rospy.ServiceException as e:
- rospy.logerr("Service call failed: %s" % e)
+ print("Service call failed: %s" % e)
+ print(False)
if __name__ == "__main__":
evaluate_stop_controllers()
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_basics/move_it_basics_3_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_basics/move_it_basics_3_eval.py
index c937512..0bef53d 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_basics/move_it_basics_3_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_basics/move_it_basics_3_eval.py
@@ -25,17 +25,15 @@ current_values = [
for i, (current, expected) in enumerate(zip(current_values, expected_pose)):
if abs(current - expected) > error_margin:
-
roscpp_shutdown()
- print("hello")
print("Orientation")
print(current_pose.orientation.x,
- current_pose.orientation.y,
- current_pose.orientation.z,
- current_pose.orientation.w)
+ current_pose.orientation.y,
+ current_pose.orientation.z,
+ current_pose.orientation.w)
print("false, Pose component {} is not in the correct position".format(i+1))
- sys.exit(0)
+ print(False)
roscpp_shutdown()
-print("true")
\ No newline at end of file
+print(True)
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py
index 5c16d17..45712ad 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py
@@ -3,24 +3,26 @@ from geometry_msgs.msg import Pose, Point, Quaternion
import os
import importlib.util
+# Search for the file 'waypoints.py' in the file system
waypoints_path = None
for root, dirs, files in os.walk('/'):
if 'waypoints.py' in files:
waypoints_path = os.path.join(root, 'waypoints.py')
break
+# Load the 'waypoints' module dynamically if the file is found
if waypoints_path:
spec = importlib.util.spec_from_file_location('waypoints', waypoints_path)
waypoints_module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(waypoints_module)
waypoints_user = waypoints_module.waypoints
- print(waypoints_user)
else:
raise FileNotFoundError('waypoints.py not found')
-
+# Example solution values
sol = [0.4, 0.2, 0.7, -1.0, 0.4, 0.0, 0.0]
+# Define target pose waypoints based on the solution values
pose1 = Pose(
position=Point(sol[0] + 0.1, sol[1], sol[2]),
orientation=Quaternion(sol[3], sol[4], sol[5], sol[6])
@@ -36,8 +38,10 @@ pose3 = Pose(
orientation=Quaternion(sol[3], sol[4], sol[5], sol[6])
)
+# List of target pose waypoints
target_waypoints = [pose1, pose2, pose3]
+# Predefined waypoints
waypoints = [
Pose(
position=Point(0.4, 0.0, 0.6),
@@ -53,16 +57,24 @@ waypoints = [
)
]
+# Function to check if two points are close to each other
def is_close(p1, p2, tolerance=0.2):
return abs(p1.x - p2.x) <= tolerance and abs(p1.y - p2.y) <= tolerance and abs(p1.z - p2.z) <= tolerance
+# Function to check if two quaternions are close to each other
def quaternion_is_close(q1, q2, tolerance=0.1):
return (abs(q1.x - q2.x) <= tolerance and abs(q1.y - q2.y) <= tolerance and
abs(q1.z - q2.z) <= tolerance and abs(q1.w - q2.w) <= tolerance)
+success = True
+
+# Check if the target pose waypoints match the predefined waypoints
for i in range(3):
if (is_close(target_waypoints[i].position, waypoints[i].position) and
quaternion_is_close(target_waypoints[i].orientation, waypoints[i].orientation)):
print(f"Pose {i+1} matches.")
else:
- print(f"Pose {i+1} does not match.")
\ No newline at end of file
+ print(f"Pose {i+1} does not match.")
+ success = False
+
+print(success)
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_1_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_1_eval.py
index 98448c4..3f357d1 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_1_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_1_eval.py
@@ -12,14 +12,6 @@ rospy.sleep(2) # Allow time for the planning scene interface to initialize
# Define the expected pose of the cylinder object
def expected_cylinder_pose():
- """
- Creates and returns the expected pose of the cylinder object.
- The pose is defined relative to the "panda_link0" frame with
- specific position and orientation values.
-
- Returns:
- PoseStamped: The expected pose of the cylinder.
- """
pose = PoseStamped()
pose.header.frame_id = "panda_link0"
pose.pose.position.x = 2.0
@@ -78,23 +70,12 @@ def expected_box_2_pose():
# Verify if an object in the scene matches the expected pose
def verify_object(object_name, expected_pose, tolerance=0.1):
- """
- Verifies whether an object exists in the scene and matches the
- expected pose within a specified tolerance.
-
- Args:
- object_name (str): The name of the object to verify.
- expected_pose (PoseStamped): The expected pose of the object.
- tolerance (float): The maximum allowed difference between the current
- and expected pose values for a match.
- Returns:
- bool: True if the object exists and matches the expected pose; otherwise, False.
- """
current_objects = scene.get_objects() # Retrieve all objects currently in the scene
if object_name not in current_objects:
rospy.logerr(f"The object '{object_name}' was not found.") # Log an error if the object is missing
+ print(f"The object '{object_name}' was not found.")
return False
# Retrieve the current pose of the object from the scene
@@ -114,12 +95,15 @@ def verify_object(object_name, expected_pose, tolerance=0.1):
# Log results and return whether both position and orientation match
if position_match and orientation_match:
rospy.loginfo(f"The object '{object_name}' is in the correct position and orientation.")
+ print(f"The object '{object_name}' is in the correct position and orientation.")
return True
elif not position_match:
rospy.logerr(f"The object '{object_name}' is NOT in the correct position.")
+ print(f"The object '{object_name}' is NOT in the correct position.")
return False
else:
rospy.logerr(f"The object '{object_name}' is NOT in the correct orientation.")
+ print(f"The object '{object_name}' is NOT in the correct orientation.")
return False
@@ -129,13 +113,19 @@ box_1_pose = expected_box_1_pose()
box_2_pose = expected_box_2_pose()
# Check if the cylinder is correctly placed in the scene
-verify_object("cylinder", cylinder_pose)
+cylinder_result = verify_object("cylinder", cylinder_pose)
# Check if box_1 is correctly placed in the scene
-verify_object("box_1", box_1_pose)
+box_1_result = verify_object("box_1", box_1_pose)
# Check if box_2 is correctly placed in the scene
-verify_object("box_2", box_2_pose)
+box_2_result = verify_object("box_2", box_2_pose)
+
+# Gebe das Gesamtergebnis aus
+if cylinder_result and box_1_result and box_2_result:
+ print(True)
+else:
+ print(False)
# Additional objects can be added for verification as needed
# Example:
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_2_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_2_eval.py
index 386922f..3c04183 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_2_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_2_eval.py
@@ -1,34 +1,116 @@
import rospy
-from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
+from moveit_commander import PlanningSceneInterface
+from geometry_msgs.msg import PoseStamped
-def evaluate_scene_empty():
- """
- Checks whether all objects have been removed from the planning environment.
- Issues a success message if there are no objects in the scene,
- Otherwise, a warning.
- """
- # Initialize ROS and PlanningSceneInterface
- roscpp_initialize([])
- rospy.init_node('evaluate_scene_empty', anonymous=True)
+# Initialize the ROS node for verifying objects in the scene
+rospy.init_node('verify_objects', anonymous=True)
- # Interface for PlanningScene
- scene = PlanningSceneInterface()
+# Create an interface for interacting with the planning scene
+scene = PlanningSceneInterface()
+rospy.sleep(2)
- # Short delay to make sure the scene is loaded
- rospy.sleep(1)
- # Query the currently existing objects in the scene
- current_objects = scene.get_known_object_names()
+# Define the expected pose of the target object
+def expected_target_pose():
+ pose = PoseStamped()
+ pose.header.frame_id = "panda_link0"
+ pose.pose.position.x = 0.5
+ pose.pose.position.y = 0.0
+ pose.pose.position.z = 0.5
+ pose.pose.orientation.x = 0.0
+ pose.pose.orientation.y = 0.0
+ pose.pose.orientation.z = 0.0
+ pose.pose.orientation.w = 1.0
+ return pose
- if not current_objects:
- rospy.loginfo("The scene is empty. All objects have been successfully removed.")
+
+# Define the expected pose of rectangle_1
+def expected_rectangle_1_pose():
+ pose = PoseStamped()
+ pose.header.frame_id = "panda_link0"
+ pose.pose.position.x = 0.5
+ pose.pose.position.y = 0.0
+ pose.pose.position.z = 0.2
+ pose.pose.orientation.x = 0.0
+ pose.pose.orientation.y = 0.0
+ pose.pose.orientation.z = 0.0
+ pose.pose.orientation.w = 1.0
+ return pose
+
+
+# Define the expected pose of rectangle_2
+def expected_rectangle_2_pose():
+ pose = PoseStamped()
+ pose.header.frame_id = "panda_link0"
+ pose.pose.position.x = 0.0
+ pose.pose.position.y = 0.5
+ pose.pose.position.z = 0.2
+ pose.pose.orientation.x = 0.0
+ pose.pose.orientation.y = 0.0
+ pose.pose.orientation.z = 0.0
+ pose.pose.orientation.w = 1.0
+ return pose
+
+
+# Verify if an object in the scene matches the expected pose
+def verify_object(object_name, expected_pose, tolerance=0.1):
+
+ current_objects = scene.get_objects()
+
+ if object_name not in current_objects:
+ rospy.logerr(f"The object '{object_name}' was not found.")
+ print(f"The object '{object_name}' was not found.")
+ return False
+
+ # Retrieve the current pose of the object from the scene
+ current_pose = current_objects[object_name].pose
+
+ # Compare the object's position with the expected position
+ position_match = (abs(current_pose.position.x - expected_pose.pose.position.x) < tolerance and
+ abs(current_pose.position.y - expected_pose.pose.position.y) < tolerance and
+ abs(current_pose.position.z - expected_pose.pose.position.z) < tolerance)
+
+ # Compare the object's orientation with the expected orientation
+ orientation_match = (abs(current_pose.orientation.x - expected_pose.pose.orientation.x) < tolerance and
+ abs(current_pose.orientation.y - expected_pose.pose.orientation.y) < tolerance and
+ abs(current_pose.orientation.z - expected_pose.pose.orientation.z) < tolerance and
+ abs(current_pose.orientation.w - expected_pose.pose.orientation.w) < tolerance)
+
+ # Log results and return whether both position and orientation match
+ if position_match and orientation_match:
+ rospy.loginfo(f"The object '{object_name}' is in the correct position and orientation.")
+ print(f"The object '{object_name}' is in the correct position and orientation.")
+ return True
+ elif not position_match:
+ rospy.logerr(f"The object '{object_name}' is NOT in the correct position.")
+ print(f"The object '{object_name}' is NOT in the correct position.")
+ return False
else:
- rospy.logwarn("The scene is not empty. The following objects are still there:")
- for obj in current_objects:
- rospy.logwarn(f" - {obj}")
+ rospy.logerr(f"The object '{object_name}' is NOT in the correct orientation.")
+ print(f"The object '{object_name}' is NOT in the correct orientation.")
+ return False
+
+
+# Verify the positions and orientations of the expected objects
+target_pose = expected_target_pose()
+rectangle_1_pose = expected_rectangle_1_pose()
+rectangle_2_pose = expected_rectangle_2_pose()
+
+# Check if the target is correctly placed in the scene
+target_result = verify_object("target", target_pose)
+
+# Check if rectangle_1 is correctly placed in the scene
+rectangle_1_result = verify_object("rectangle_1", rectangle_1_pose)
+
+# Check if rectangle_2 is correctly placed in the scene
+rectangle_2_result = verify_object("rectangle_2", rectangle_2_pose)
- # Shutdown ROS
- roscpp_shutdown()
+# Print the final result
+if target_result and rectangle_1_result and rectangle_2_result:
+ print(True)
+else:
+ print(False)
-if __name__ == "__main__":
- evaluate_scene_empty()
\ No newline at end of file
+# Additional objects can be added for verification as needed
+# Example:
+# verify_object("another_object", expected_other_object_pose())
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_3_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_3_eval.py
index 86f4058..4de48d5 100644
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_3_eval.py
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_objects/move_it_objects_3_eval.py
@@ -1,142 +1,39 @@
import rospy
-from moveit_commander import PlanningSceneInterface
-from geometry_msgs.msg import PoseStamped
+from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
-# Initialize the ROS node for verifying objects in the scene
-rospy.init_node('verify_objects', anonymous=True)
-
-# Create an interface for interacting with the planning scene
-scene = PlanningSceneInterface()
-rospy.sleep(2) # Allow time for the planning scene interface to initialize
-
-
-# Define the expected pose of the target object
-def expected_target_pose():
- """
- Creates and returns the expected pose of the target object.
- The pose is defined relative to the "panda_link0" frame with
- specific position and orientation values.
-
- Returns:
- PoseStamped: The expected pose of the target.
- """
- pose = PoseStamped()
- pose.header.frame_id = "panda_link0"
- pose.pose.position.x = 0.5
- pose.pose.position.y = 0.0
- pose.pose.position.z = 0.5
- pose.pose.orientation.x = 0.0
- pose.pose.orientation.y = 0.0
- pose.pose.orientation.z = 0.0
- pose.pose.orientation.w = 1.0
- return pose
-
-
-# Define the expected pose of rectangle_1
-def expected_rectangle_1_pose():
+def evaluate_scene_empty():
"""
- Creates and returns the expected pose of rectangle_1.
- The pose is defined relative to the "panda_link0" frame with
- specific position and orientation values.
-
- Returns:
- PoseStamped: The expected pose of rectangle_1.
+ Checks whether all objects have been removed from the planning environment.
+ Issues a success message if there are no objects in the scene,
+ Otherwise, a warning.
"""
- pose = PoseStamped()
- pose.header.frame_id = "panda_link0"
- pose.pose.position.x = 0.5
- pose.pose.position.y = 0.0
- pose.pose.position.z = 0.2
- pose.pose.orientation.x = 0.0
- pose.pose.orientation.y = 0.0
- pose.pose.orientation.z = 0.0
- pose.pose.orientation.w = 1.0
- return pose
+ # Initialize ROS and PlanningSceneInterface
+ roscpp_initialize([])
+ rospy.init_node('evaluate_scene_empty', anonymous=True)
+ # Interface for PlanningScene
+ scene = PlanningSceneInterface()
-# Define the expected pose of rectangle_2
-def expected_rectangle_2_pose():
- """
- Creates and returns the expected pose of rectangle_2.
- The pose is defined relative to the "panda_link0" frame with
- specific position and orientation values.
+ # Short delay to make sure the scene is loaded
+ rospy.sleep(1)
- Returns:
- PoseStamped: The expected pose of rectangle_2.
- """
- pose = PoseStamped()
- pose.header.frame_id = "panda_link0"
- pose.pose.position.x = 0.0
- pose.pose.position.y = 0.5
- pose.pose.position.z = 0.2
- pose.pose.orientation.x = 0.0
- pose.pose.orientation.y = 0.0
- pose.pose.orientation.z = 0.0
- pose.pose.orientation.w = 1.0
- return pose
-
-
-# Verify if an object in the scene matches the expected pose
-def verify_object(object_name, expected_pose, tolerance=0.1):
- """
- Verifies whether an object exists in the scene and matches the
- expected pose within a specified tolerance.
+ # Query the currently existing objects in the scene
+ current_objects = scene.get_known_object_names()
- Args:
- object_name (str): The name of the object to verify.
- expected_pose (PoseStamped): The expected pose of the object.
- tolerance (float): The maximum allowed difference between the current
- and expected pose values for a match.
-
- Returns:
- bool: True if the object exists and matches the expected pose; otherwise, False.
- """
- current_objects = scene.get_objects() # Retrieve all objects currently in the scene
-
- if object_name not in current_objects:
- rospy.logerr(f"The object '{object_name}' was not found.") # Log an error if the object is missing
- return False
-
- # Retrieve the current pose of the object from the scene
- current_pose = current_objects[object_name].pose
-
- # Compare the object's position with the expected position
- position_match = (abs(current_pose.position.x - expected_pose.pose.position.x) < tolerance and
- abs(current_pose.position.y - expected_pose.pose.position.y) < tolerance and
- abs(current_pose.position.z - expected_pose.pose.position.z) < tolerance)
-
- # Compare the object's orientation with the expected orientation
- orientation_match = (abs(current_pose.orientation.x - expected_pose.pose.orientation.x) < tolerance and
- abs(current_pose.orientation.y - expected_pose.pose.orientation.y) < tolerance and
- abs(current_pose.orientation.z - expected_pose.pose.orientation.z) < tolerance and
- abs(current_pose.orientation.w - expected_pose.pose.orientation.w) < tolerance)
-
- # Log results and return whether both position and orientation match
- if position_match and orientation_match:
- rospy.loginfo(f"The object '{object_name}' is in the correct position and orientation.")
- return True
- elif not position_match:
- rospy.logerr(f"The object '{object_name}' is NOT in the correct position.")
- return False
+ if not current_objects:
+ rospy.loginfo("The scene is empty. All objects have been successfully removed.")
+ print("The scene is empty. All objects have been successfully removed.")
+ print(True)
else:
- rospy.logerr(f"The object '{object_name}' is NOT in the correct orientation.")
- return False
-
-
-# Verify the positions and orientations of the expected objects
-target_pose = expected_target_pose()
-rectangle_1_pose = expected_rectangle_1_pose()
-rectangle_2_pose = expected_rectangle_2_pose()
-
-# Check if the target is correctly placed in the scene
-verify_object("target", target_pose)
-
-# Check if rectangle_1 is correctly placed in the scene
-verify_object("rectangle_1", rectangle_1_pose)
-
-# Check if rectangle_2 is correctly placed in the scene
-verify_object("rectangle_2", rectangle_2_pose)
-
-# Additional objects can be added for verification as needed
-# Example:
-# verify_object("another_object", expected_other_object_pose())
\ No newline at end of file
+ rospy.logwarn("The scene is not empty. The following objects are still there:")
+ print("The scene is not empty. The following objects are still there:")
+ for obj in current_objects:
+ rospy.logwarn(f" - {obj}")
+ print(f" - {obj}")
+ print(False)
+
+ # Shutdown ROS
+ roscpp_shutdown()
+
+if __name__ == "__main__":
+ evaluate_scene_empty()
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/reset_robot.py b/catkin_ws/src/learn_environment/task_pool/reset_robot.py
index 6016e20..fb8c9a0 100644
--- a/catkin_ws/src/learn_environment/task_pool/reset_robot.py
+++ b/catkin_ws/src/learn_environment/task_pool/reset_robot.py
@@ -3,7 +3,7 @@ import math
import sys
from moveit_commander import MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
-
+from controller_manager_msgs.srv import ListControllers, LoadController, SwitchController, SwitchControllerRequest
def robot_already_in_default_position(group, robot_default_position):
current_joint_values = group.get_current_joint_values()
@@ -31,18 +31,43 @@ def reset_robot(group):
group.set_joint_value_target(joint_goal)
group.go(wait=True)
-
-def remove_objects(scene):
- objects = scene.get_known_object_names()
-
- for object_name in objects:
- scene.remove_world_object(object_name)
+def ensure_controllers_active(controllers):
+ # Ensure that the given controllers are loaded and running and that all other controllers are stopped.
+ rospy.wait_for_service('/controller_manager/list_controllers')
+ list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)
+ loaded_controllers = [controller.name for controller in list_controllers().controller]
+ running_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']
+
+ rospy.wait_for_service('/controller_manager/load_controller')
+ load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)
+
+ for controller in controllers:
+ if controller not in loaded_controllers:
+ response = load_service(controller)
+ assert response.ok, f"Controller {controller} could not be loaded."
+
+ rospy.wait_for_service('/controller_manager/switch_controller')
+ switch_service = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)
+ switch_req = SwitchControllerRequest()
+ switch_req.start_controllers = controllers
+ switch_req.stop_controllers = running_controllers
+ switch_req.strictness = SwitchControllerRequest.BEST_EFFORT
+ response = switch_service(switch_req)
+ assert response.ok, "Controllers could not be started."
rospy.sleep(2)
-
rospy.init_node('reset', anonymous=True)
roscpp_initialize(sys.argv)
+
+# remove all objects from the scene
+scene = PlanningSceneInterface(synchronous=True)
+scene.remove_world_object()
+
+print("Successfully reset the robot and removed all objects.")
+
+rospy.sleep(2)
+
group = MoveGroupCommander("panda_arm")
robot_default_position = [0.0, -math.pi / 4, 0.0, -math.pi * 3 / 4, 0.0, math.pi / 2, math.pi / 4]
@@ -50,12 +75,12 @@ robot_default_position = [0.0, -math.pi / 4, 0.0, -math.pi * 3 / 4, 0.0, math.pi
if not robot_already_in_default_position(group, robot_default_position):
reset_robot(group)
-scene = PlanningSceneInterface(synchronous=True)
+rospy.sleep(2)
-if len(scene.get_known_object_names()) > 0:
- remove_objects(scene)
+controllers_to_activate = ['franka_gripper', 'franka_state_controller', 'effort_joint_trajectory_controller']
+ensure_controllers_active(controllers_to_activate)
-roscpp_shutdown()
-rospy.signal_shutdown("Task completed.")
+rospy.sleep(2)
-print("Successfully reset the robot and removed all objects.")
\ No newline at end of file
+roscpp_shutdown()
+rospy.signal_shutdown("Task completed.")
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_1.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_1.ipynb
index 00c5e02..e807c5c 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_1.ipynb
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_1.ipynb
@@ -243,6 +243,8 @@
"\n",
"scene.add_cylinder(\"cylinder\", cylinder_pose, height=height, radius=radius)\n",
"\n",
+ "rospy.sleep(2)\n",
+ "\n",
"##### YOUR CODE HERE #####"
]
},
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_2.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_2.ipynb
index 5bafab6..6f45b67 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_2.ipynb
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_2.ipynb
@@ -243,6 +243,8 @@
"# add target_object to the planning scene\n",
"planning_scene_interface.add_object(target_object)\n",
"\n",
+ "rospy.sleep(2)\n",
+ "\n",
"##### YOUR CODE HERE #####"
]
},
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_3.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_3.ipynb
index be1e743..25c588c 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_3.ipynb
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_objects/move_it_objects_3.ipynb
@@ -72,7 +72,7 @@
"\n",
"# Initialize PlanningScene\n",
"scene = PlanningSceneInterface()\n",
- "rospy.sleep(1)\n",
+ "rospy.sleep(2)\n",
"\n",
"##### DO NOT CHANGE #####"
]
@@ -98,7 +98,7 @@
"scene.remove_world_object(\"box_2\")\n",
"scene.remove_world_object(\"cylinder\")\n",
"\n",
- "# Alternativ : remove_world_object()\n",
+ "# scene.remove_world_object()\n",
"\n",
"##### YOUR CODE HERE #####"
]
diff --git a/catkin_ws/src/learn_environment/task_pool/task_definitions.json b/catkin_ws/src/learn_environment/task_pool/task_definitions.json
index 00df4eb..cee0d61 100644
--- a/catkin_ws/src/learn_environment/task_pool/task_definitions.json
+++ b/catkin_ws/src/learn_environment/task_pool/task_definitions.json
@@ -89,24 +89,28 @@
"folder": "/move_it/move_it_objects",
"topic": "MoveIt",
"difficulty": "beginner",
+ "previous_subtasks_required": true,
"subtasks": [
{
"title": "Task 1 ",
"description": "Create an object in moveit",
"solution_file": "/move_it_objects_1.ipynb",
- "evaluation_file": "/move_it_objects_1_eval.py"
+ "evaluation_file": "/move_it_objects_1_eval.py",
+ "reset_robot_before_executing": true
},
{
"title": "Task 2 ",
"description": "Get to know new classes for creating objects",
"solution_file": "/move_it_objects_2.ipynb",
- "evaluation_file": "/move_it_objects_2_eval.py"
+ "evaluation_file": "/move_it_objects_2_eval.py",
+ "reset_robot_before_executing": true
},
{
"title": "Task 3 ",
"description": "Understand how to remove objects from a scene",
"solution_file": "/move_it_objects_3.ipynb",
- "evaluation_file": "/move_it_objects_3_eval.py"
+ "evaluation_file": "/move_it_objects_3_eval.py",
+ "reset_robot_before_executing": false
}
]
},
diff --git a/catkin_ws/src/learn_environment/tasks/move_it/move_it_objects/move_it_objects_3.ipynb b/catkin_ws/src/learn_environment/tasks/move_it/move_it_objects/move_it_objects_3.ipynb
index a6c5c46..0a64dd8 100644
--- a/catkin_ws/src/learn_environment/tasks/move_it/move_it_objects/move_it_objects_3.ipynb
+++ b/catkin_ws/src/learn_environment/tasks/move_it/move_it_objects/move_it_objects_3.ipynb
@@ -109,6 +109,28 @@
"raise NotImplementedError()"
]
},
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "solution_cell"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##############################################################\n#### THIS IS A SOLUTION CELL. IT WILL NOT EXECUTE. ####\n#### YOU CAN RUN THE SOLUTION DIRECTLY WITHIN THE PLUGIN. ####\n#### USE THIS CELL AS INSPIRATION FOR YOUR OWN CODE. ####\n##############################################################\n\n",
+ "## ↓↓↓↓ SOLUTION CODE HERE ↓↓↓↓ ##\n",
+ "\n",
+ "scene.remove_world_object(\"box_1\")\n",
+ "scene.remove_world_object(\"box_2\")\n",
+ "scene.remove_world_object(\"cylinder\")\n",
+ "\n",
+ "# Alternativ : remove_world_object()\n",
+ "\n",
+ "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+ ]
+ },
{
"cell_type": "markdown",
"metadata": {},
--
GitLab