diff --git a/catkin_ws/src/learn_environment/converter/waypoints.py b/catkin_ws/src/learn_environment/converter/waypoints.py
index 4521577460269123bc2f247bc9380150a71b3b45..d5e267856088d6462b939db9242e81571c6ece67 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.40688524573041374, y=2.5047173614282502e-05, z=0.5904684599013774), orientation=Quaternion(x=-0.9239080655138032, y=0.3826143303130406, z=-0.0003820078098665038, w=0.00012160459885735887)),
- Pose(position=Point(x=0.40688524573041374, y=0.2000250471736143, z=0.5904684599013774), orientation=Quaternion(x=-0.9239080655138032, y=0.3826143303130406, z=-0.0003820078098665038, w=0.00012160459885735887)),
- Pose(position=Point(x=0.40688524573041374, y=0.2000250471736143, z=0.6904684599013774), orientation=Quaternion(x=-0.9239080655138032, y=0.3826143303130406, z=-0.0003820078098665038, w=0.00012160459885735887)),
+ 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)),
]
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller.py
deleted file mode 100644
index 3c5957517c688483f08f897e5294b4faaf13966d..0000000000000000000000000000000000000000
--- a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller.py
+++ /dev/null
@@ -1,42 +0,0 @@
-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()
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
new file mode 100644
index 0000000000000000000000000000000000000000..2f2b23682a7cb8caebef813cf6daf63a01388eff
--- /dev/null
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/position_controller_eval.py
@@ -0,0 +1,43 @@
+import rospy
+from sensor_msgs.msg import JointState
+import math
+import moveit_commander
+import sys
+
+# Target positions
+target_positions = [math.pi / 2
+, -math.pi / 4, 0.0, -math.pi * 3 / 4, 0.0, math.pi / 2, math.pi / 4]
+
+# Tolerance for position verification
+tolerance = 0.1
+
+# Initialize ROS node
+rospy.init_node('position_controller_evaluation', anonymous=True)
+
+# Initialize MoveIt Commander
+moveit_commander.roscpp_initialize(sys.argv)
+group = moveit_commander.MoveGroupCommander("panda_arm")
+
+# Wait until joint states are received
+rospy.sleep(5)
+
+# Get current joint states
+current_positions = group.get_current_joint_values()
+
+# Wait until joint states are received
+rospy.sleep(5)
+
+# Verify joint positions
+if 'current_positions' in globals() and len(current_positions) == len(target_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)
+ break
+ else:
+ rospy.loginfo("All joints are in the target positions.")
+else:
+ rospy.loginfo("Joint states not received or length mismatch.")
+
+# Shutdown the ROS node
+rospy.signal_shutdown("Evaluation completed")
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/switch_controller_eval.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/switch_controller_eval.py
new file mode 100644
index 0000000000000000000000000000000000000000..a11cfec9eda2041f6af3177844c2186342c4fa94
--- /dev/null
+++ b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/controller/controller_task/switch_controller_eval.py
@@ -0,0 +1 @@
+print("NO NEED TO EVALUATE THIS SCRIPT")
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/position_controller.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/position_controller.ipynb
index 05fa9816ac38a326547164ddbdad6dbd2e24dc96..56801f3464b3ac71dcbd13ad3dbca67fb9cd10b0 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/position_controller.ipynb
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/position_controller.ipynb
@@ -15,12 +15,15 @@
"## Controller_Task\n",
"### Position_Controller\n",
"\n",
- "In this task, you will learn... \n",
+ "In this task, you will learn: \n",
">\n",
"> - how to work with a position controller to move the joints of a robot to specific positions.\n",
- "> - how to start the position controller\n",
- "> - how to create a message for the position controller\n",
- "> - how to send the target position and check the movement"
+ "> - how to stop all active controllers to ensure no conflicts occur.\n",
+ "> - how to start the position controller if it is not already running.\n",
+ "> - how to create a `JointTrajectory` message for the position controller.\n",
+ "> - how to define target positions for each joint and set the time to reach these positions.\n",
+ "> - how to send the target position to the controller and verify the movement.\n",
+ "> - how to switch back to the previously active controllers after completing the task."
]
},
{
@@ -32,9 +35,10 @@
"##### DO NOT CHANGE #####\n",
"\n",
"import rospy\n",
- "from controller_manager_msgs.srv import LoadController, SwitchController, SwitchControllerRequest, UnloadController\n",
+ "from controller_manager_msgs.srv import LoadController, SwitchController, SwitchControllerRequest, UnloadController, ListControllers\n",
"from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint\n",
"from moveit_commander import roscpp_shutdown\n",
+ "import math\n",
"\n",
"##### DO NOT CHANGE #####"
]
@@ -43,15 +47,68 @@
"cell_type": "markdown",
"metadata": {},
"source": [
- "The following code cell loads and activates a new controller in ROS after a potentially conflicting controller has been unloaded.\n",
+ "### Task 1:\n",
+ "\n",
+ "Stop all active controllers.\n",
+ "\n",
+ "**Important**\n",
+ "Use the `wait_for_service` method to wait for the `/controller_manager/list_controllers` service.\n",
+ "- Then create a service proxy for this service and retrieve the list of active controllers.\n",
+ "- Filter the controllers that have the state 'running' and save their names in a list.\n",
+ "- Use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.\n",
+ "- Then create a service proxy for this service.\n",
+ "- Create a new request (`SwitchControllerRequest`) and add the list of active controllers to the `stop_controllers` list.\n",
+ "- Set the `strictness` to `BEST_EFFORT`.\n",
+ "- Check if the service was successful."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### YOUR CODE HERE #####\n",
+ "\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "active_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']\n",
+ "\n",
+ "if active_controllers:\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 = active_controllers\n",
+ " req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+ " response = switch_controller(req)\n",
+ " assert response.ok, \"Active controllers could not be stopped.\"\n",
+ " except rospy.ServiceException as e:\n",
+ " rospy.logerr(\"Service call failed: %s\" % e)\n",
+ "\n",
+ "##### YOUR CODE HERE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Task 2:\n",
"\n",
- "First, the new controller and the conflicting controller are defined.\n",
+ "Activate the position controller.\n",
"\n",
- "Now the conflicting controller will load. First, the /controller_manager/switch_controller service is used to stop the controller. Then the /controller_manager service/unload_controller is used to completely unload. If any of the service calls fail, an error message is issued.\n",
+ "- First, use the `wait_for_service` method to wait for the `/controller_manager/list_controllers` service.\n",
+ "- Then create a service proxy for this service and retrieve the list of loaded controllers.\n",
"\n",
- "It then waits for the /controller_manager/load_controller service to load the new controller. The service is called and it checks that the controller has been loaded successfully.\n",
+ "- Check if the `position_joint_trajectory_controller` is already loaded.\n",
+ "- If not, use the `wait_for_service` method to wait for the `/controller_manager/load_controller` service.\n",
+ "- Then create a service proxy for this service and load the controller.\n",
"\n",
- "Finally, wait for the /controller_manager/switch_controller service to activate the new controller. The service is called and it checks that the controller has been successfully started."
+ "- Now use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.\n",
+ "- Then create a service proxy for this service.\n",
+ "- Create a new request (`SwitchControllerRequest`) and add the `position_joint_trajectory_controller` to the `start_controllers` list.\n",
+ "- Set the `strictness` to `BEST_EFFORT`.\n",
+ "- Check if the service was successful.\n"
]
},
{
@@ -60,47 +117,33 @@
"metadata": {},
"outputs": [],
"source": [
- "##### DO NOT CHANGE #####\n",
- "\n",
- "controller_name = 'position_joint_trajectory_controller'\n",
- "conflicting_controller = 'effort_joint_trajectory_controller'\n",
+ "##### YOUR CODE HERE #####\n",
"\n",
- "# Service for unloading the conflicting 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 = [conflicting_controller]\n",
- " req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
- " response = switch_controller(req)\n",
- " assert response.ok, \"Controller konnte nicht entladen werden.\"\n",
- "except rospy.ServiceException as e:\n",
- " rospy.logerr(\"Service call failed: %s\" % e)\n",
+ "# Define the controllers\n",
+ "controller_to_activate = 'position_joint_trajectory_controller'\n",
"\n",
- "rospy.wait_for_service('/controller_manager/unload_controller')\n",
- "try:\n",
- " unload_srv = rospy.ServiceProxy('/controller_manager/unload_controller', UnloadController)\n",
- " unload_resp = unload_srv(conflicting_controller)\n",
- " assert unload_resp.ok, \"Controller konnte nicht entladen werden.\"\n",
- "except rospy.ServiceException as e:\n",
- " rospy.logerr(\"Service call failed: %s\" % e)\n",
+ "# Check if the controller is already loaded\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "loaded_controllers = [controller.name for controller in list_controllers().controller]\n",
"\n",
- "# Service for loading the controller\n",
- "rospy.wait_for_service('/controller_manager/load_controller')\n",
- "load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)\n",
- "response = load_service(controller_name)\n",
- "assert response.ok, \"Controller konnte nicht geladen werden.\"\n",
+ "if controller_to_activate not in loaded_controllers:\n",
+ " # Service for loading the controller to be activated\n",
+ " rospy.wait_for_service('/controller_manager/load_controller')\n",
+ " load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)\n",
+ " response = load_service(controller_to_activate)\n",
+ " assert response.ok, \"Controller could not be loaded.\"\n",
"\n",
- "# Sevice for switching the controller\n",
+ "# Service for switching to the controller to be activated\n",
"rospy.wait_for_service('/controller_manager/switch_controller')\n",
"switch_service = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)\n",
"switch_req = SwitchControllerRequest()\n",
- "switch_req.start_controllers = [controller_name]\n",
+ "switch_req.start_controllers = [controller_to_activate]\n",
"switch_req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
"response = switch_service(switch_req)\n",
- "assert response.ok, \"Controller konnte nicht gestartet werden.\"\n",
+ "assert response.ok, \"Controller could not be started.\"\n",
"\n",
- "##### DO NOT CHANGE #####"
+ "##### YOUR CODE HERE #####"
]
},
{
@@ -120,14 +163,14 @@
"source": [
"##### DO NOT CHANGE #####\n",
"\n",
- "# Initialisiere ROS\n",
+ "# Initialize ROS\n",
"rospy.init_node('position_control', anonymous=True)\n",
"\n",
- "# Publisher für das Command-Topic\n",
+ "# Publisher for the command topic\n",
"pub = rospy.Publisher('/position_joint_trajectory_controller/command', JointTrajectory, queue_size=10)\n",
"\n",
- "# Warte, bis der Publisher bereit ist\n",
- "rospy.sleep(1)\n",
+ "# Wait until the publisher is ready\n",
+ "rospy.sleep(2)\n",
"\n",
"##### DO NOT CHANGE #####"
]
@@ -148,23 +191,20 @@
"source": [
"##### DO NOT CHANGE #####\n",
"\n",
- "# Erstelle die Nachricht\n",
+ "# create a JointTrajectory message\n",
"traj_msg = JointTrajectory()\n",
"traj_msg.joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']\n",
"\n",
- "# Definiere die Zielpositionen\n",
+ "# create a JointTrajectoryPoin\n",
"point = JointTrajectoryPoint()\n",
- "# Hier wird die Zielpositionen (in Radiant) für jedes Gelenk als eine Liste hinzugefügt\n",
+ "# Here the target positions (in radians) for each joint are added as a list\n",
"point.positions = [0.5, -0.5, 0.0, 0.0, 0.5, -0.5, 0.0]\n",
- "# Nun wird die Zeit festgelegt, bis die Bewegung abgeschlossen sein soll\n",
+ "# Now the time is set until the movement should be completed\n",
"point.time_from_start = rospy.Duration(2.0) \n",
"\n",
- "# Zuletzt wird der Punkt zur Trajektorie hinzugefügt\n",
+ "# Finally, the point is added to the trajectory\n",
"traj_msg.points.append(point)\n",
"\n",
- "# und die Nachricht wird gesendet\n",
- "pub.publish(traj_msg)\n",
- "\n",
"##### DO NOT CHANGE #####"
]
},
@@ -172,18 +212,12 @@
"cell_type": "markdown",
"metadata": {},
"source": [
- "### Task:\n",
+ "### Task 3 :\n",
"\n",
- "Now you have to define two more target positions yourself. We reach the first target position before the robot moves to the second target position.\n",
+ "Now you have to define more target position yourself.\n",
"\n",
"#### What you need to do:\n",
"\n",
- "\n",
- "##### Create the message:\n",
- "\n",
- "Start by creating a JointTrajectory message.\n",
- "Next, define the joints you want to move by entering the names in the joint_names list of the JointTrajectory message.\n",
- "\n",
"##### Add target positions:\n",
"\n",
"Create a JointTrajectoryPoint to define the target positions for each joint and complete the following fields:\n",
@@ -192,12 +226,17 @@
"\n",
"- time_from_start: The time it takes for the robot to reach the positions\n",
"\n",
- "Now add the point to the trajectory and do the same for the second point as for the first.\n",
+ "The joints should take the following values:\n",
"\n",
+ " - Joint1 : π / 2\n",
+ " - Joint2 : - π / 4\n",
+ " - Joint3 : 0.0\n",
+ " - Joint4 : - 3 * π / 4 \n",
+ " - Joint5 : 0.0\n",
+ " - Joint6 : π / 2\n",
+ " - Joint7 : π / 4\n",
"\n",
- "##### Send the message:\n",
- "\n",
- "Post the message to the topic /position_joint_trajectory_controller/command."
+ "Now add the point to the trajectory."
]
},
{
@@ -208,25 +247,12 @@
"source": [
"##### YOUR CODE HERE #####\n",
"\n",
- "# Nachricht erstellen\n",
- "traj_msg = JointTrajectory()\n",
- "traj_msg.joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']\n",
- "\n",
- "# Erste Zielposition definieren\n",
+ "# define the target position\n",
"point1 = JointTrajectoryPoint()\n",
- "point1.positions = [0.6, -0.6, 0.2, 0.2, 0.5, -0.5, 0.2]\n",
- "point1.time_from_start = rospy.Duration(2.0)\n",
+ "point1.positions = [math.pi / 2, -math.pi / 4, 0.0, -math.pi * 3 / 4, 0.0, math.pi / 2, math.pi / 4]\n",
+ "point1.time_from_start = rospy.Duration(4.0)\n",
"traj_msg.points.append(point1)\n",
"\n",
- "# Zweite Zielposition definieren\n",
- "point2 = JointTrajectoryPoint()\n",
- "point2.positions = [0.7, -0.2, -0.5, -0.2, -0.5, 0.7, -0.2]\n",
- "point2.time_from_start = rospy.Duration(5.0)\n",
- "traj_msg.points.append(point2)\n",
- "\n",
- "# Nachricht senden\n",
- "pub.publish(traj_msg)\n",
- "\n",
"##### YOUR CODE HERE #####"
]
},
@@ -234,8 +260,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
- "### Cleaning up and quitting the ROS node\n",
- "At the end, the ROS node is shut down and the resources are released."
+ "Now a message is sent via a publisher and it is ensured that the message is processed by waiting briefly."
]
},
{
@@ -246,34 +271,79 @@
"source": [
"##### DO NOT CHANGE #####\n",
"\n",
- "# Shutdown the ROS node\n",
- "rospy.signal_shutdown(\"Task completed\")\n",
- "roscpp_shutdown()\n",
+ "# send the message\n",
+ "pub.publish(traj_msg)\n",
+ "\n",
+ "# Ensure the message is processed\n",
+ "rospy.sleep(5)\n",
"\n",
"##### DO NOT CHANGE #####"
]
},
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Task 4:\n",
+ "\n",
+ "Now switch back to the previously active controllers.\n",
+ "\n",
+ "Tip: You have already saved these in a list before.\n",
+ "\n",
+ "### Procedure:\n",
+ "\n",
+ "- Use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.\n",
+ "- Then create a service proxy for this service.\n",
+ "- Create a new request (`SwitchControllerRequest`) and add the previously active controllers to the `start_controllers` list.\n",
+ "- Set the `strictness` to `BEST_EFFORT`.\n",
+ "- Check if the service was successful.\n"
+ ]
+ },
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
- "##### DO NOT CHANGE #####\n",
+ "##### YOUR CODE HERE #####\n",
"\n",
- "# Wechsel zurück zum effort_joint_trajectory_controller\n",
+ "# switch back to the previous controllers\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",
+ " # restart the previously active controllers\n",
+ " req.start_controllers = active_controllers\n",
" req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
" response = switch_controller(req)\n",
- " assert response.ok, \"Controller konnte nicht gewechselt werden.\"\n",
+ " assert response.ok, \"Previous controllers could not be restarted.\"\n",
"except rospy.ServiceException as e:\n",
" rospy.logerr(\"Service call failed: %s\" % e)\n",
"\n",
+ "##### YOUR CODE HERE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Cleaning up and quitting the ROS node\n",
+ "At the end, the ROS node is shut down and the resources are released."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Shutdown the ROS node\n",
+ "rospy.signal_shutdown(\"Task completed\")\n",
+ "roscpp_shutdown()\n",
+ "\n",
"##### DO NOT CHANGE #####"
]
}
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/switch_controller.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/switch_controller.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..fac989a475113e8ad1d30e11cd7a619031a35d82
--- /dev/null
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/controller/controller_task/switch_controller.ipynb
@@ -0,0 +1,276 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Educational Plattform Panda Robot\n",
+ "Author: uninh, uqwfa, ultck"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Controller_Task\n",
+ "### Switch_Controller\n",
+ "\n",
+ "In this task, you will learn...\n",
+ "\n",
+ "> - Understand how to stop a controller and load a new controller.\n",
+ "> - Learn how to activate a new controller.\n",
+ "> - Find out how to switch back to the previous controllers after completing the task"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "import rospy\n",
+ "from controller_manager_msgs.srv import LoadController, SwitchController, SwitchControllerRequest, ListControllers\n",
+ "from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint\n",
+ "from moveit_commander import roscpp_shutdown\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Creating a List of All Active Controllers\n",
+ "\n",
+ "In the next section, a list of all active controllers will be created.\n",
+ "\n",
+ "The method `wait_for_service` blocks the code until the service `/controller_manager/list_controllers` is available. This ensures that the service is ready before attempting to use it.\n",
+ "\n",
+ "After that, a service proxy is created. A service proxy is an object that allows making service calls as if it were a local function. In this case, the service `/controller_manager/list_controllers` is used.\n",
+ "\n",
+ "Finally, a list of names of the active controllers is created."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# create a list of active controllers\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "active_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Stopping Active Controllers\n",
+ "\n",
+ "If there are active controllers, they should be stopped in the following section.\n",
+ "\n",
+ "As before, we wait until the service is available. Then, in the try block, a proxy for the ROS service `switch_controller` is created and a new request (`SwitchControllerRequest`) is made. The list of controllers to be stopped in the request is set to the list of all active controllers. Setting the strictness to `BEST_EFFORT` means that the service will do its best to stop the controllers, but it is not guaranteed that all will be stopped. Finally, the response is saved and checked to see if all services were successfully stopped."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "if active_controllers:\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 = active_controllers\n",
+ " req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+ " response = switch_controller(req)\n",
+ " assert response.ok, \"Active controllers could not be stopped.\"\n",
+ " except rospy.ServiceException as e:\n",
+ " rospy.logerr(\"Service call failed: %s\" % e)\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Loading a New Controller\n",
+ "\n",
+ "Now we load the new controller.\n",
+ "\n",
+ "First, the name of the controller to be activated is defined. Then it is checked whether the controller is already loaded. If not, the service `/controller_manager/load_controller` is used to load the controller. If the controller is successfully loaded, this is confirmed with a confirmation message."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Define the controller to be activated\n",
+ "controller_to_activate = 'position_joint_trajectory_controller'\n",
+ "\n",
+ "# Check if the controller is already loaded\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "loaded_controllers = [controller.name for controller in list_controllers().controller]\n",
+ "if controller_to_activate not in loaded_controllers:\n",
+ " # Service for loading the controller to be activated\n",
+ " rospy.wait_for_service('/controller_manager/load_controller')\n",
+ " load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)\n",
+ " response = load_service(controller_to_activate)\n",
+ " assert response.ok, \"Controller could not be loaded.\"\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Activating the New Controller\n",
+ "\n",
+ "After the new controller has been loaded, it will now be activated.\n",
+ "\n",
+ "First, wait until the service `/controller_manager/switch_controller` is available. Then, a service proxy for the service is created. A new request (`SwitchControllerRequest`) is made and the name of the controller to be started is added to the request. The strictness is set to `BEST_EFFORT`, which means that the service will do its best to start the controller. Finally, the response is saved and checked to see if the controller was successfully started."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Service for switching to the controller to be activated\n",
+ "rospy.wait_for_service('/controller_manager/switch_controller')\n",
+ "switch_service = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)\n",
+ "switch_req = SwitchControllerRequest()\n",
+ "switch_req.start_controllers = [controller_to_activate]\n",
+ "switch_req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+ "response = switch_service(switch_req)\n",
+ "assert response.ok, \"Controller could not be started.\"\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Task\n",
+ "\n",
+ "Now create a list of all currently active controllers as done in a previous code cell.\n",
+ "\n",
+ "**Important**\n",
+ "Assign different identifiers to the individual elements to avoid confusion !!!\n",
+ "\n",
+ "- Use the method `wait_for_service` to wait for the service `/controller_manager/list_controllers`\n",
+ "- Then create a service proxy for this service and retrieve the list of active controllers\n",
+ "- Filter the controllers that have the state 'running' and save their names in a list\n",
+ "- Finally, print the list to the terminal"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### YOUR CODE HERE #####\n",
+ "\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers_1 = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "active_controllers_1 = [controller.name for controller in list_controllers_1().controller if controller.state == 'running']\n",
+ "print(f\"Active controllers: {active_controllers_1}\")\n",
+ "\n",
+ "##### YOUR CODE HERE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Finally, we switch back to the previous controllers, which you should now implement yourself.\n",
+ "\n",
+ "- Use the method `wait_for_service` to wait for the service `/controller_manager/switch_controller`.\n",
+ "- Then create a service proxy for this service.\n",
+ "- Create a new request (`SwitchControllerRequest`) and add the name of the controller to be stopped (`'position_joint_trajectory_controller'`) to the `stop_controllers` list.\n",
+ "- Add the list of previous active controllers (`active_controllers`) to the `start_controllers` list.\n",
+ "- Set the `strictness` to `BEST_EFFORT`.\n",
+ "- Check if the service was successful."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### YOUR CODE HERE #####\n",
+ "\n",
+ "# Switch back to previous controllers\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",
+ " # the controllers that were active before\n",
+ " req.start_controllers = active_controllers\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",
+ "##### YOUR CODE HERE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Cleaning up and quitting the ROS node\n",
+ "At the end, the ROS node is shut down and the resources are released."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Shutdown the ROS node\n",
+ "rospy.signal_shutdown(\"Task completed\")\n",
+ "roscpp_shutdown()\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ }
+ ],
+ "metadata": {
+ "language_info": {
+ "name": "python"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb
index 4bbe597fa2c0c4df882240e706ce1865d81398c3..d3abb5865e325a9935bd2eedc779b1b14bd8274e 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb
+++ b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb
@@ -283,6 +283,11 @@
"source": [
"##### DO NOT CHANGE #####\n",
"\n",
+ "'''\n",
+ "In the following section, the waypoints list\n",
+ "is written to the file waypoints.py to be evaluated later.\n",
+ "'''\n",
+ "\n",
"import os\n",
"import importlib.util\n",
"\n",
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 901fb038802df1bfc254cabb8e184527428a77e3..568ea9598ba7daaeffdc83f93811cea8645532f5 100644
--- a/catkin_ws/src/learn_environment/task_pool/task_definitions.json
+++ b/catkin_ws/src/learn_environment/task_pool/task_definitions.json
@@ -128,13 +128,19 @@
"title": "Controller Basics",
"folder": "/controller/controller_task",
"topic": "Controller",
- "difficulty": "beginner",
+ "difficulty": "intermediate",
"subtasks": [
{
"title": "Task 1",
- "description": "Undersatnd the basics of the position controller",
+ "description": "Understand how to switch between controllers",
+ "solution_file": "/switch_controller.ipynb",
+ "evaluation_file": "/switch_controller_eval.py"
+ },
+ {
+ "title": "Task 2",
+ "description": "Understand the basics of the position controller",
"solution_file": "/position_controller.ipynb",
- "evaluation_file": "/position_controller.py"
+ "evaluation_file": "/position_controller_eval.py"
}
]
}
diff --git a/catkin_ws/src/learn_environment/tasks/controller/controller_task/switch_controller.ipynb b/catkin_ws/src/learn_environment/tasks/controller/controller_task/switch_controller.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..802db37712b8064f7618b6c620dbc1d4f2b6854f
--- /dev/null
+++ b/catkin_ws/src/learn_environment/tasks/controller/controller_task/switch_controller.ipynb
@@ -0,0 +1,340 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Educational Plattform Panda Robot\n",
+ "Author: uninh, uqwfa, ultck"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Controller_Task\n",
+ "### Switch_Controller\n",
+ "\n",
+ "In this task, you will learn...\n",
+ "\n",
+ "> - Understand how to stop a controller and load a new controller.\n",
+ "> - Learn how to activate a new controller.\n",
+ "> - Find out how to switch back to the previous controllers after completing the task"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_1"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "import rospy\n",
+ "from controller_manager_msgs.srv import LoadController, SwitchController, SwitchControllerRequest, ListControllers\n",
+ "from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint\n",
+ "from moveit_commander import roscpp_shutdown\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Liste aller aktiven Controller erstellen\n",
+ "\n",
+ "Im nächsten Abschnitt wird nun eine Liste aller aktiven Controller erstellt.\n",
+ "\n",
+ "Die Methode wait_for_service blockiert den Code, bis der Service /controller_manager/list_controllers verfügbar ist. Dies stellt sicher, dass der Service bereit ist, bevor versucht wird, ihn zu verwenden.\n",
+ "\n",
+ "Danach wird ein Service-Proxy erstellt. Ein Service-Proxy ist ein Objekt, das es ermöglicht, Service-Aufrufe zu machen, als ob es eine lokale Funktion wäre. In diesem Fall wird der Service /controller_manager/list_controllers verwendet.\n",
+ "\n",
+ "Und zuletzt wird eine Liste von Namen der aktiven Controller erstellt."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_2"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# create a list of active controllers\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "active_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Aktive Controller stoppen\n",
+ "\n",
+ "Wenn nun aktive Controller vorhanden sind, dann sollen diese im folgenden Abschnitt nun gestoppt werden.\n",
+ "\n",
+ "Wieder wird, wie zuvor auch, gewartet, bis der Service verfügbar ist. Danach wird im try Block ein Proxy für den ROS Service switch_controller erstellt und eine neue Anfrage (SwitchControllerRequest) erstellt. Dann wird die Liste der zu stoppenden Controllern in der Anfrage, auf die Liste aller aktiven Controller gesetzt. Dass die strictness auf BEST_EFFORT gesetzt wird bedeutet, dass der Service sein Bestes tun wird, um die Controller zu stoppen, aber es ist nicht garantiert, dass alle gestoppt werden. Zuletzt wird die Antwort gespeichert und geprüft, ob alle Service erfolgreich gestoppt wurden.\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_3"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "if active_controllers:\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 = active_controllers\n",
+ " req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+ " response = switch_controller(req)\n",
+ " assert response.ok, \"Active controllers could not be stopped.\"\n",
+ " except rospy.ServiceException as e:\n",
+ " rospy.logerr(\"Service call failed: %s\" % e)\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Laden eines neuen Controllers\n",
+ "\n",
+ "Nun laden wir den neuen Controller.\n",
+ "\n",
+ "Zuerst wird der Name des zu aktivierenden Controllers definiert. Danach wird überprüft, ob der Controller bereits geladen ist. Falls nicht, wird der Service /controller_manager/load_controller verwendet, um den Controller zu laden. Wenn der Controller erfolgreich geladen wurde, wird dies durch eine Bestätigungsmeldung angezeigt."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_4"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Define the controller to be activated\n",
+ "controller_to_activate = 'position_joint_trajectory_controller'\n",
+ "\n",
+ "# Check if the controller is already loaded\n",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "loaded_controllers = [controller.name for controller in list_controllers().controller]\n",
+ "if controller_to_activate not in loaded_controllers:\n",
+ " # Service for loading the controller to be activated\n",
+ " rospy.wait_for_service('/controller_manager/load_controller')\n",
+ " load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)\n",
+ " response = load_service(controller_to_activate)\n",
+ " assert response.ok, \"Controller could not be loaded.\"\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Aktivieren des neuen Controllers\n",
+ "\n",
+ "Nachdem der neue Controller geladen wurde, wird er nun aktiviert.\n",
+ "\n",
+ "Zuerst wird gewartet, bis der Service /controller_manager/switch_controller verfügbar ist. Danach wird ein Service-Proxy für den Service erstellt. Eine neue Anfrage (SwitchControllerRequest) wird erstellt und der Name des zu startenden Controllers wird in die Anfrage eingefügt. Die strictness wird auf BEST_EFFORT gesetzt, was bedeutet, dass der Service sein Bestes tun wird, um den Controller zu starten. Zuletzt wird die Antwort gespeichert und geprüft, ob der Controller erfolgreich gestartet wurde."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_5"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Service for switching to the controller to be activated\n",
+ "rospy.wait_for_service('/controller_manager/switch_controller')\n",
+ "switch_service = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)\n",
+ "switch_req = SwitchControllerRequest()\n",
+ "switch_req.start_controllers = [controller_to_activate]\n",
+ "switch_req.strictness = SwitchControllerRequest.BEST_EFFORT\n",
+ "response = switch_service(switch_req)\n",
+ "assert response.ok, \"Controller could not be started.\"\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "#### Aufgabe\n",
+ "\n",
+ "Erstelle nun wie in einer Code Zelle zuvor schon einmal getan, eine liste aller aktuell aktiven controller.\n",
+ "\n",
+ "**Wichtig**\n",
+ "Vergebe für die einzelen Elemente andere Bezeichner, um Verwechslungsgefahr zu vermeiden.\n",
+ "\n",
+ "- Verwenden Sie dazu die Methode `wait_for_service`, um auf den Service `/controller_manager/list_controllers` zu warten\n",
+ "- Erstellen Sie dann einen Service-Proxy für diesen Service und rufen Sie die Liste der aktiven Controller ab\n",
+ "- Filtern Sie die Controller, die den Zustand 'running' haben, und speichern Sie deren Namen in einer Liste\n",
+ "- Zuletzt soll nun die Liste mithilfe der Methode `loginfo`ausgegeben werden"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_6",
+ "solution_removed_cell"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### YOUR CODE HERE #####\n",
+ "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",
+ "rospy.wait_for_service('/controller_manager/list_controllers')\n",
+ "list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)\n",
+ "active_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']\n",
+ "rospy.loginfo(\"Active controllers: %s\", active_controllers)\n",
+ "\n",
+ "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Zum Schluss wechseln wir zurück zu den vorherigen Controllern, was Sie nun auch selbst implementieren sollen.\n",
+ "\n",
+ "- Verwenden Sie die Methode `wait_for_service`, um auf den Service `/controller_manager/switch_controller` zu warten.\n",
+ "- Erstellen Sie dann einen Service-Proxy für diesen Service.\n",
+ "- Erstellen Sie eine neue Anfrage (`SwitchControllerRequest`) und fügen Sie den Namen des zu stoppenden Controllers (`'position_joint_trajectory_controller'`) in die `stop_controllers`-Liste ein.\n",
+ "- Fügen Sie die Liste der vorherigen aktiven Controller (`active_controllers`) in die `start_controllers`-Liste ein.\n",
+ "- Setzen Sie die `strictness` auf `BEST_EFFORT`.\n",
+ "- Überprüfen Sie, ob der Service erfolgreich war."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_7",
+ "solution_removed_cell"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### YOUR CODE HERE #####\n",
+ "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",
+ "# Switch back to previous controllers\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",
+ " # the controllers that were active before\n",
+ " req.start_controllers = active_controllers\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",
+ "## ↑↑↑↑ SOLUTION CODE HERE ↑↑↑↑ ##"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Cleaning up and quitting the ROS node\n",
+ "At the end, the ROS node is shut down and the resources are released."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": [
+ "task_cell_8"
+ ]
+ },
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "# Shutdown the ROS node\n",
+ "rospy.signal_shutdown(\"Task completed\")\n",
+ "roscpp_shutdown()\n",
+ "\n",
+ "##### DO NOT CHANGE #####"
+ ]
+ }
+ ],
+ "metadata": {
+ "language_info": {
+ "name": "python"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/tasks/controller/controller_task/trajectory_controller.ipynb b/catkin_ws/src/learn_environment/tasks/controller/controller_task/trajectory_controller.ipynb
deleted file mode 100644
index 608d157f46d8d3af36bd1b7d9b1d2e14383b2254..0000000000000000000000000000000000000000
--- a/catkin_ws/src/learn_environment/tasks/controller/controller_task/trajectory_controller.ipynb
+++ /dev/null
@@ -1,73 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "tags": [
- "task_cell_1"
- ],
- "vscode": {
- "languageId": "plaintext"
- }
- },
- "outputs": [],
- "source": [
- "#!/usr/bin/env python\n",
- "\n",
- "import rospy\n",
- "from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal\n",
- "from trajectory_msgs.msg import JointTrajectoryPoint\n",
- "import actionlib\n",
- "\n",
- "# ROS-Knoten initialisieren\n",
- "rospy.init_node(\"position_controller_example\", anonymous=True)\n",
- "\n",
- "# Verbindung mit dem Trajectory Controller herstellen\n",
- "# Der Code verwendet den ROS-Action-Server /panda_arm_controller/follow_joint_trajectory, um Gelenkpositionen anzusteuern.\n",
- "# Diese Schnittstelle basiert auf dem Trajectory Controller, der für die präzise Steuerung von Gelenkbewegungen zuständig ist.\n",
- "\n",
- "client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)\n",
- "rospy.loginfo(\"Waiting for trajectory controller...\")\n",
- "client.wait_for_server()\n",
- "rospy.loginfo(\"Connected to trajectory controller!\")\n",
- "\n",
- "# Zielgelenkpositionen definieren\n",
- "joint_goal = [0.0, -0.785, 0.0, -2.356, 0.0, 1.57, 0.785]\n",
- "\n",
- "# Trajektorie erstellen\n",
- "goal = FollowJointTrajectoryGoal()\n",
- "goal.trajectory.joint_names = [\n",
- " \"panda_joint1\", \"panda_joint2\", \"panda_joint3\",\n",
- " \"panda_joint4\", \"panda_joint5\", \"panda_joint6\", \"panda_joint7\"\n",
- "]\n",
- "\n",
- "point = JointTrajectoryPoint()\n",
- "point.positions = joint_goal\n",
- "point.time_from_start = rospy.Duration(5.0) # Die Bewegung soll in 5 Sekunden abgeschlossen sein\n",
- "\n",
- "goal.trajectory.points.append(point)\n",
- "goal.trajectory.header.stamp = rospy.Time.now()\n",
- "\n",
- "# Ziel senden\n",
- "rospy.loginfo(\"Sending goal to trajectory controller...\")\n",
- "client.send_goal(goal)\n",
- "client.wait_for_result()\n",
- "\n",
- "# Ergebnis überprüfen\n",
- "result = client.get_result()\n",
- "if result:\n",
- " rospy.loginfo(\"Trajectory successfully executed!\")\n",
- "else:\n",
- " rospy.logwarn(\"Failed to execute trajectory.\")"
- ]
- }
- ],
- "metadata": {
- "language_info": {
- "name": "python"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 2
-}
\ No newline at end of file