Skip to content
Snippets Groups Projects
Commit 867d3221 authored by Florian Beyer's avatar Florian Beyer
Browse files

finished eval of movit_intermediate

parent 253a6605
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
catkin_ws/src/learn_environment/converter/waypoints.py 0 → 100644
+ 7
− 0
View file @ 867d3221
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
catkin_ws/src/learn_environment/task_pool/evaluation_scripts/move_it/move_it_intermediate/move_it_intermediate_eval_1.py
+ 21
− 8
View file @ 867d3221
import math import math
from geometry_msgs.msg import Pose, Point, Quaternion
import os import os
import importlib.util import importlib.util
from geometry_msgs.msg import Pose, Point, Quaternion
# Dynamically import waypoints module
waypoints_path = None waypoints_path = None
for root, dirs, files in os.walk('/'): for root, dirs, files in os.walk('/'):
if 'converted.py' in files: if 'waypoints.py' in files:
waypoints_path = os.path.join(root, 'converted.py') waypoints_path = os.path.join(root, 'waypoints.py')
break break
if waypoints_path: if waypoints_path:
spec = importlib.util.spec_from_file_location('waypoints', waypoints_path) spec = importlib.util.spec_from_file_location('waypoints', waypoints_path)
waypoints_module = importlib.util.module_from_spec(spec) waypoints_module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(waypoints_module) spec.loader.exec_module(waypoints_module)
waypoints = waypoints_module.waypoints waypoints_user = waypoints_module.waypoints
print(waypoints_user)
else: 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] sol = [0.4, 0.2, 0.7, -1.0, 0.4, 0.0, 0.0]
...@@ -40,6 +38,21 @@ pose3 = Pose( ...@@ -40,6 +38,21 @@ pose3 = Pose(
target_waypoints = [pose1, pose2, pose3] 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): 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 return abs(p1.x - p2.x) <= tolerance and abs(p1.y - p2.y) <= tolerance and abs(p1.z - p2.z) <= tolerance
......
catkin_ws/src/learn_environment/task_pool/solution_scripts/move_it/move_it_intermediate/move_it_intermediate_1.ipynb
+ 45
− 5
View file @ 867d3221
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Educational Plattform Franka Emika Panda robot arm # Educational Plattform Franka Emika Panda robot arm
Author: uninh, uqwfa, ultck Author: uninh, uqwfa, ultck
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## MoveIt Intermediate ## MoveIt Intermediate
### Task 1 ### Task 1
--- ---
> >
The goal of this notebook is : The goal of this notebook is :
- to move the robotic arm through a series of predetermined positions (waypoints) in Cartesian space - to move the robotic arm through a series of predetermined positions (waypoints) in Cartesian space
- how to plan movements that run along a complex trajectory - how to plan movements that run along a complex trajectory
- how multiple positions can be defined, converted into a trajectory and finally executed - how multiple positions can be defined, converted into a trajectory and finally executed
> >
--- ---
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
First, all the necessary packages and classes are imported. You already know these from the move_it_basics and move_it_objects tasks. First, all the necessary packages and classes are imported. You already know these from the move_it_basics and move_it_objects tasks.
Then, as usual, we initialize the MoveIt Commander, create a ROS node and initialize the Move Group. Then, as usual, we initialize the MoveIt Commander, create a ROS node and initialize the Move Group.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
import rospy import rospy
import sys import sys
from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import Pose from geometry_msgs.msg import Pose
# Initialize the MoveIt Commander # Initialize the MoveIt Commander
roscpp_initialize(sys.argv) roscpp_initialize(sys.argv)
# Initialize the ROS-Node # Initialize the ROS-Node
rospy.init_node('move_it_intermediate', anonymous=True) rospy.init_node('move_it_intermediate', anonymous=True)
# Initialize the MoveGroup # Initialize the MoveGroup
group = MoveGroupCommander('panda_arm') group = MoveGroupCommander('panda_arm')
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
With group.get_current_pose() you query the current pose of the end effector in MoveIt. The method returns a PoseStamped-object, whose pose property contains the actual geometry (position and orientation). With group.get_current_pose() you query the current pose of the end effector in MoveIt. The method returns a PoseStamped-object, whose pose property contains the actual geometry (position and orientation).
So your task is to first create a variable current_pose and then store the current pose of the end effector in it. So your task is to first create a variable current_pose and then store the current pose of the end effector in it.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
# Get the current pose of the end-effector # Get the current pose of the end-effector
current_pose = group.get_current_pose().pose current_pose = group.get_current_pose().pose
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Here a list of waypoints is initialized, in which all waypoints that the robot arm should run must be added later. Here a list of waypoints is initialized, in which all waypoints that the robot arm should run must be added later.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
# Define the waypoints the robot should move to # Define the waypoints the robot should move to
waypoints = [] waypoints = []
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Now you want to define the first waypoint. So create a new object from the class pose. Now you want to define the first waypoint. So create a new object from the class pose.
For the first waypoint the robot arm should move 10 cm (0.1) along the X axis, whereby the y coordinate and z coordinate as well as the orientation should remain unchanged. For the first waypoint the robot arm should move 10 cm (0.1) along the X axis, whereby the y coordinate and z coordinate as well as the orientation should remain unchanged.
Add this waypunt at the end of the waypoints list. Add this waypunt at the end of the waypoints list.
Tip: Do this with the current pose Tip: Do this with the current pose
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
waypoint_1 = Pose() waypoint_1 = Pose()
waypoint_1.position.x = current_pose.position.x + 0.1 waypoint_1.position.x = current_pose.position.x + 0.1
waypoint_1.position.y = current_pose.position.y waypoint_1.position.y = current_pose.position.y
waypoint_1.position.z = current_pose.position.z waypoint_1.position.z = current_pose.position.z
waypoint_1.orientation = current_pose.orientation waypoint_1.orientation = current_pose.orientation
waypoints.append(waypoint_1) waypoints.append(waypoint_1)
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Next, create a second waypoint, also of the pose type. Next, create a second waypoint, also of the pose type.
Now hold the X and Z positions from the first waypoint and shift the Y position by 0.2 (20 cm). Now hold the X and Z positions from the first waypoint and shift the Y position by 0.2 (20 cm).
Again, the orientation should be taken from the current_pose. Again, the orientation should be taken from the current_pose.
Finally, add the new waypoint to the waypoints list. Finally, add the new waypoint to the waypoints list.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
waypoint_2 = Pose() waypoint_2 = Pose()
waypoint_2.position.x = waypoint_1.position.x waypoint_2.position.x = waypoint_1.position.x
waypoint_2.position.y = waypoint_1.position.y + 0.2 waypoint_2.position.y = waypoint_1.position.y + 0.2
waypoint_2.position.z = waypoint_1.position.z waypoint_2.position.z = waypoint_1.position.z
waypoint_2.orientation = waypoint_1.orientation waypoint_2.orientation = current_pose.orientation
waypoints.append(waypoint_2) waypoints.append(waypoint_2)
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The last waypoint should also be of the type Pose. The last waypoint should also be of the type Pose.
Hold the X and Y positions from the second waypoint and move the Z position 0.1 (10 cm) up. Hold the X and Y positions from the second waypoint and move the Z position 0.1 (10 cm) up.
Take the orientation from the current_pose. Take the orientation from the current_pose.
Add the new waypoint to the waypoints list. Add the new waypoint to the waypoints list.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
waypoint_3 = Pose() waypoint_3 = Pose()
waypoint_3.position.x = waypoint_2.position.x waypoint_3.position.x = waypoint_2.position.x
waypoint_3.position.y = waypoint_2.position.y waypoint_3.position.y = waypoint_2.position.y
waypoint_3.position.z = waypoint_2.position.z + 0.1 waypoint_3.position.z = waypoint_2.position.z + 0.1
waypoint_3.orientation = current_pose.orientation waypoint_3.orientation = current_pose.orientation
waypoints.append(waypoint_3) waypoints.append(waypoint_3)
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Now we define parameters that have the following meaning: Now we define parameters that have the following meaning:
fraction = 0.0, sets the initial value of the calculated path portion to 0, fraction = 0.0, sets the initial value of the calculated path portion to 0,
max_attempts = 100, sets the maximum number of attempts to calculate the path, max_attempts = 100, sets the maximum number of attempts to calculate the path,
attempts = 0, counts the number of attempts. attempts = 0, counts the number of attempts.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
# Compute the Cartesian path # Compute the Cartesian path
fraction = 0.0 fraction = 0.0
max_attempts = 100 max_attempts = 100
attempts = 0 attempts = 0
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Create a loop that runs as long as the calculated fraction is less than 1.0 and the number of attempts is less than max_attempts. Create a loop that runs as long as the calculated fraction is less than 1.0 and the number of attempts is less than max_attempts.
Within the loop, calculate the Cartesian path based on the specified waypoints. Use 0.01 as the step distance of the end effector and 0.0 as the jump threshold. The method returns a plan of motion and the calculated path fraction. Within the loop, calculate the Cartesian path based on the specified waypoints. Use 0.01 as the step distance of the end effector and 0.0 as the jump threshold. The method returns a plan of motion and the calculated path fraction.
Don't forget to increase the number of attempts by 1 at the end of the loop. Don't forget to increase the number of attempts by 1 at the end of the loop.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) # Plan the trajectory (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) # Plan the trajectory
attempts += 1
##### YOUR CODE HERE ##### ##### YOUR CODE HERE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Execute the planned path ### Execute the planned path
Now we will execute the planned path. Now we will execute the planned path.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
# Execute the planned path if fraction == 1.0:
group.execute(plan, wait=True) rospy.loginfo("Successfully computed the Cartesian path. Executing...")
group.execute(plan, wait=True)
rospy.loginfo("Execution completed.")
else:
rospy.logwarn(f"Failed to compute a complete path after {attempts} attempts.")
##### DO NOT CHANGE #####
```
%% Cell type:code id: tags:
```
##### DO NOT CHANGE #####
import os
import importlib.util
waypoints_path = None
for root, dirs, files in os.walk('/'):
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_module.waypoints = waypoints
with open(waypoints_path, 'w') as f:
f.write('from geometry_msgs.msg import Pose, Point, Quaternion\n\n')
f.write('waypoints = [\n')
for waypoint in waypoints:
f.write(f' Pose(position=Point(x={waypoint.position.x}, y={waypoint.position.y}, z={waypoint.position.z}), '
f'orientation=Quaternion(x={waypoint.orientation.x}, y={waypoint.orientation.y}, z={waypoint.orientation.z}, w={waypoint.orientation.w})),\n')
f.write(']')
else:
raise FileNotFoundError('waypoints.py not found')
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### Cleaning up and quitting the ROS node ### Cleaning up and quitting the ROS node
At the end, the ROS node is shut down and the resources are released. At the end, the ROS node is shut down and the resources are released.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` ```
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
# Shutdown the ROS node # Shutdown the ROS node
rospy.signal_shutdown("Task completed") rospy.signal_shutdown("Task completed")
roscpp_shutdown() roscpp_shutdown()
##### DO NOT CHANGE ##### ##### DO NOT CHANGE #####
``` ```
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Help | Imprint | Privacy policy | Accessibility | Contact