small changes in postition controller describtion

5e539195 · Florian Beyer · 94d22d55 · 5e539195
Commit 5e539195 authored 2 months ago by Florian Beyer
--- 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
@@ -53,6 +53,9 @@
    "\n",
    "**Important**\n",
    "Use the `wait_for_service` method to wait for the `/controller_manager/list_controllers` service.\n",
+    "\n",
+    "#### Procedure:\n",
+    "\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",
@@ -97,6 +100,8 @@
    "\n",
    "Activate the position controller.\n",
    "\n",
+    "#### Procedure:\n",
+    "\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",
@@ -212,11 +217,11 @@
   "cell_type": "markdown",
   "metadata": {},
   "source": [
-    "### Task 3 :\n",
+    "### Task 3:\n",
    "\n",
    "Now you have to define more target position yourself.\n",
    "\n",
-    "#### What you need to do:\n",
+    "#### Procedure:\n",
    "\n",
    "##### Add target positions:\n",
    "\n",
@@ -290,7 +295,7 @@
    "\n",
    "Tip: You have already saved these in a list before.\n",
    "\n",
-    "### Procedure:\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",

 %% Cell type:markdown id: tags:

 # Educational Plattform Panda Robot
 Author: uninh, uqwfa, ultck

 %% Cell type:markdown id: tags:

 ## Controller_Task
 ### Position_Controller

 In this task, you will learn:
 >
 > - how to work with a position controller to move the joints of a robot to specific positions.
 > - how to stop all active controllers to ensure no conflicts occur.
 > - how to start the position controller if it is not already running.
 > - how to create a `JointTrajectory` message for the position controller.
 > - how to define target positions for each joint and set the time to reach these positions.
 > - how to send the target position to the controller and verify the movement.
 > - how to switch back to the previously active controllers after completing the task.

 %% Cell type:code id: tags:

 ``` 
 ##### DO NOT CHANGE #####

 import rospy
 from controller_manager_msgs.srv import LoadController, SwitchController, SwitchControllerRequest, UnloadController, ListControllers
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from moveit_commander import roscpp_shutdown
 import math

 ##### DO NOT CHANGE #####
 ```

 %% Cell type:markdown id: tags:

 ### Task 1:

 Stop all active controllers.

 **Important**
 Use the `wait_for_service` method to wait for the `/controller_manager/list_controllers` service.
+
+#### Procedure:
+
 - Then create a service proxy for this service and retrieve the list of active controllers.
 - Filter the controllers that have the state 'running' and save their names in a list.
 - Use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.
 - Then create a service proxy for this service.
 - Create a new request (`SwitchControllerRequest`) and add the list of active controllers to the `stop_controllers` list.
 - Set the `strictness` to `BEST_EFFORT`.
 - Check if the service was successful.

 %% Cell type:code id: tags:

 ``` 
 ##### YOUR CODE HERE #####

 rospy.wait_for_service('/controller_manager/list_controllers')
 list_controllers = rospy.ServiceProxy('/controller_manager/list_controllers', ListControllers)
 active_controllers = [controller.name for controller in list_controllers().controller if controller.state == 'running']

 if active_controllers:
    rospy.wait_for_service('/controller_manager/switch_controller')
    try:
        switch_controller = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)
        req = SwitchControllerRequest()
        req.stop_controllers = active_controllers
        req.strictness = SwitchControllerRequest.BEST_EFFORT
        response = switch_controller(req)
        assert response.ok, "Active controllers could not be stopped."
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s" % e)

 ##### YOUR CODE HERE #####
 ```

 %% Cell type:markdown id: tags:

 ### Task 2:

 Activate the position controller.

+#### Procedure:
+
 - First, use the `wait_for_service` method to wait for the `/controller_manager/list_controllers` service.
 - Then create a service proxy for this service and retrieve the list of loaded controllers.

 - Check if the `position_joint_trajectory_controller` is already loaded.
 - If not, use the `wait_for_service` method to wait for the `/controller_manager/load_controller` service.
 - Then create a service proxy for this service and load the controller.

 - Now use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.
 - Then create a service proxy for this service.
 - Create a new request (`SwitchControllerRequest`) and add the `position_joint_trajectory_controller` to the `start_controllers` list.
 - Set the `strictness` to `BEST_EFFORT`.
 - Check if the service was successful.

 %% Cell type:code id: tags:

 ``` 
 ##### YOUR CODE HERE #####

 # Define the controllers
 controller_to_activate = 'position_joint_trajectory_controller'

 # Check if the controller is already loaded
 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]

 if controller_to_activate not in loaded_controllers:
    # Service for loading the controller to be activated
    rospy.wait_for_service('/controller_manager/load_controller')
    load_service = rospy.ServiceProxy('/controller_manager/load_controller', LoadController)
    response = load_service(controller_to_activate)
    assert response.ok, "Controller could not be loaded."

 # Service for switching to the controller to be activated
 rospy.wait_for_service('/controller_manager/switch_controller')
 switch_service = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)
 switch_req = SwitchControllerRequest()
 switch_req.start_controllers = [controller_to_activate]
 switch_req.strictness = SwitchControllerRequest.BEST_EFFORT
 response = switch_service(switch_req)
 assert response.ok, "Controller could not be started."

 ##### YOUR CODE HERE #####
 ```

 %% Cell type:markdown id: tags:

 Now the ROS node is initialized, a publisher is created for the command topic and waited until the publisher is created.

 The publisher sends messages of the type JointTrajectory to the topic /position_joint_trajectory_controller/command. The parameter queue_size=10 sets the size of the message queue. This means that up to 10 messages are held in the queue before old messages are discarded.

 %% Cell type:code id: tags:

 ``` 
 ##### DO NOT CHANGE #####

 # Initialize ROS
 rospy.init_node('position_control', anonymous=True)

 # Publisher for the command topic
 pub = rospy.Publisher('/position_joint_trajectory_controller/command', JointTrajectory, queue_size=10)

 # Wait until the publisher is ready
 rospy.sleep(2)

 ##### DO NOT CHANGE #####
 ```

 %% Cell type:markdown id: tags:

 The position controller receives messages of the type trajectory_msgs/JointTrajectory.
 This message contains joint_names, a list of the joints to be controlled, and points, a list of positions to control the joints.

 %% Cell type:code id: tags:

 ``` 
 ##### DO NOT CHANGE #####

 # create a JointTrajectory message
 traj_msg = JointTrajectory()
 traj_msg.joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']

 # create a JointTrajectoryPoin
 point = JointTrajectoryPoint()
 # Here the target positions (in radians) for each joint are added as a list
 point.positions = [0.5, -0.5, 0.0, 0.0, 0.5, -0.5, 0.0]
 # Now the time is set until the movement should be completed
 point.time_from_start = rospy.Duration(2.0)

 # Finally, the point is added to the trajectory
 traj_msg.points.append(point)

 ##### DO NOT CHANGE #####
 ```

 %% Cell type:markdown id: tags:

-### Task 3 :
+### Task 3:

 Now you have to define more target position yourself.

-#### What you need to do:
+#### Procedure:

 ##### Add target positions:

 Create a JointTrajectoryPoint to define the target positions for each joint and complete the following fields:

 - positions: The target positions in Radiant (e.g. [0.5, -0.5, 0.0])

 - time_from_start: The time it takes for the robot to reach the positions

 The joints should take the following values:

 - Joint1 : π / 2
 - Joint2 : - π / 4
 - Joint3 : 0.0
 - Joint4 : - 3 * π / 4
 - Joint5 : 0.0
 - Joint6 : π / 2
 - Joint7 : π / 4

 Now add the point to the trajectory.

 %% Cell type:code id: tags:

 ``` 
 ##### YOUR CODE HERE #####

 # define the target position
 point1 = JointTrajectoryPoint()
 point1.positions = [math.pi / 2, -math.pi / 4, 0.0, -math.pi * 3 / 4, 0.0, math.pi / 2, math.pi / 4]
 point1.time_from_start = rospy.Duration(4.0)
 traj_msg.points.append(point1)

 ##### YOUR CODE HERE #####
 ```

 %% Cell type:markdown id: tags:

 Now a message is sent via a publisher and it is ensured that the message is processed by waiting briefly.

 %% Cell type:code id: tags:

 ``` 
 ##### DO NOT CHANGE #####

 # send the message
 pub.publish(traj_msg)

 # Ensure the message is processed
 rospy.sleep(5)

 ##### DO NOT CHANGE #####
 ```

 %% Cell type:markdown id: tags:

 ### Task 4:

 Now switch back to the previously active controllers.

 Tip: You have already saved these in a list before.

-### Procedure:
+#### Procedure:

 - Use the `wait_for_service` method to wait for the `/controller_manager/switch_controller` service.
 - Then create a service proxy for this service.
 - Create a new request (`SwitchControllerRequest`) and add the previously active controllers to the `start_controllers` list.
 - Set the `strictness` to `BEST_EFFORT`.
 - Check if the service was successful.

 %% Cell type:code id: tags:

 ``` 
 ##### YOUR CODE HERE #####

 # switch back to the previous controllers
 rospy.wait_for_service('/controller_manager/switch_controller')
 try:
    switch_controller = rospy.ServiceProxy('/controller_manager/switch_controller', SwitchController)
    req = SwitchControllerRequest()
    req.stop_controllers = ['position_joint_trajectory_controller']
    # restart the previously active controllers
    req.start_controllers = active_controllers
    req.strictness = SwitchControllerRequest.BEST_EFFORT
    response = switch_controller(req)
    assert response.ok, "Previous controllers could not be restarted."
 except rospy.ServiceException as e:
    rospy.logerr("Service call failed: %s" % e)

 ##### YOUR CODE HERE #####
 ```

 %% Cell type:markdown id: tags:

 ### Cleaning up and quitting the ROS node
 At the end, the ROS node is shut down and the resources are released.

 %% Cell type:code id: tags:

 ``` 
 ##### DO NOT CHANGE #####

 # Shutdown the ROS node
 rospy.signal_shutdown("Task completed")
 roscpp_shutdown()

 ##### DO NOT CHANGE #####
 ```