added eval for the intermediate and position controller

catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller.py

+import rospy
+import sys
+from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+def check_joint_positions_executed(target_positions):
+    # Initialize the MoveIt Commander
+    roscpp_initialize(sys.argv)
+    
+    # Initialize the ROS-Node
+    rospy.init_node('position_controller_eval', anonymous=True)
+    
+    # Initialize the MoveGroup
+    group = MoveGroupCommander('panda_arm')
+    
+    # Check each target position
+    for i, target_position in enumerate(target_positions):
+        group.set_joint_value_target(target_position)
+        plan = group.go(wait=True)
+        group.stop()
+        group.clear_pose_targets()
+        
+        current_joint_values = group.get_current_joint_values()
+        if not are_joint_positions_close(current_joint_values, target_position):
+            rospy.logwarn(f"Target position {i+1} not reached. Expected: {target_position}, but got: {current_joint_values}")
+            return False
+        rospy.sleep(1)
+    
+    return True
+
+def are_joint_positions_close(current, target, tolerance=0.1):
+    return all(abs(c - t) < tolerance for c, t in zip(current, target))
+
+if __name__ == "__main__":
+    target_positions = [
+        [0.5, -0.5, 0.0, 0.0, 0.5, -0.5, 0.0],
+        [0.6, -0.6, 0.2, 0.2, 0.5, -0.5, 0.2],
+        [0.7, -0.2, -0.5, -0.2, -0.5, 0.7, -0.2]
+    ]
+    check_joint_positions_executed(target_positions)
+    rospy.signal_shutdown("Evaluation completed")
+    roscpp_shutdown()

catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_basics/move_it_basics_3_eval.py

@@ -4,7 +4,6 @@ import math
 import rospy
 from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown
 
-
 rospy.loginfo("exercise_4_1_evaluation")
 
 roscpp_initialize(sys.argv)
@@ -13,24 +12,29 @@ rospy.init_node('exercise_4_1_evaluation', anonymous=True)
 group = MoveGroupCommander("panda_arm")
 
 current_pose = group.get_current_pose().pose
-expected_pose = [0.1, 0.1, 0.1, 0.0, 0.0, 0.0, 1.0]
-error_margin = 0.01
+expected_pose = [0.5, 0, 0.6, 1, 0.0, 0.0, 0.0]
+error_margin = 0.1
 
 current_values = [
     current_pose.position.x,
     current_pose.position.y,
     current_pose.position.z,
+    current_pose.orientation.w,
     current_pose.orientation.x,
     current_pose.orientation.y,
-    current_pose.orientation.z,
-    current_pose.orientation.w
+    current_pose.orientation.z
 ]
 
 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)
         print("false, Pose component {} is not in the correct position".format(i+1))
         sys.exit(0)
 

catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py

-print("TODO: WRITE EVALUATION SCRIPT")
\ No newline at end of file
+import math
+import os
+import importlib.util
+from geometry_msgs.msg import Pose, Point, Quaternion
+
+# Dynamically import waypoints module
+waypoints_path = None
+for root, dirs, files in os.walk('/'):
+    if 'converted.py' in files:
+        waypoints_path = os.path.join(root, 'converted.py')
+        break
+
+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 = waypoints_module.waypoints
+else:
+    raise FileNotFoundError("The converted.py file was not found in the file system")
+
+print("Waypoints:")
+print(waypoints)
+
+sol = [0.4, 0.2, 0.7, -1.0, 0.4, 0.0, 0.0]
+
+pose1 = Pose(
+    position=Point(sol[0] + 0.1, sol[1], sol[2]),
+    orientation=Quaternion(sol[3], sol[4], sol[5], sol[6])
+)
+
+pose2 = Pose(
+    position=Point(sol[0] + 0.1, sol[1] + 0.2, sol[2]),
+    orientation=Quaternion(sol[3], sol[4], sol[5], sol[6])
+)
+
+pose3 = Pose(
+    position=Point(sol[0] + 0.1, sol[1] + 0.2, sol[2] + 0.1),
+    orientation=Quaternion(sol[3], sol[4], sol[5], sol[6])
+)
+
+target_waypoints = [pose1, pose2, pose3]
+
+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
+
+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)
+
+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

catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/position_controller.ipynb

@@ -252,6 +252,30 @@
     "\n",
     "##### DO NOT CHANGE #####"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "##### DO NOT CHANGE #####\n",
