def check_trajectory_collision(robot, trajectory, bodies):
# TODO: each new addition makes collision checking more expensive
#offset = 4
movable_joints = get_movable_joints(robot)
#for q in trajectory[offset:-offset]:
collisions = [False for _ in range(len(bodies))] # TODO: batch collision detection
for q in trajectory:
set_joint_positions(robot, movable_joints, q)
for i, body in enumerate(bodies):
if not collisions[i]:
collisions[i] |= pairwise_collision(robot, body)
return collisions
def display_trajectories(assembly_network,
process_trajs,
extrusion_time_step=0.075,
transition_time_step=0.1):
disconnect()
assert (process_trajs)
connect(use_gui=True)
floor, robot = load_world()
camera_base_pt = assembly_network.get_end_points(0)[0]
camera_pt = np.array(camera_base_pt) + np.array([0.1, 0, 0.05])
set_camera_pose(tuple(camera_pt), camera_base_pt)
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
# connected = set(ground_nodes)
for seq_id, unit_proc in sorted(process_trajs.items()):
# if isinstance(trajectory, PrintTrajectory):
# print(trajectory, trajectory.n1 in connected, trajectory.n2 in connected,
# is_ground(trajectory.element, ground_nodes), len(trajectory.path))
# connected.add(trajectory.n2)
# #wait_for_interrupt()
# #set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
if 'transition' in unit_proc and unit_proc['transition']:
for conf in unit_proc['transition']:
set_joint_positions(robot, movable_joints, conf)
wait_for_duration(transition_time_step)
else:
print(
'seq #{} does not have transition traj found!'.format(seq_id))
for conf in unit_proc['print']:
set_joint_positions(robot, movable_joints, conf)
# if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_FRAME)))
if last_point is not None:
color = (
0, 0, 1
) #if is_ground(trajectory.element, ground_nodes) else (1, 0, 0)
handles.append(add_line(last_point, current_point,
color=color))
last_point = current_point
wait_for_duration(extrusion_time_step)
print('simulation done.')
wait_for_user()
disconnect()
def direct_ladder_graph_solve(robot, assembly_network, element_seq, seq_poses, obstacles):
dof = len(get_movable_joints(robot))
graph_list = []
built_obstacles = obstacles
disabled_collisions = get_disabled_collisions(robot)
for i in element_seq.keys():
e_id = element_seq[i]
p1, p2 = assembly_network.get_end_points(e_id)
collision_fn = get_collision_fn(robot, get_movable_joints(robot), built_obstacles,
attachments=[], self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits={} )
st_time = time.time()
while True:
ee_dir = random.choice(seq_poses[i])
rot_angle = random.uniform(-np.pi, np.pi)
way_poses = generate_way_point_poses(p1, p2, ee_dir.phi, ee_dir.theta, rot_angle, WAYPOINT_DISC_LEN)
graph = generate_ladder_graph_from_poses(robot, dof, way_poses, collision_fn)
if graph is not None:
# print(graph)
graph_list.append(graph)
break
if time.time() - st_time > 5:
break
built_obstacles.append(assembly_network.get_element_body(e_id))
unified_graph = LadderGraph(dof)
for g in graph_list:
unified_graph = append_ladder_graph(unified_graph, g)
dag_search = DAGSearch(unified_graph)
dag_search.run()
graph_sizes = [g.size for g in graph_list]
tot_traj = dag_search.shortest_path()
return tot_traj, graph_sizes
def __init__(self, robot, dof, assembly_network, element_seq, seq_poses, static_obstacles=[]):
"""seq_poses = {e_id: [(phi, theta)], ..}"""
assert(isinstance(assembly_network, AssemblyNetwork))
assert(isinstance(dof, int))
self.dof = dof
self.cap_rungs = []
self.robot = robot
disabled_collisions = get_disabled_collisions(robot)
built_obstacles = copy(static_obstacles)
seq = set()
for i in sorted(element_seq.keys()):
e_id = element_seq[i]
# TODO: temporal fix, this should be consistent with the seq search!!!
if not assembly_network.is_element_grounded(e_id):
ngbh_e_ids = seq.intersection(assembly_network.get_element_neighbor(e_id))
shared_node = set()
for n_e in ngbh_e_ids:
shared_node.update([assembly_network.get_shared_node_id(e_id, n_e)])
shared_node = list(shared_node)
else:
shared_node = [v_id for v_id in assembly_network.get_element_end_point_ids(e_id)
if assembly_network.assembly_joints[v_id].is_grounded]
assert(shared_node)
e_ns = set(assembly_network.get_element_end_point_ids(e_id))
e_ns.difference_update([shared_node[0]])
way_points = interpolate_straight_line_pts(assembly_network.get_node_point(shared_node[0]),
assembly_network.get_node_point(e_ns.pop()),
WAYPOINT_DISC_LEN)
e_body = assembly_network.get_element_body(e_id)
cap_rung = CapRung()
cap_rung.path_pts = way_points
assert(seq_poses[i])
cap_rung.ee_dirs = seq_poses[i]
cap_rung.collision_fn = get_collision_fn(robot, get_movable_joints(robot), built_obstacles,
attachments=[], self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits={})
self.cap_rungs.append(cap_rung)
built_obstacles.append(e_body)
seq.update([e_id])
def display_trajectories(assembly_network, trajectories, time_step=0.075):
disconnect()
if trajectories is None:
return
connect(use_gui=True)
floor, robot = load_world()
camera_base_pt = assembly_network.get_end_points(0)[0]
camera_pt = np.array(camera_base_pt) + np.array([0.1, 0, 0.05])
set_camera_pose(tuple(camera_pt), camera_base_pt)
# wait_for_interrupt()
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
# connected = set(ground_nodes)
for trajectory in trajectories:
# if isinstance(trajectory, PrintTrajectory):
# print(trajectory, trajectory.n1 in connected, trajectory.n2 in connected,
# is_ground(trajectory.element, ground_nodes), len(trajectory.path))
# connected.add(trajectory.n2)
# #wait_for_interrupt()
# #set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for conf in trajectory: #.path:
set_joint_positions(robot, movable_joints, conf)
# if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_FRAME)))
if last_point is not None:
color = (
0, 0, 1
) #if is_ground(trajectory.element, ground_nodes) else (1, 0, 0)
handles.append(add_line(last_point, current_point,
color=color))
last_point = current_point
wait_for_duration(time_step)
# wait_for_interrupt()
wait_for_interrupt()
disconnect()
def main(precompute=False):
parser = argparse.ArgumentParser()
# four-frame | simple_frame | djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
parser.add_argument('-p',
'--problem',
default='simple_frame',
help='The name of the problem to solve')
parser.add_argument(
'-sm',
'--search_method',
default='b',
help='csp search method, b for backward, f for forward.')
parser.add_argument(
'-vom',
'--value_order_method',
default='sp',
help=
'value ordering method, sp for special heuristic, random for random value ordering'
)
parser.add_argument('-l',
'--use_layer',
action='store_true',
help='use layer info in the search.')
# parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions with obstacles')
# parser.add_argument('-m', '--motions', action='store_true', help='Plans motions between each extrusion')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
parser.add_argument(
'-s',
'--parse_seq',
action='store_true',
help=
'parse sequence planning result from a file and proceed to motion planning'
)
parser.add_argument('-d',
'--debug',
action='store_true',
help='Debug mode.')
args = parser.parse_args()
print('Arguments:', args)
elements, node_points, ground_nodes, file_path = load_extrusion(
args.problem, parse_layers=True)
node_order = list(range(len(node_points)))
# vert indices sanity check
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element.node_ids)
]
connect(use_gui=args.viewer)
floor, robot = load_world()
# TODO: import other static obstacles
static_obstacles = [floor]
movable_joints = get_movable_joints(robot)
disabled_collisions = get_disabled_collisions(robot)
initial_conf = get_joint_positions(robot, movable_joints)
camera_pt = np.array(node_points[10]) + np.array([0.1, 0, 0.05])
target_camera_pt = node_points[0]
# create collision bodies
bodies = create_elements(node_points,
[tuple(e.node_ids) for e in elements])
for e, b in zip(elements, bodies):
e.element_body = b
# draw_pose(get_pose(b), length=0.004)
assembly_network = AssemblyNetwork(node_points, elements, ground_nodes)
assembly_network.compute_traversal_to_ground_dist()
sc = stiffness_checker(json_file_path=file_path, verbose=False)
# sc.set_self_weight_load(True)
print('test stiffness check on the whole structure: {0}'.format(
sc.solve()))
if has_gui():
pline_handle = draw_model(assembly_network, draw_tags=False)
set_camera_pose(tuple(camera_pt), target_camera_pt)
wait_for_user()
use_seq_existing_plan = args.parse_seq
if not use_seq_existing_plan:
with LockRenderer():
search_method = SEARCH_METHODS[args.search_method]
element_seq, seq_poses = plan_sequence(
robot,
static_obstacles,
assembly_network,
stiffness_checker=sc,
search_method=search_method,
value_ordering_method=args.value_order_method,
use_layer=args.use_layer,
file_name=args.problem)
write_seq_json(assembly_network, element_seq, seq_poses, args.problem)
else:
element_seq, seq_poses = read_seq_json(args.problem)
# TODO: sequence direction routing (if real fab)
####################
# sequence planning completed
if has_gui():
# wait_for_interrupt('Press a key to visualize the plan...')
# map(p.removeUserDebugItem, pline_handle)
remove_all_debug()
# draw_assembly_sequence(assembly_network, element_seq, seq_poses, time_step=1)
if USE_MESHCAT:
print('Visualizing assembly seq in meshcat...')
vis = meshcat.Visualizer()
try:
vis.open()
except:
vis.url()
meshcat_visualize_assembly_sequence(vis,
assembly_network,
element_seq,
seq_poses,
scale=3,
time_step=0.5,
direction_len=0.025)
# motion planning phase
# assume that the robot's dof is all included in the ikfast model
print('start sc motion planning.')
with LockRenderer():
# tot_traj, graph_sizes = direct_ladder_graph_solve(robot, assembly_network, element_seq, seq_poses, static_obstacles)
sg = SparseLadderGraph(robot, len(get_movable_joints(robot)),
assembly_network, element_seq, seq_poses,
static_obstacles)
sg.find_sparse_path(max_time=SPARSE_LADDER_GRAPH_SOLVE_TIMEOUT)
tot_traj, graph_sizes = sg.extract_solution()
process_trajs = {}
for seq_id, print_jt_list in enumerate(
list(divide_list_chunks(tot_traj, graph_sizes))):
process_trajs[seq_id] = {}
process_trajs[seq_id]['print'] = print_jt_list
# TODO: a separate function
# transition planning
print('start transition planning.')
moving_obstacles = {}
for seq_id, e_id in sorted(element_seq.items()):
print('transition planning # {} - E#{}'.format(seq_id, e_id))
# print('moving obs: {}'.format([get_name(mo) for mo in moving_obstacles.values()]))
# print('static obs: {}'.format([get_name(so) for so in static_obstacles]))
print('---')
if seq_id != 0:
set_joint_positions(robot, movable_joints,
process_trajs[seq_id - 1]['print'][-1])
else:
set_joint_positions(robot, movable_joints, initial_conf)
transition_traj = plan_joint_motion(robot,
movable_joints,
process_trajs[seq_id]['print'][0],
obstacles=static_obstacles +
list(moving_obstacles.values()),
self_collisions=SELF_COLLISIONS)
if not transition_traj:
add_line(*assembly_network.get_end_points(e_id))
cfn = get_collision_fn_diagnosis(
robot,
movable_joints,
obstacles=static_obstacles + list(moving_obstacles.values()),
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
st_conf = get_joint_positions(robot, movable_joints)
print('start extrusion pose:')
cfn(st_conf)
end_conf = process_trajs[seq_id]['print'][0]
print('end extrusion pose:')
cfn(end_conf)
process_trajs[seq_id]['transition'] = transition_traj
e_body = assembly_network.get_element_body(e_id)
moving_obstacles[seq_id] = e_body
print('transition planning done! proceed to simulation?')
wait_for_user()
display_trajectories(assembly_network,
process_trajs,
extrusion_time_step=0.15,
transition_time_step=0.1)
print('Quit?')
if has_gui():
wait_for_user()
def main(precompute=False):
parser = argparse.ArgumentParser()
# four-frame | simple_frame | djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
parser.add_argument('-p',
'--problem',
default='simple_frame',
help='The name of the problem to solve')
# parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions with obstacles')
# parser.add_argument('-m', '--motions', action='store_true', help='Plans motions between each extrusion')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
# parser.add_argument('-s', '--parse_seq', action='store_true', help='parse sequence planning result from a file and proceed to motion planning')
parser.add_argument('-d',
'--debug',
action='store_true',
help='Debug mode.')
args = parser.parse_args()
print('Arguments:', args)
elements, node_points, ground_nodes, file_path = load_extrusion(
args.problem, parse_layers=True)
node_order = list(range(len(node_points)))
# vert indices sanity check
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [
element for element in elements
if all(n in node_order for n in element.node_ids)
]
connect(use_gui=args.viewer)
floor, robot = load_world()
# static obstacles
obstacles = [floor]
# initial_conf = get_joint_positions(robot, get_movable_joints(robot))
# dump_body(robot)
camera_pt = np.array(node_points[10]) + np.array([0.1, 0, 0.05])
target_camera_pt = node_points[0]
# create collision bodies
bodies = create_elements(node_points,
[tuple(e.node_ids) for e in elements])
for e, b in zip(elements, bodies):
e.element_body = b
# draw_pose(get_pose(b), length=0.004)
if has_gui():
pline_handle = draw_model(assembly_network, draw_tags=False)
set_camera_pose(tuple(camera_pt), target_camera_pt)
wait_for_interrupt('Continue?')
use_seq_existing_plan = args.parse_seq
####################
# sequence planning completed
if has_gui():
# wait_for_interrupt('Press a key to visualize the plan...')
map(p.removeUserDebugItem, pline_handle)
# draw_assembly_sequence(assembly_network, element_seq, seq_poses, time_step=1)
# print('Visualizing assembly seq in meshcat...')
# vis = meshcat.Visualizer()
# try:
# vis.open()
# except:
# vis.url()
# meshcat_visualize_assembly_sequence(vis, assembly_network, element_seq, seq_poses,
# scale=3, time_step=0.5, direction_len=0.025)
# motion planning phase
# assume that the robot's dof is all included in the ikfast model
print('start sc motion planning.')
with LockRenderer():
# tot_traj, graph_sizes = direct_ladder_graph_solve(robot, assembly_network, element_seq, seq_poses, obstacles)
sg = SparseLadderGraph(robot, len(get_movable_joints(robot)),
assembly_network, element_seq, seq_poses,
obstacles)
sg.find_sparse_path(max_time=2)
tot_traj, graph_sizes = sg.extract_solution()
trajectories = list(divide_list_chunks(tot_traj, graph_sizes))
# if args.viewer:
# display_trajectories(assembly_network, trajectories, time_step=0.15)
print('Quit?')
if has_gui():
wait_for_interrupt()
def exist_valid_ee_pose(self, var, val, assignment):
# will printing edge #val collide with any assigned element?
# if var in assignment.keys():
# return False
# else:
if sum(self.cmaps[val]) == 0:
return False
else:
val_cmap = copy(self.cmaps[val])
built_obstacles = self.obstacles
success = False
# forward search
if self.search_method == 'forward':
built_obstacles = built_obstacles + [
self.net.get_element_body(assignment[i])
for i in assignment.keys() if i != var
]
# check against all existing edges except itself
for k in assignment.keys():
if k == var:
continue
exist_e_id = assignment[k]
# TODO: check print nodal direction n1 -> n2
val_cmap = update_collision_map(
self.net, self.ee_body, val, exist_e_id, val_cmap,
self.obstacles)
if sum(val_cmap) == 0:
success = False
self.constr_check_time['exist_valid_ee_pose'][
success] += 1
return success
# backward search
elif self.search_method == 'backward':
# all unassigned values are assumed to be collision objects
# TODO: only check current layer's value?
# TODO: these set difference stuff should use domain value
unassigned_vals = list(
set(range(len(self.variables))).difference(
assignment.values()))
if not self.net.is_element_grounded(val):
ngbh_e_ids = set(self.net.get_element_neighbor(
val)).intersection(unassigned_vals)
shared_node = set()
for n_e in ngbh_e_ids:
shared_node.update(
[self.net.get_shared_node_id(val, n_e)])
shared_node = list(shared_node)
else:
shared_node = [
v_id
for v_id in self.net.get_element_end_point_ids(val)
if self.net.assembly_joints[v_id].is_grounded
]
assert (shared_node)
# everytime we start fresh
# val_cmap = np.ones(PHI_DISC * THETA_DISC, dtype=int)
# print('checking #{0}-e{1}, before pruning, cmaps sum: {2}'.format(var, val, sum(val_cmap)))
# print('checking print #{} collision against: '.format(val))
# print(sorted(unassigned_vals))
# print('static obstables: {}'.format(built_obstacles))
built_obstacles = built_obstacles + [
self.net.get_element_body(unass_val)
for unass_val in unassigned_vals
]
val_cmap = update_collision_map_batch(
self.net,
self.ee_body,
print_along_e_id=val,
print_along_cmap=val_cmap,
printed_nodes=shared_node,
bodies=built_obstacles)
# print('after pruning, cmaps sum: {}'.format(sum(val_cmap)))
# print('-----')
if sum(val_cmap) < 5: #== 0:
success = False
self.constr_check_time['exist_valid_ee_pose'][
success] += 1
return success
collision_fn = get_collision_fn(
self.robot,
get_movable_joints(self.robot),
built_obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=self.disabled_collisions,
custom_limits={})
success = check_exist_valid_kinematics(self.net, val,
self.robot, val_cmap,
collision_fn)
self.constr_check_time['exist_valid_ee_pose'][success] += 1
return success
