diff --git a/catkin_ws/src/learn_environment/converter/waypoints.py b/catkin_ws/src/learn_environment/converter/waypoints.py
new file mode 100644
index 0000000000000000000000000000000000000000..4521577460269123bc2f247bc9380150a71b3b45
--- /dev/null
+++ b/catkin_ws/src/learn_environment/converter/waypoints.py
@@ -0,0 +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)),
+]
\ No newline at end of file
diff --git a/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py b/catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py
index d3fd332cbb415a1af0437a1dd3f47e8670df807c..5c16d1747df009838ec9d5fe5befd52aa4366d7a 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,25 +1,23 @@
import math
+from geometry_msgs.msg import Pose, Point, Quaternion
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')
+ if 'waypoints.py' in files:
+ waypoints_path = os.path.join(root, 'waypoints.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
+ waypoints_user = waypoints_module.waypoints
+ print(waypoints_user)
else:
- raise FileNotFoundError("The converted.py file was not found in the file system")
+ raise FileNotFoundError('waypoints.py not found')
-print("Waypoints:")
-print(waypoints)
sol = [0.4, 0.2, 0.7, -1.0, 0.4, 0.0, 0.0]
@@ -40,6 +38,21 @@ pose3 = Pose(
target_waypoints = [pose1, pose2, pose3]
+waypoints = [
+ Pose(
+ position=Point(0.4, 0.0, 0.6),
+ orientation=Quaternion(-1.0, 0.4, 0.0, 0.0)
+ ),
+ Pose(
+ position=Point(0.4, 0.2, 0.6),
+ orientation=Quaternion(-1.0, 0.4, 0.0, 0.0)
+ ),
+ Pose(
+ position=Point(0.4, 0.2, 0.7),
+ orientation=Quaternion(-1.0, 0.4, 0.0, 0.0)
+ )
+]
+
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
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 9a577899361ce32048179b418109d4df8b149a48..4bbe597fa2c0c4df882240e706ce1865d81398c3 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 = waypoint_1.orientation\n",
+ "waypoint_2.orientation = current_pose.orientation\n",
"waypoints.append(waypoint_2)\n",
"\n",
"##### YOUR CODE HERE #####"
@@ -242,8 +242,9 @@
"source": [
"##### YOUR CODE HERE #####\n",
"\n",
- "\n",
- "(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) # Plan the trajectory\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",
"##### YOUR CODE HERE #####"
]
@@ -264,12 +265,51 @@
"source": [
"##### DO NOT CHANGE #####\n",
"\n",
- "# Execute the planned path\n",
- "group.execute(plan, wait=True)\n",
+ "if fraction == 1.0:\n",
+ " rospy.loginfo(\"Successfully computed the Cartesian path. Executing...\")\n",
+ " group.execute(plan, wait=True)\n",
+ " rospy.loginfo(\"Execution completed.\")\n",
+ "else:\n",
+ " rospy.logwarn(f\"Failed to compute a complete path after {attempts} attempts.\")\n",
"\n",
"##### DO NOT CHANGE #####"
]
},
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "##### DO NOT CHANGE #####\n",
+ "\n",
+ "import os\n",
+ "import importlib.util\n",
+ "\n",
+ "waypoints_path = None\n",
+ "for root, dirs, files in os.walk('/'):\n",
+ " if 'waypoints.py' in files:\n",
+ " waypoints_path = os.path.join(root, 'waypoints.py')\n",
+ " break\n",
+ "\n",
+ "if waypoints_path:\n",
+ " spec = importlib.util.spec_from_file_location('waypoints', waypoints_path)\n",
+ " waypoints_module = importlib.util.module_from_spec(spec)\n",
+ " spec.loader.exec_module(waypoints_module)\n",
+ " waypoints_module.waypoints = waypoints\n",
+ " with open(waypoints_path, 'w') as f:\n",
+ " f.write('from geometry_msgs.msg import Pose, Point, Quaternion\\n\\n')\n",
+ " f.write('waypoints = [\\n')\n",
+ " for waypoint in waypoints:\n",
+ " f.write(f' Pose(position=Point(x={waypoint.position.x}, y={waypoint.position.y}, z={waypoint.position.z}), '\n",
+ " f'orientation=Quaternion(x={waypoint.orientation.x}, y={waypoint.orientation.y}, z={waypoint.orientation.z}, w={waypoint.orientation.w})),\\n')\n",
+ " f.write(']')\n",
+ "else:\n",
+ " raise FileNotFoundError('waypoints.py not found')\n",
+ "\n",
+ "##### DO NOT CHANGE #####\n"
+ ]
+ },
{
"cell_type": "markdown",
"metadata": {},