def get_intermediate_move_actions(self, from_location, to_location, hold_time, tolerance):
intermediate_moves = self.get_intermediate_moves(from_location, to_location, config.INTERMEDIATE_WAYPOINT_DISTANCE)
return [
assembly_action.AssemblyAction(
action_type='move',
goal_pose=intermediate,
pose_tolerance=tolerance,
position_hold_time=hold_time
) for intermediate in intermediate_moves
]
def get_actions_to_pick_up_block(self, platform_id, slot_coords):
"""
approach the slot then move back to center back pos
"""
actions = []
platform = self.platforms_by_id[platform_id]
center_back_pose = list(platform.center_back_pose)
if config.RANDOMLY_DISPLACE_CENTER_BACK:
center_back_pose = displace_point_randomly(center_back_pose, config.RANDOM_DISPLACEMENT_RANGE)
from_slot_low = platform.get_slot_coordinates(slot_coords)
from_slot_mid_low = list(from_slot_low)
from_slot_mid_low[2] = from_slot_low[2] + self.mid_low_offset
from_slot_high = list(from_slot_low)
from_slot_high[2] = from_slot_high[2] + self.high_offset
center_back_to_pickup_intermediates = self.get_intermediate_move_actions(
center_back_pose,
from_slot_high,
1.0,
config.ULTRA_COARSE_POSE_TOLERANCE
)
pickup_to_center_back_intermediates = self.get_intermediate_move_actions(
from_slot_high,
center_back_pose,
1.0,
config.ULTRA_COARSE_POSE_TOLERANCE
)
actions.append(assembly_action.AssemblyAction('move', center_back_pose, config.ULTRA_COARSE_POSE_TOLERANCE))
actions.extend(center_back_to_pickup_intermediates)
actions.append(assembly_action.AssemblyAction('move', from_slot_high, config.COARSE_POSE_TOLERANCE))
actions.append(assembly_action.AssemblyAction('move', from_slot_mid_low, config.COARSE_POSE_TOLERANCE))
actions.append(assembly_action.AssemblyAction('move', from_slot_low, config.TIGHT_POSE_TOLERANCE))
actions.append(assembly_action.AssemblyAction('close_gripper', from_slot_low, config.TIGHT_POSE_TOLERANCE))
actions.append(assembly_action.AssemblyAction('move', from_slot_high, config.COARSE_POSE_TOLERANCE))
actions.extend(pickup_to_center_back_intermediates)
actions.append(assembly_action.AssemblyAction('move', center_back_pose, config.ULTRA_COARSE_POSE_TOLERANCE))
return actions
def construct_tolerance_motion_experiment(cube_dimensions, cube_center, n_samples_per_tol, tol_step, tol_range, cutoff_seconds):
actions = []
initial_tolerance = [
tol_range[1],
tol_range[1],
0.03,
1000.0,
1000.0,
0.017
]
tol_samples = []
n_tolerance_steps = int((tol_range[1] - tol_range[0]) / tol_step)
for tol_step_x in range(n_tolerance_steps):
for tol_step_y in range(n_tolerance_steps):
tol_samples.append(
[
initial_tolerance[0] - float(tol_step_x) * tol_step,
initial_tolerance[1] - float(tol_step_y) * tol_step,
initial_tolerance[2],
initial_tolerance[3],
initial_tolerance[4],
initial_tolerance[5],
]
)
rospy.loginfo("Sampling {} tolerances".format(tol_samples))
goal_moves_with_tol = []
pt_1 = list(cube_center)
pt_1[1] = pt_1[1] + 0.20
pt_2 = list(cube_center)
pt_2[0] = pt_2[0] + 0.20
move_triangle = [
cube_center,
pt_1,
pt_2
]
assert(n_samples_per_tol == 3)
for sample in tol_samples:
for goal_idx in range(n_samples_per_tol):
next_point = move_triangle[goal_idx]
goal_move = [
next_point[0],
next_point[1],
next_point[2],
0.0,
0.0,
0.0,
]
goal_moves_with_tol.append(
(goal_move, sample)
)
rospy.loginfo("Number moves {}".format(len(goal_moves_with_tol)))
new_action = assembly_action.AssemblyAction(
action_type="move",
goal_pose=[move_triangle[2][0], move_triangle[2][1], move_triangle[2][2], 0.0, 0.0, 0.0],
pose_tolerance=initial_tolerance,
position_hold_time=6.0
)
for move, tol in goal_moves_with_tol:
new_action = assembly_action.AssemblyAction(
action_type="move",
goal_pose=move,
pose_tolerance=tol,
position_hold_time=6.0
)
actions.append(new_action)
new_action = assembly_action.AssemblyAction(
action_type="move",
goal_pose=move,
pose_tolerance=tol,
position_hold_time=0.0
)
new_action.timeout = cutoff_seconds
actions.append(new_action)
for idx, action in enumerate(actions):
action.high_level_build_step = "TOL_SAMPLING"
action.sequence_id = idx
return actions
def construct_actions_from_text(self, build_plan_file, start_platform):
parsed_actions = []
current_platform = start_platform
holding_block = False
last_pose = None
with open(build_plan_file) as file_stream:
file_lines = file_stream.readlines()
for line_number, line in enumerate(file_lines):
print line
line = line.strip()
command = line.split(';')[0]
if command:
if command.startswith("PICKUP"):
if holding_block:
raise Exception("Cannot do a pickup action while a block is still picked up.")
next_platform_id, x_coord, y_coord, z_coord = map(int, line.split(" ")[1:5])
if next_platform_id not in self.platforms_by_id:
raise Exception("Attempting to pick up from platform {} which does not exist. Valid platforms: {}".format(next_platform_id, list(self.platforms_by_id.keys())))
holding_block = True
if next_platform_id != current_platform:
# switch platform
next_action = assembly_action.AssemblyAction.construct_change_platforms(next_platform_id)
next_action.high_level_build_step = line
parsed_actions.append(next_action)
current_platform = next_platform_id
next_action = assembly_action.AssemblyAction('binary_P_move', self.platforms_by_id[current_platform].center_back_pose, config.ULTRA_COARSE_POSE_TOLERANCE)
next_action.high_level_build_step = line
parsed_actions.append(next_action)
# add the actions to move to the point and then close the gripper
actions_to_pick_up_block = self.get_actions_to_pick_up_block(
current_platform,
(x_coord, y_coord, z_coord)
)
for action in actions_to_pick_up_block:
action.high_level_build_step = line
parsed_actions.extend(
actions_to_pick_up_block
)
last_pose = parsed_actions[-1].goal_pose
elif command.startswith("DROP"):
next_platform_id, x_coord, y_coord, z_coord = map(int, line.split(" ")[1:5])
if not holding_block:
raise Exception("Cannot do a drop action while not holding a block.")
if next_platform_id not in self.platforms_by_id:
raise Exception("Attempting to drop on platform {} which does not exist. Valid platforms: {}".format(next_platform_id, list(self.platforms_by_id.keys())))
holding_block = False
if next_platform_id != current_platform:
# switch platform
next_action = assembly_action.AssemblyAction.construct_change_platforms(next_platform_id)
next_action.high_level_build_step = line
current_platform = next_platform_id
parsed_actions.append(next_action)
next_action = assembly_action.AssemblyAction('binary_P_move', self.platforms_by_id[current_platform].center_back_pose, config.ULTRA_COARSE_POSE_TOLERANCE)
next_action.high_level_build_step = line
parsed_actions.append(next_action)
# add the actions to move to the point and then open the gripper
actions_to_drop_block = self.get_actions_to_drop_block(
current_platform,
(x_coord, y_coord, z_coord)
)
for action in actions_to_drop_block:
action.high_level_build_step = line
parsed_actions.extend(
actions_to_drop_block
)
last_pose = parsed_actions[-1].goal_pose
elif command.startswith("MOVE"):
"""
move to an xyzrpy location relative to the platform
"""
platform_id = int(command.split(" ")[1])
x, y, z, roll, pitch, yaw = map(float, command.split(" ")[2:8])
target_location = [
x,y,z,roll,pitch,yaw
]
parsed_actions.append(
assembly_action.AssemblyAction('move', target_location, config.COARSE_POSE_TOLERANCE)
)
parsed_actions[-1].high_level_build_step = line
last_pose = parsed_actions[-1].goal_pose
parsed_actions[-1].high_level_build_step = line
elif command.startswith("ROTATE_WRIST"):
# add a rotate wrist command
target_pose = last_pose
if last_pose is None:
rospy.logwarn("ROTATE_WRIST called before a last pose is known.")
target_pose = self.platforms_by_id[current_platform].center_back_pose
target_pwm = int(command.split(" ")[1])
parsed_actions.append(
assembly_action.AssemblyAction.construct_rotate_wrist(
pwm=target_pwm,
pose=target_pose
)
)
parsed_actions[-1].high_level_build_step = line
else:
raise Exception("Unrecognized command in line {}: {}".format(line_number, line))
for idx, action in enumerate(parsed_actions):
action.sequence_id = idx
return parsed_actions