From 253a6605ccc7b0e9213c522e89cbcb547134e1b6 Mon Sep 17 00:00:00 2001
From: uninh <uninh@student.kit.edu>
Date: Mon, 13 Jan 2025 01:01:39 +0000
Subject: [PATCH] added eval for the intermediate and position controller
---
.../controller_task/position_controller.py | 42 ++++++++++++
.../move_it_basics/move_it_basics_3_eval.py | 16 +++--
.../move_it_intermediate_eval_1.py | 56 +++++++++++++++-
.../controller_task/position_controller.ipynb | 24 +++++++
.../move_it_basics/move_it_basics_3.ipynb | 67 +++++++++++++------
.../move_it_intermediate_1.ipynb | 20 +++---
.../move_it_basics/move_it_basics_3.ipynb | 41 ++++++++++++
.../move_it_basics/move_it_basics_4.ipynb | 43 ++++++++++++
8 files changed, 272 insertions(+), 37 deletions(-)
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
index e69de29..3c59575 100644
--- 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
@@ -0,0 +1,42 @@
+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/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 6811699..09168be 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
@@ -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)
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 8982201..d3fd332 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
@@ -1 +1,55 @@
-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
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 ac46280..05fa981 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
@@ -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": {
diff --git a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_basics/move_it_basics_3.ipynb b/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_basics/move_it_basics_3.ipynb
index f09cc26..a6c95cb 100644
--- a/catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_basics/move_it_basics_3.ipynb
+++ b/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 #####"
]
},
{
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 369765d..9a57789 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
@@ -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 #####"
]
diff --git a/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_3.ipynb b/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_3.ipynb
index 1ea1023..5b92ff4 100644
--- a/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_3.ipynb
+++ b/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": {},
diff --git a/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_4.ipynb b/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_4.ipynb
index bbbbe6d..e1ce1bb 100644
--- a/catkin_ws/src/learn_environment/tasks/move_it/move_it_basics/move_it_basics_4.ipynb
+++ b/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": {},
--
GitLab