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
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
......
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:
# Educational Plattform Franka Emika Panda robot arm
Author: uninh, uqwfa, ultck
%% Cell type:markdown id: tags:
## MoveIt Intermediate
### Task 1
---
>
The goal of this notebook is :
- 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 multiple positions can be defined, converted into a trajectory and finally executed
>
---
%% 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.
Then, as usual, we initialize the MoveIt Commander, create a ROS node and initialize the Move Group.
%% Cell type:code id: tags:
```
##### DO NOT CHANGE #####
import rospy
import sys
from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import Pose
# Initialize the MoveIt Commander
roscpp_initialize(sys.argv)
# Initialize the ROS-Node
rospy.init_node('move_it_intermediate', anonymous=True)
# Initialize the MoveGroup
group = MoveGroupCommander('panda_arm')
##### DO NOT CHANGE #####
```
%% 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).
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:
```
##### YOUR CODE HERE #####
# Get the current pose of the end-effector
current_pose = group.get_current_pose().pose
##### YOUR CODE HERE #####
```
%% 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.
%% Cell type:code id: tags:
```
##### DO NOT CHANGE #####
# Define the waypoints the robot should move to
waypoints = []
##### DO NOT CHANGE #####
```
%% Cell type:markdown id: tags:
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.
Add this waypunt at the end of the waypoints list.
Tip: Do this with the current pose
%% Cell type:code id: tags:
```
##### YOUR CODE HERE #####
waypoint_1 = Pose()
waypoint_1.position.x = current_pose.position.x + 0.1
waypoint_1.position.y = current_pose.position.y
waypoint_1.position.z = current_pose.position.z
waypoint_1.orientation = current_pose.orientation
waypoints.append(waypoint_1)
##### YOUR CODE HERE #####
```
%% Cell type:markdown id: tags:
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).
Again, the orientation should be taken from the current_pose.
Finally, add the new waypoint to the waypoints list.
%% Cell type:code id: tags:
```
##### YOUR CODE HERE #####
waypoint_2 = Pose()
waypoint_2.position.x = waypoint_1.position.x
waypoint_2.position.y = waypoint_1.position.y + 0.2
waypoint_2.position.z = waypoint_1.position.z
waypoint_2.orientation = waypoint_1.orientation
waypoint_2.orientation = current_pose.orientation
waypoints.append(waypoint_2)
##### YOUR CODE HERE #####
```
%% Cell type:markdown id: tags:
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.
Take the orientation from the current_pose.
Add the new waypoint to the waypoints list.
%% Cell type:code id: tags:
```
##### YOUR CODE HERE #####
waypoint_3 = Pose()
waypoint_3.position.x = waypoint_2.position.x
waypoint_3.position.y = waypoint_2.position.y
waypoint_3.position.z = waypoint_2.position.z + 0.1
waypoint_3.orientation = current_pose.orientation
waypoints.append(waypoint_3)
##### YOUR CODE HERE #####
```
%% Cell type:markdown id: tags:
Now we define parameters that have the following meaning:
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,
attempts = 0, counts the number of attempts.
%% Cell type:code id: tags:
```
##### DO NOT CHANGE #####
# Compute the Cartesian path
fraction = 0.0
max_attempts = 100
attempts = 0
##### DO NOT CHANGE #####
```
%% 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.
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.
%% Cell type:code id: tags:
```
##### YOUR CODE HERE #####
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) # Plan the trajectory
while fraction < 1.0 and attempts < max_attempts:
(plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0.0) # Plan the trajectory
attempts += 1
##### YOUR CODE HERE #####
```
%% Cell type:markdown id: tags:
### Execute the planned path
Now we will execute the planned path.
%% Cell type:code id: tags:
```
##### DO NOT CHANGE #####
# Execute the planned path
group.execute(plan, wait=True)
if fraction == 1.0:
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 #####
```
%% 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 #####
```
......
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