-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy path03_plan_paths_for_assembly.py
172 lines (134 loc) · 6.23 KB
/
03_plan_paths_for_assembly.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
import os
import math
import time
import json
from compas.geometry import Vector
from compas.geometry import Frame
from compas.geometry import Transformation
from compas_fab.backends import RosClient
from compas_fab.robots import PlanningScene
from compas_fab.robots import Configuration
from compas_fab.robots import Tool
from compas_fab.robots import AttachedCollisionMesh
from compas_fab.robots import CollisionMesh
from assembly import Element, Assembly
# Path settings
HERE = os.path.dirname(__file__)
DATA = os.path.abspath(os.path.join(HERE, "data"))
PATH_TO = os.path.join(DATA, "flemish_bond_planned.json")
# Load assembly settings and create instances
settings_file = os.path.join(DATA, "settings.json")
with open(settings_file, 'r') as f:
data = json.load(f)
width, length, height = data['brick_dimensions']
tolerance_vector = Vector.from_data(data['tolerance_vector'])
savelevel_vector = Vector.from_data(data['savelevel_vector'])
brick = Element.from_data(data['brick'])
start_configuration = Configuration.from_data(data['start_configuration'])
picking_frame = Frame.from_data(data['picking_frame'])
savelevel_picking_frame = picking_frame.copy()
savelevel_picking_frame.point += savelevel_vector
picking_frame.point += tolerance_vector
# create tool from json
filepath = os.path.join(DATA, "vacuum_gripper.json")
tool = Tool.from_json(filepath)
# load assembly from file
filepath = os.path.join(DATA, 'flemish_bond.json')
# load from existing if calculation failed at one point...
clear_planning_scene = True
if os.path.isfile(PATH_TO):
assembly = Assembly.from_json(PATH_TO)
clear_planning_scene = False
else:
assembly = Assembly.from_json(filepath)
# create an attached collision mesh to be attached to the robot's end effector.
T = Transformation.from_frame_to_frame(brick.gripping_frame, tool.frame)
brick_tool0 = brick.transformed(T)
attached_brick_mesh = AttachedCollisionMesh(CollisionMesh(brick_tool0.mesh, 'brick'), 'ee_link')
# ==============================================================================
# From here on: fill in code, whereever you see this dots ...
def plan_picking_motion(robot, picking_frame, savelevel_picking_frame, start_configuration, attached_brick_mesh):
"""Returns a cartesian trajectory to pick an element.
Parameters
----------
robot : :class:`compas.robots.Robot`
picking_frame : :class:`Frame`
save_level_picking_frame : :class:`Frame`
start_configuration : :class:`Configuration`
attached_brick_mesh : :class:`AttachedCollisionMesh`
Returns
-------
:class:`JointTrajectory`
"""
# Calculate frames at tool0 and picking_configuration
# ...
picking_trajectory = robot.plan_cartesian_motion(frames_tool0,
picking_configuration,
max_step=0.01,
attached_collision_meshes=[attached_brick_mesh])
return picking_trajectory
def plan_moving_and_placing_motion(robot, brick, start_configuration, tolerance_vector, savelevel_vector, attached_brick_mesh):
"""Returns two trajectories for moving and placing a brick.
Parameters
----------
robot : :class:`compas.robots.Robot`
brick : :class:`Element`
start_configuration : :class:`Configuration`
tolerance_vector : :class:`Vector`
savelevel_vector : :class:`Vector`
attached_brick_mesh : :class:`AttachedCollisionMesh`
Returns
-------
list of :class:`JointTrajectory`
"""
# Calculate goal constraints
# ...
moving_trajectory = robot.plan_motion(goal_constraints,
start_configuration,
planner_id='RRT',
attached_collision_meshes=[attached_brick_mesh],
num_planning_attempts=20,
allowed_planning_time=10)
# as start configuration take last trajectory's end configuration
# last_configuration = ...
# ...
placing_trajectory = robot.plan_cartesian_motion(frames_tool0,
last_configuration,
max_step=0.01,
attached_collision_meshes=[attached_brick_mesh])
return moving_trajectory, placing_trajectory
# NOTE: If you run Docker Toolbox, change `localhost` to `192.168.99.100`
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
robot.attach_tool(tool)
# 1. Add a collison mesh to the planning scene: floor, desk, etc.
# ...
if clear_planning_scene:
scene.remove_collision_mesh('brick_wall')
time.sleep(0.1)
# 2. Compute picking trajectory
# picking_trajectory = ...
# 3. Save the last configuration from that trajectory as new start_configuration
# start_configuration = ...
sequence = [key for key in assembly.network.vertices()]
exclude_keys = [
vkey for vkey in assembly.network.vertices_where({'is_planned': True})]
sequence = [k for k in sequence if k not in exclude_keys]
for key in sequence:
print("=" * 30 + "\nCalculating path for brick with key %d." % key)
brick = assembly.element(key)
# 4. Create an attached collision mesh and attach it to the robot's end effector.
T = Transformation.from_frame_to_frame(brick.gripping_frame, tool.frame)
brick_tool0 = brick.transformed(T)
ee_link_name = robot.get_end_effector_link_name()
attached_brick_mesh = AttachedCollisionMesh(CollisionMesh(brick_tool0.mesh, 'brick'), ee_link_name)
# 5. Calculate moving_ and placing trajectories
# moving_trajectory, placing_trajectory = ...
# 6. Add the brick to the planning scene and wait a bit after that
# ...
# 7. Add calculated trajectories to element and set to 'planned'
# brick.trajectory = [picking_trajectory, moving_trajectory, placing_trajectory]
assembly.network.set_vertex_attribute(key, 'is_planned', True)
# 8. Save assembly to json after every placed element
assembly.to_json(PATH_TO, pretty=True)