def get_grasp_pose(translation, direction, angle, reverse, offset=1e-3):
#direction = Pose(euler=Euler(roll=np.pi / 2, pitch=direction))
return multiply(Pose(point=Point(z=offset)),
Pose(euler=Euler(yaw=angle)),
direction,
Pose(point=Point(z=translation)),
Pose(euler=Euler(roll=(1-reverse) * np.pi)))
def check_ee_element_collision(ee_body,
way_points,
phi,
theta,
exist_e_body=None,
static_bodies=[],
in_print_collision_obj=[]):
ee_yaw = sample_ee_yaw()
ee_tip_from_base = get_tip_from_ee_base(ee_body)
print_orientation = make_print_pose(phi, theta, ee_yaw)
# TODO: this assuming way points form a line
assert (len(way_points) >= 2)
e_dir = np.array(way_points[1]) - np.array(way_points[0])
(_, print_quat) = print_orientation
print_z_dir = matrix_from_quat(print_quat)[:, 2]
e_dir = e_dir / np.linalg.norm(e_dir)
print_z_dir = print_z_dir / np.linalg.norm(print_z_dir)
angle = np.arccos(np.clip(np.dot(e_dir, print_z_dir), -1.0, 1.0))
if angle > np.pi - SELF_COLLISION_ANGLE:
return True
# TODO: make this more formal...
if in_print_collision_obj:
way_pt = way_points[-1]
world_from_ee_tip = multiply(Pose(point=Point(*way_pt)),
print_orientation)
world_from_ee_base = multiply(world_from_ee_tip, ee_tip_from_base)
set_pose(ee_body, world_from_ee_base)
for ip_obj in in_print_collision_obj:
if pairwise_collision(ee_body, ip_obj) != 0:
return True
for way_pt in way_points:
world_from_ee_tip = multiply(Pose(point=Point(*way_pt)),
print_orientation)
world_from_ee_base = multiply(world_from_ee_tip, ee_tip_from_base)
set_pose(ee_body, world_from_ee_base)
if exist_e_body:
if pairwise_collision(ee_body, exist_e_body) != 0:
return True
for static_body in static_bodies:
if pairwise_collision(ee_body, static_body) != 0:
return True
return False
def check_and_draw_ee_collision(robot,
static_obstacles,
assembly_network,
exist_element_id,
check_e_id,
line_width=10,
text_size=1,
direction_len=0.005):
ee_body = load_end_effector()
val_cmap = np.ones(PHI_DISC * THETA_DISC, dtype=int)
built_obstacles = static_obstacles
built_obstacles = built_obstacles + [
assembly_network.get_element_body(exist_e_id)
for exist_e_id in exist_element_id
]
print('before pruning, cmaps sum: {}'.format(sum(val_cmap)))
print('checking print #{} collision against: '.format(check_e_id))
print(sorted(exist_element_id))
print('obstables: {}'.format(built_obstacles))
val_cmap = update_collision_map_batch(assembly_network,
ee_body,
print_along_e_id=check_e_id,
print_along_cmap=val_cmap,
bodies=built_obstacles)
print('remaining feasible directions: {}'.format(sum(val_cmap)))
# 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={})
# return check_exist_valid_kinematics(self.net, val, self.robot, val_cmap, collision_fn)
# drawing
handles = []
for e_id in exist_element_id:
p1, p2 = assembly_network.get_end_points(e_id)
handles.append(
add_line(p1, p2, color=np.array([0, 0, 1]), width=line_width))
p1, p2 = assembly_network.get_end_points(check_e_id)
e_mid = (np.array(p1) + np.array(p2)) / 2
handles.append(
add_line(p1, p2, color=np.array([0, 0, 1]), width=line_width))
for i in range(len(val_cmap)):
if val_cmap[i] == 1:
phi, theta = cmap_id2angle(i)
cmap_pose = multiply(Pose(point=e_mid),
make_print_pose(phi, theta))
origin_world = tform_point(cmap_pose, np.zeros(3))
axis = np.zeros(3)
axis[2] = 1
axis_world = tform_point(cmap_pose, direction_len * axis)
handles.append(add_line(origin_world, axis_world, color=axis))
def check_valid_kinematics(robot, way_points, phi, theta, collision_fn):
ee_yaw = sample_ee_yaw()
print_orientation = make_print_pose(phi, theta, ee_yaw)
for way_pt in way_points:
world_from_ee_tip = multiply(Pose(point=Point(*way_pt)),
print_orientation)
conf = sample_tool_ik(robot, world_from_ee_tip)
if not conf or collision_fn(conf):
# conf not exist or collision
return False
return True
def generate_sample(cap_rung, cap_vert):
assert(cap_vert)
assert(isinstance(cap_rung, CapRung) and isinstance(cap_vert, CapVert))
dir_sample = random.choice(cap_rung.ee_dirs)
ee_yaw = sample_ee_yaw()
poses = []
cap_vert.z_axis_angle = ee_yaw
cap_vert.quat_pose = make_print_pose(dir_sample[0], dir_sample[1], ee_yaw)
for pt in cap_rung.path_pts:
poses.append(multiply(Pose(point=pt), make_print_pose(dir_sample.phi, dir_sample.theta, ee_yaw)))
return poses, cap_vert
def extract_solution(self):
graphs = []
graph_indices = []
last_cap_vert = min(self.cap_rungs[-1].cap_verts, key = lambda x: x.get_cost_to_root())
while last_cap_vert:
cap_rung = self.cap_rungs[last_cap_vert.rung_id]
poses = [multiply(Pose(point=pt), last_cap_vert.quat_pose) for pt in cap_rung.path_pts]
unit_ladder_graph = generate_ladder_graph_from_poses(self.robot, self.dof, poses)
assert(unit_ladder_graph)
graphs = [unit_ladder_graph] + graphs
graph_indices = [unit_ladder_graph.size] + graph_indices
last_cap_vert = last_cap_vert.parent_vert
unified_graph = LadderGraph(self.dof)
for g in graphs:
assert(g.dof == unified_graph.dof)
unified_graph = append_ladder_graph(unified_graph, g)
dag_search = DAGSearch(unified_graph)
dag_search.run()
tot_traj = dag_search.shortest_path()
return tot_traj, graph_indices
def draw_assembly_sequence(assembly_network,
element_id_sequence,
seq_poses=None,
line_width=10,
text_size=1,
time_step=0.5,
direction_len=0.005):
handles = []
for k in element_id_sequence.keys():
e_id = element_id_sequence[k]
# n1, n2 = assembly_network.assembly_elements[e_id].node_ids
# e_body = assembly_network.assembly_elements[e_id].element_body
# p1 = assembly_network.assembly_joints[n1].node_point
# p2 = assembly_network.assembly_joints[n2].node_point
p1, p2 = assembly_network.get_end_points(e_id)
e_mid = (np.array(p1) + np.array(p2)) / 2
seq_ratio = float(k) / (len(element_id_sequence) - 1)
color = np.array([0, 0, 1]) * (1 - seq_ratio) + np.array(
[1, 0, 0]) * seq_ratio
handles.append(add_line(p1, p2, color=tuple(color), width=line_width))
handles.append(add_text(str(k), position=e_mid, text_size=text_size))
if seq_poses is not None:
assert (k in seq_poses)
for ee_dir in seq_poses[k]:
assert (isinstance(ee_dir, EEDirection))
cmap_pose = multiply(Pose(point=e_mid),
make_print_pose(ee_dir.phi, ee_dir.theta))
origin_world = tform_point(cmap_pose, np.zeros(3))
axis = np.zeros(3)
axis[2] = 1
axis_world = tform_point(cmap_pose, direction_len * axis)
handles.append(add_line(origin_world, axis_world, color=axis))
# handles.append(draw_pose(cmap_pose, direction_len))
time.sleep(time_step)
def generate_way_point_poses(p1, p2, phi, theta, ee_yaw, disc_len):
way_points = interpolate_straight_line_pts(p1, p2, disc_len)
return [
multiply(Pose(point=pt), make_print_pose(phi, theta, ee_yaw))
for pt in way_points
]
def make_print_pose(phi, theta, ee_yaw=0):
return multiply(Pose(euler=Euler(roll=theta, pitch=0, yaw=phi)),
Pose(euler=Euler(yaw=ee_yaw)))
def get_tip_from_ee_base(ee):
world_from_tool_base = get_link_pose(ee,
link_from_name(ee, EE_TOOL_BASE_LINK))
world_from_tool_tip = get_link_pose(ee, link_from_name(ee, EE_TOOL_TIP))
return multiply(invert(world_from_tool_tip), world_from_tool_base)
def meshcat_visualize_assembly_sequence(meshcat_vis, assembly_network, element_id_sequence, seq_poses,
stiffness_checker=None, viz_pose=False, viz_deform=False,
scale=1.0, time_step=1, direction_len=0.01, exagg=1.0):
# EE direction color
dir_color = [0, 1, 0]
disc = 10 # disc for deformed beam
ref_pt, _ = assembly_network.get_end_points(0)
existing_e_ids = []
if stiffness_checker:
t_tol, r_tol = stiffness_checker.get_nodal_deformation_tol()
for k in element_id_sequence.keys():
e_id = element_id_sequence[k]
existing_e_ids.append(e_id)
if not viz_deform and stiffness_checker:
print(existing_e_ids)
assert(stiffness_checker.solve(existing_e_ids))
max_t, max_r = stiffness_checker.get_max_nodal_deformation()
print("max_t: {0} / {1}, max_r: {2} / {3}".format(max_t, t_tol, max_r, r_tol))
orig_shape = stiffness_checker.get_original_shape(disc=disc, draw_full_shape=False)
beam_disp = stiffness_checker.get_deformed_shape(exagg_ratio=exagg, disc=disc)
meshcat_visualize_deformed(meshcat_vis, beam_disp, orig_shape, disc, scale=scale)
else:
p1, p2 = assembly_network.get_end_points(e_id)
p1 = ref_pt + (p1 - ref_pt) * scale
p2 = ref_pt + (p2 - ref_pt) * scale
e_mid = (np.array(p1) + np.array(p2)) / 2
seq_ratio = float(k)/(len(element_id_sequence)-1)
color = np.array([0, 0, 1])*(1-seq_ratio) + np.array([1, 0, 0])*seq_ratio
# TODO: add_text(str(k), position=e_mid, text_size=text_size)
# print('color {0} -> {1}'.format(color, rgb_to_hex(color)))
vertices = np.vstack((p1, p2)).T
mc_key = 'ase_seq_' + str(k)
meshcat_vis[mc_key].set_object(g.LineSegments(g.PointsGeometry(vertices), g.MeshBasicMaterial(color=rgb_to_hex(color))))
if seq_poses is not None and viz_pose:
assert(k in seq_poses)
dir_vertices = np.zeros([3, 2*len(seq_poses[k])])
for i, ee_dir in enumerate(seq_poses[k]):
assert(isinstance(ee_dir, EEDirection))
cmap_pose = multiply(Pose(point=e_mid), make_print_pose(ee_dir.phi, ee_dir.theta))
origin_world = e_mid
axis = np.zeros(3)
axis[2] = 1
axis_world = tform_point(cmap_pose, direction_len*axis)
dir_vertices[:, 2*i] = np.array(origin_world)
dir_vertices[:, 2*i+1] = np.array(axis_world)
mc_dir_key = 'as_dir_' + str(k)
meshcat_vis[mc_dir_key + 'line'].set_object(
g.LineSegments(g.PointsGeometry(dir_vertices),
g.MeshBasicMaterial(color=rgb_to_hex(dir_color), opacity=0.1)))
# meshcat_vis[mc_dir_key].set_object(g.Points(
# g.PointsGeometry(dir_vertices),
# g.PointsMaterial(size=0.01)))
time.sleep(time_step)