+    "\n",
+    "# Wechsel zurück zum effort_joint_trajectory_controller\n",
+    "rospy.wait_for_service('/controller_manager/switch_controller')\n",
+    "try:\n",
+    "    switch_controller = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)\n",
+    "    req = SwitchControllerRequest()\n",
+    "    req.stop_controllers = ['position_joint_trajectory_controller']\n",
+    "    req.start_controllers = ['effort_joint_trajectory_controller']\n",
+    "    req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+    "    response = switch_controller(req)\n",
+    "    assert response.ok, \"Controller konnte nicht gewechselt werden.\"\n",
+    "except rospy.ServiceException as e:\n",
+    "    rospy.logerr(\"Service call failed: %s\" % e)\n",
+    "\n",
+    "##### DO NOT CHANGE #####"
+   ]
   }
  ],
  "metadata": {

catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_basics/move_it_basics_3.ipynb

@@ -30,30 +30,34 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
+    "### Introduction planning algorithms\n",
     "### Introduction planning algorithms\n",
     "\n",
     "#### What are planning algorithms?\n",
     "\n",
     "- Methods used to calculate a sequence of movements or actions that the robot must perform in order to reach a target (e.g. a final position)\n",
-    "- help solve complex problems, such as overcoming obstacles, precise reaching of endpoint\n",
-    "- ensure that movement is efficient, collision-free and physically possible\n",
+    "- Help solve complex problems, such as overcoming obstacles, precise reaching of endpoint\n",
+    "- Ensure that movement is efficient, collision-free, and physically possible\n",
     "\n",
-    "#### What types of planning algorithms are there ?\n",
+    "#### What types of planning algorithms are there?\n",
     "\n",
-    "Sampling-based algorithms\n",
+    "##### Sampling-based algorithms\n",
     "\n",
-    "- create random sample to find way from start to finish\n",
-    "- Examples: RTT, RTTConnect\n",
+    "- Create random samples to find a way from start to finish\n",
+    "- Examples: RRT, RRTConnect\n",
     "\n",
     "##### Search-based algorithms\n",
     "\n",
-    "- use grids or graphs to calculate shortest path\n",
+    "- Use grids or graphs to calculate the shortest path\n",
     "- Example: A*\n",
     "\n",
-    "##### Optimization based algorithms\n",
+    "##### Optimization-based algorithms\n",
+    "\n",
+    "- Improve existing paths through optimization criteria, such as efficiency\n",
+    "- Examples: CHOMP, STOMP\n",
     "\n",
-    "- improve existing paths through optimization criteria, such as efficiency\n",
-    "- Examples: CHOMP, STOMP"
+    "Important:\n",
+    "When using a planning algorithm, it is important to ensure that it is defined in the MoveIt configuration file (.yaml)."
    ]
   },
   {
@@ -100,7 +104,7 @@
    "source": [
     "Next, we want to select the planning algorithm we want to use to calculate the motion. To do this, we use the set_planner_id method to select the specific planning algorithm.\n",
     "\n",
-    "Next, your task is to select the planning algorithm with the ID RRTConnectkConfigDefault for the previously defined MoveGroupCommander instance."
+    "Next, your task is to select the planning algorithm with the ID geometric::RRTConnect for the previously defined MoveGroupCommander instance."
    ]
   },
   {
@@ -112,11 +116,31 @@
     "##### YOUR CODE HERE #####\n",
     "\n",
     "# Select the desired planning algorithm\n",
-    "group.set_planner_id(\"RRTConnectkConfigDefault\")\n",
+    "group.set_planner_id(\"RRTConnect\")\n",
     "\n",
     "##### YOUR CODE HERE #####"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Here, the current pose of the robot's end effector is retrieved and stored in the variable current_pose."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "##### DO NOT CHANGE #####\n",
+    "\n",
+    "current_pose = group.get_current_pose().pose\n",
+    "\n",
+    "##### DO NOT CHANGE #####"
+   ]
+  },
   {
    "cell_type": "markdown",
    "metadata": {},
@@ -126,13 +150,15 @@
     "The pose class provides a data structure that describes the spatial position and orientation of an object in 3D space. The position contains the x-, y- and z-coordinates, which in the following code defines the target position of the end effector in Cartesian space (in meters).\n",
     "The orientation is represented by a quaternion, where the w component describes the rotation around the axis.\n",
     "\n",
-    "Your task now is to define a new pose, which has the point (0.1, 0.1, 0.1) and the orientation 1 (neutral).\n",
+    "Your task now is to define a new pose, which adds 0.2 to the current x - koordinate, and does not change the y - and z - koordinate and the orientation w 1.\n",
     "\n",
     "Procedure:\n",
     "\n",
     "- 1. Create a new object of type Pose\n",
     "- 2. Set the variables for the position\n",
-    "- 3. Set the required variables for the orientation"
+    "- 3. Set the required variables for the orientation\n",
+    "\n",
+    "Finally, add the target pose to the planning queue using the set_pose_target method of the MoveGroupCommander instance."
    ]
   },
   {
@@ -144,11 +170,14 @@
     "##### YOUR CODE HERE #####\n",
     "\n",
     "target_pose = Pose()\n",
-    "target_pose.position.x = 0.1\n",
-    "target_pose.position.y = 0.1\n",
-    "target_pose.position.z = 0.1\n",
+    "target_pose.position.x = current_pose.position.x + 0.2\n",
+    "target_pose.position.y = current_pose.position.y\n",
+    "target_pose.position.z = current_pose.position.z\n",
     "target_pose.orientation.w = 1.0\n",
     "\n",
+    "# add the target pose to the planning queue\n",
+    "group.set_pose_target(target_pose)\n",
+    "\n",
     "##### YOUR CODE HERE #####"
    ]
   },
@@ -176,13 +205,13 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "##### DO NOT CHANGE #####\n",
+    "##### YOUR CODE HERE #####\n",
     "\n",
     "plan = group.go(wait=True)\n",
     "group.stop()\n",
     "group.clear_pose_targets()\n",
     "\n",
-    "##### DO NOT CHANGE #####"
+    "##### YOUR CODE HERE #####"
    ]
   },
   {

catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb

@@ -158,7 +158,7 @@
     "waypoint_2.position.x = waypoint_1.position.x\n",
     "waypoint_2.position.y = waypoint_1.position.y + 0.2\n",
     "waypoint_2.position.z = waypoint_1.position.z\n",
-    "waypoint_2.orientation = current_pose.orientation\n",
+    "waypoint_2.orientation = waypoint_1.orientation\n",
     "waypoints.append(waypoint_2)\n",
     "\n",
     "##### YOUR CODE HERE #####"
@@ -170,7 +170,7 @@
    "source": [
     "The last waypoint should also be of the type Pose.\n",
     "\n",
-    "Hold the X and Y positions from the second waypoint and move the Z position 0.3 (30 cm) up.\n",
+    "Hold the X and Y positions from the second waypoint and move the Z position 0.1 (10 cm) up.\n",
     "Take the orientation from the current_pose.\n",
     "\n",
     "Add the new waypoint to the waypoints list."
@@ -187,7 +187,7 @@
     "waypoint_3 = Pose()\n",
     "waypoint_3.position.x = waypoint_2.position.x\n",
     "waypoint_3.position.y = waypoint_2.position.y\n",
-    "waypoint_3.position.z = waypoint_2.position.z + 0.3\n",
+    "waypoint_3.position.z = waypoint_2.position.z + 0.1\n",
     "waypoint_3.orientation = current_pose.orientation\n",
     "waypoints.append(waypoint_3)\n",
     "\n",
@@ -242,9 +242,8 @@
    "source": [
     "##### YOUR CODE HERE #####\n",
     "\n",
-    "while fraction < 1.0 and attempts < max_attempts:\n",
-    "    (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)  # Plan the trajectory\n",
-    "    attempts += 1\n",
+    "\n",
+    "(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0)  # Plan the trajectory\n",
     "\n",
     "##### YOUR CODE HERE #####"
    ]
@@ -253,7 +252,8 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "The executed method now executes the created plan."
+    "### Execute the planned path\n",
+    "Now we will execute the planned path."
    ]
   },
   {
@@ -264,10 +264,8 @@
    "source": [
     "##### DO NOT CHANGE #####\n",
     "\n",
-    "if fraction == 1.0:\n",
-    "    group.execute(plan, wait=True)\n",
-    "else:\n",
-    "    rospy.logwarn(f\"Failed to compute a complete path after {attempts} attempts.\")\n",
+    "# Execute the planned path\n",
+    "group.execute(plan, wait=True)\n",
     "\n",
     "##### DO NOT CHANGE #####"
    ]

catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_3.ipynb

@@ -117,6 +117,25 @@
                 "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",
+                "# Select the desired planning algorithm\n",
+                "group.set_planner_id(\"RRTConnectkConfigDefault\")\n",
+                "\n",
+                "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+            ]
+        },
         {
             "cell_type": "markdown",
             "metadata": {},
@@ -147,6 +166,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",
+                "target_pose = Pose()\n",
+                "target_pose.position.x = 0.2\n",
+                "target_pose.position.y = 0.25\n",
+                "target_pose.position.z = 0.3\n",
+                "target_pose.orientation.w = 1.0\n",
+                "\n",
+                "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+            ]
+        },
         {
             "cell_type": "markdown",
             "metadata": {},

catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_4.ipynb

@@ -116,6 +116,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",
+                "# Query the current joint values\n",
+                "joint_values = group.get_current_joint_values() \n",
+                "rospy.loginfo(\"Current Joint Values: \")\n",
+                "for i, joint in enumerate(joint_values):\n",
+                "    rospy.loginfo(f\"Joint {i+1}: {joint:.2f} rad\")\n",
+                "\n",
+                "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+            ]
+        },
         {
             "cell_type": "markdown",
             "metadata": {},
@@ -143,6 +165,27 @@
                 "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",
+                "# Query the current end-effector pose\n",
+                "pose = group.get_current_pose().pose\n",
+                "rospy.loginfo(f\"Endeffector Position: x={pose.position.x:.3f}, y={pose.position.y:.3f}, z={pose.position.z:.3f}\")\n",
+                "rospy.loginfo(f\"Endeffector Orientation: x={pose.orientation.x:.3f}, y={pose.orientation.y:.3f}, z={pose.orientation.z:.3f}, w={pose.orientation.w:.3f}\")\n",
+                "\n",
+                "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##\n"
+            ]
+        },
         {
             "cell_type": "markdown",
             "metadata": {},