def tween_points_distance(points1, points2, dist, index=None):
"""Compute an interpolated set of points between two sets of points, at
a given distance.
Parameters
----------
points1 : list
The first set of points
points2 : list
The second set of points
dist : float
The distance from the first set to the second at which to compute the
interpolated set.
index: int
The index of the point in the first set from which to calculate the
distance to the second set. If no value is given, the first point will be used.
Returns
-------
list
List of points
"""
if not index:
index = 0
d = distance_point_point(points1[index], points2[index])
scale = float(dist) / d
tweens = []
for i in range(len(points1)):
tweens.append(
add_vectors(
points1[i],
scale_vector(vector_from_points(points1[i], points2[i]),
scale)))
return tweens
def tween_points(points1, points2, num):
"""Compute the interpolated points between two sets of points.
Parameters
----------
points1 : list
The first set of points
points2 : list
The second set of points
num : int
The number of interpolated sets to return
Returns
-------
list
Nested list of points
"""
tweens = []
for j in range(num - 1):
tween = []
for i in range(len(points1)):
tween.append(
add_vectors(
points1[i],
scale_vector(vector_from_points(points1[i], points2[i]),
1 / (num / (j + 1)))))
tweens.append(tween)
return tweens
def calculate_offset_pos_two_side_one_point_locked(b_struct, v_key, pt_1, pt_2,
v1, v2, d_o_1, d_o_2):
"""calculate offsetted plane when the bar's both sides are blocked by vector v1 and v2
# ! Note: the old y axis is kept in this function, local x axis is updated
Parameters
----------
b_struct : [type]
[description]
v_key : int
vertex key in BarStructure, representing a physical bar
pt_1 : list
first contact point's projection on the bar's axis
pt_2 : list
second contact point's projection on the bar's axis
v1 : list
first contact point - contact point vector
v2 : list
second contact point - contact point vector
d_o_1 : float
offset distance for end point #1
d_o_2 : float
offset distance for end point #2
Returns
-------
tuple
offset plane's origin, x-, y-, z-axis
"""
pt_1_new = add_vectors(pt_1, scale_vector(v1, -1. * d_o_1))
pt_2_new = add_vectors(pt_2, scale_vector(v2, -1. * d_o_2))
vec_x_new = normalize_vector(vector_from_points(pt_1_new, pt_2_new))
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
if not angle_vectors(x_ax, vec_x_new, deg=True) < 90:
vec_x_new = scale_vector(vec_x_new, -1.)
# transform gripping plane
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
vec_z = cross_vectors(vec_x_new, y_ax)
l_n = (pt_1_new, pt_2_new)
pt_o_new = closest_point_on_line(pt_o, l_n)
return pt_o_new, vec_x_new, y_ax, vec_z
def calculate_offset_pos_two_side_two_point_locked(b_struct, v_key, vecs_con_1,
vecs_con_2, pts_con_1,
pts_con_2, d_o_1, d_o_2):
"""calculate offsetted plane when the bar's both ends have two contact points
"""
assert len(vecs_con_1) == 2 and len(pts_con_1) == 2
assert len(vecs_con_2) == 2 and len(pts_con_2) == 2
map(normalize_vector, vecs_con_1)
map(normalize_vector, vecs_con_2)
v1_1, v1_2 = vecs_con_1
v2_1, v2_2 = vecs_con_2
pt_1_1, pt_1_2 = pts_con_1
pt_2_1, pt_2_2 = pts_con_2
vm_1 = scale_vector(normalize_vector(add_vectors(v1_1, v1_2)), -1. * d_o_1)
# original contact point (assuming two bars have the same radius)
pt_1 = centroid_points([pt_1_1, pt_1_2])
pt_1_new = translate_points([pt_1], vm_1)[0]
vm_2 = scale_vector(normalize_vector(add_vectors(v2_1, v2_2)), -1. * d_o_2)
pt_2 = centroid_points([pt_2_1, pt_2_2])
pt_2_new = translate_points([pt_2], vm_2)[0]
vec_x_new = normalize_vector(vector_from_points(pt_1_new, pt_2_new))
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
if not angle_vectors(x_ax, vec_x_new, deg=True) < 90:
vec_x_new = scale_vector(vec_x_new, -1.)
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
vec_z = cross_vectors(vec_x_new, y_ax)
l_n = (pt_1_new, pt_2_new)
pt_o_n = closest_point_on_line(pt_o, l_n)
return pt_o_n, vec_x_new, y_ax, vec_z
def f_tangent_point_2(x, *args):
ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, d1, d2, ind = args
r_c = 2 * radius
ref_point_tmp = add_vectors(scale_vector(ex, x),
scale_vector(ey, math.sqrt(r_c * r_c - x * x)))
ref_point = add_vectors(ref_point_tmp, ptM)
vecs_l_all = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ref_point,
d1, d2, ind)
if vecs_l_all:
vec_l = vecs_l_all[0]
else:
print("error in f")
f = 1
return f
f = abs(
dot_vectors(normalize_vector(vec_l),
normalize_vector(vector_from_points(ptM, ref_point))))
return f
def third_tangent(b_struct,
b_v_old,
b_v1,
b3_1,
b3_2,
pt_mean_3,
max_len,
b_v3_1,
b_v3_2,
pt_mean,
radius,
b_v0_n=None,
check_collision=False):
line_1 = b_struct.vertex[b_v_old]["axis_endpoints"]
line_2 = b_struct.vertex[b_v1]["axis_endpoints"]
b1 = b_struct.vertex[b_v_old]
b2 = b_struct.vertex[b_v1]
pt_b_1 = line_1[0]
pt_b_2 = line_2[0]
pt_b_3 = b3_1["axis_endpoints"][0]
pt_b_4 = b3_2["axis_endpoints"][0]
l_1 = normalize_vector(vector_from_points(line_1[0], line_1[1]))
l_2 = normalize_vector(vector_from_points(line_2[0], line_2[1]))
l_3 = normalize_vector(
vector_from_points(b3_1["axis_endpoints"][0],
b3_1["axis_endpoints"][1]))
l_4 = normalize_vector(
vector_from_points(b3_2["axis_endpoints"][0],
b3_2["axis_endpoints"][1]))
pts_axis_1 = dropped_perpendicular_points(line_1[0], line_1[1], line_2[0],
line_2[1])
pt_axis_1 = centroid_points(pts_axis_1)
pts_axis_2 = dropped_perpendicular_points(b3_1["axis_endpoints"][0],
b3_1["axis_endpoints"][1],
b3_2["axis_endpoints"][0],
b3_2["axis_endpoints"][1])
pt_axis_2 = centroid_points(pts_axis_2)
pt_mid = centroid_points((pt_axis_1, pt_axis_2))
axis = vector_from_points(pt_axis_1, pt_axis_2)
ex = normalize_vector(cross_vectors(normalize_vector(axis), (1, 0, 0)))
ey = normalize_vector(cross_vectors(normalize_vector(axis), ex))
bounds = (-100.0, 100.0)
args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds
# solutions_1 = []
# solutions_2 = []
# pts_3 = []
if check_collision == False:
if b_v0_n:
ind_1 = b_struct.vertex[b_v0_n]["index_sol"][0]
ind_2 = b_struct.vertex[b_v0_n]["index_sol"][1]
else:
ind_1 = 0
ind_2 = 0
# args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2
# xfunc = XFunc('coop_assembly.help_functions.tangents.solve_third_tangent', radius'C:\Users\parascho\Documents\git_repos')
# xfunc(pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2)
# ret_stt = xfunc.data
## ret_stt = solve_third_tangent(*args)
ret_stt = solve_third_tangent(pt_mid, ex, ey, radius, pt_b_1, l_1,
pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4,
bounds, ind_1, ind_2)
# if max(b_struct.vertex.keys()) == 67: print("args", args)
if ret_stt:
pt3, vec_l1, vec_l2, ang_check = ret_stt
else:
return None
# pts_3.append(pt3)
# solutions_1.append(vec_l1)
# solutions_2.append(vec_l2)
solution_1 = vec_l1
solution_2 = vec_l2
test_1 = check_length_sol_one(solution_2, pt_mean_3, pt3, b3_1, b3_2,
b_v3_1, b_v3_2, b_struct)
test_2 = check_length_sol_one(solution_1, pt_mean_3, pt3, b1, b2,
b_v_old, b_v1, b_struct)
if not test_1 or not test_2:
return None
# for n in test_1:
# for m in test_2:
# if n[4] == m[4]:
# vec_sol_31, l31, pts_b3_11, pts_b3_21, ind = n
# vec_sol_32, l32, pts_b3_12, pts_b3_22, ind_2 = m
vec_sol_31, l31, pts_b3_11, pts_b3_21 = test_1
vec_sol_32, l32, pts_b3_12, pts_b3_22 = test_2
pt3_e1 = add_vectors(pt3, scale_vector(vec_sol_31, l31))
pt3_e2 = add_vectors(pt3, scale_vector(vec_sol_32, -1 * l32))
end_pts_0 = (pt3_e2, pt3_e1)
else:
bool_test = False
for i in range(4):
for j in range(4):
ind_1 = i
ind_2 = j
args = pt_mid, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, pt_b_3, l_3, pt_b_4, l_4, bounds, ind_1, ind_2
ret_stt = solve_third_tangent(*args)
if ret_stt:
pt3, vec_l1, vec_l2, ang_check = ret_stt
else:
return None
# pts_3.append(pt3)
# solutions_1.append(vec_l1)
# solutions_2.append(vec_l2)
solution_1 = vec_l1
solution_2 = vec_l2
#for j in range(4):
test_1 = check_length_sol_one(solution_2, pt_mean_3, pt3, b3_1,
b3_2, b_v3_1, b_v3_2, b_struct)
test_2 = check_length_sol_one(solution_1, pt_mean_3, pt3, b1,
b2, b_v_old, b_v1, b_struct)
if not test_1 or not test_2:
return None
vec_sol_31, l31, pts_b3_11, pts_b3_21 = test_1
vec_sol_32, l32, pts_b3_12, pts_b3_22 = test_2
pt3_e1 = add_vectors(pt3, scale_vector(vec_sol_31, l31))
pt3_e2 = add_vectors(pt3, scale_vector(vec_sol_32, -1 * l32))
end_pts_0 = (pt3_e2, pt3_e1)
ext_len = 30
end_pts_0 = (add_vectors(
pt3_e2,
scale_vector(
normalize_vector(vector_from_points(pt3_e1, pt3_e2)),
ext_len)),
add_vectors(
pt3_e1,
scale_vector(
normalize_vector(
vector_from_points(pt3_e2, pt3_e1)),
ext_len)))
bool_col = check_colisions(b_struct,
end_pts_0,
radius,
bar_nb=b_v0_n)
if bool_col == True:
end_pts_check = b_struct.vertex[b_v3_1]["axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v3_1)
if bool_col == True:
end_pts_check = b_struct.vertex[b_v3_2][
"axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v3_2)
# bool_col = True
if bool_col == False:
print("COLLIDE", len(b_struct.vertex))
if i == 3 and j == 3 and bool_col == False:
print("NO TANGENT 3 FOUND IN ONE BAR COMBINATION")
return None
if bool_col == True:
bool_test = True
break
if bool_test == True: break
# end_pts_0 = [map(float, p) for p in end_pts_0]
vec_x, vec_y, vec_z = calculate_coord_sys(end_pts_0, pt_mean)
# pt_o = centroid_points(end_pts_0)
if not b_v0_n:
# b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (2*radius, 2.0), vec_z)
b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (25.0, 2.0), vec_z)
else:
b_v0 = b_v0_n
b_struct.vertex[b_v0].update({"axis_endpoints": end_pts_0})
b_struct.vertex[b_v0].update({"index_sol": [ind_1, ind_2]})
# b_struct.vertex[b_v0].update({"gripping_plane_no_offset":(pt_o, vec_x, vec_y, vec_z)})
# calculate_gripping_plane(b_struct, b_v0, pt_mean)
b_struct.vertex[b_v0].update({"mean_point": pt_mean})
b3_1.update({"axis_endpoints": pts_b3_11})
b3_2.update({"axis_endpoints": pts_b3_21})
if not b_v0_n:
b_struct.connect_bars(b_v0, b_v3_1)
b_struct.connect_bars(b_v0, b_v3_2)
dpp_1 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v3_1]["axis_endpoints"][0],
b_struct.vertex[b_v3_1]["axis_endpoints"][1])
dpp_2 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v3_2]["axis_endpoints"][0],
b_struct.vertex[b_v3_2]["axis_endpoints"][1])
# b_struct.edge[b_v0][b_v3_1].update({"endpoints":[dpp_1[0], dpp_1[1]]})
# b_struct.edge[b_v0][b_v3_2].update({"endpoints":[dpp_2[0], dpp_2[1]]})
k_1 = list(b_struct.edge[b_v0][b_v3_1]["endpoints"].keys())[0]
k_2 = list(b_struct.edge[b_v0][b_v3_2]["endpoints"].keys())[0]
b_struct.edge[b_v0][b_v3_1]["endpoints"].update(
{k_1: (dpp_1[0], dpp_1[1])})
b_struct.edge[b_v0][b_v3_2]["endpoints"].update(
{k_2: (dpp_2[0], dpp_2[1])})
return b_v0, pt3, end_pts_0
def second_tangent(b2_1,
b2_2,
pt_mean_2,
b_v2_1,
b_v2_2,
b_struct,
b_v_old,
pt1,
radius,
max_len,
pt_mean,
b_v0_n=None,
check_collision=False):
line = b_struct.vertex[b_v_old]["axis_endpoints"]
vec_l_0 = vector_from_points(line[0], line[1])
ex = normalize_vector(cross_vectors(normalize_vector(vec_l_0), (1, 0, 0)))
ey = normalize_vector(cross_vectors(normalize_vector(vec_l_0), ex))
ptM = pt1
pt_b_1 = b2_1["axis_endpoints"][0]
pt_b_1_2 = b2_1["axis_endpoints"][1]
l_1 = vector_from_points(pt_b_1, pt_b_1_2)
pt_b_2 = b2_2["axis_endpoints"][0]
pt_b_2_2 = b2_2["axis_endpoints"][1]
l_2 = vector_from_points(pt_b_2, pt_b_2_2)
# sols_test = tangent_from_point(pt_b_1, l_1, pt_b_2, l_2, ptM, 2*radius, 2*radius)
if check_collision == False:
if b_v0_n:
ind = b_struct.vertex[b_v0_n]["index_sol"][0]
else:
ind = 0
sols_test = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ptM,
2 * radius, 2 * radius, ind)
if not sols_test:
return None
# args = ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2*radius, 2*radius, ind
# xfunc = XFunc('coop_assembly.help_functions.tangents.solve_second_tangent', radius'C:\Users\parascho\Documents\git_repos')
# xfunc(ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2*radius, 2*radius, ind)
# ret_sst = xfunc.data
## ret_sst = solve_second_tangent(*args)
ret_sst = solve_second_tangent(ptM, ex, ey, radius, pt_b_1, l_1,
pt_b_2, l_2, 2 * radius, 2 * radius,
ind)
if ret_sst:
pt2, vec_l = ret_sst
else:
return None
solution = vec_l
ret_cls = check_length_sol_one(solution, pt_mean_2, pt2, b2_1, b2_2,
b_v2_1, b_v2_2, b_struct)
if not ret_cls:
return None
vec_sol_2, l2, pts_b2_1, pts_b2_2 = ret_cls
pt2_e = add_vectors(pt2, scale_vector(vec_sol_2, l2))
end_pts_0 = (pt2, pt2_e)
else:
for ind in range(4):
sols_test = tangent_from_point_one(pt_b_1, l_1, pt_b_2, l_2, ptM,
2 * radius, 2 * radius, ind)
if ind == 3 and sols_test == None:
return None
if sols_test == None:
continue
args = ptM, ex, ey, radius, pt_b_1, l_1, pt_b_2, l_2, 2 * radius, 2 * radius, ind
ret_sst = solve_second_tangent(*args)
if ret_sst:
pt2, vec_l = ret_sst
else:
return None
solution = vec_l
ret_cls = check_length_sol_one(solution, pt_mean_2, pt2, b2_1,
b2_2, b_v2_1, b_v2_2, b_struct)
if not ret_cls:
return None
vec_sol_2, l2, pts_b2_1, pts_b2_2 = ret_cls
pt2_e = add_vectors(pt2, scale_vector(vec_sol_2, l2))
end_pts_0 = (pt2, pt2_e)
ext_len = 30
end_pts_0 = (add_vectors(
pt2,
scale_vector(normalize_vector(vector_from_points(pt2_e, pt2)),
ext_len)),
add_vectors(
pt2_e,
scale_vector(
normalize_vector(
vector_from_points(pt2, pt2_e)),
ext_len)))
bool_col = check_colisions(b_struct,
end_pts_0,
radius,
bar_nb=b_v0_n)
if bool_col == True:
end_pts_check = b_struct.vertex[b_v2_1]["axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v2_1)
if bool_col == True:
end_pts_check = b_struct.vertex[b_v2_2]["axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v2_2)
# bool_col = True
if bool_col == False:
print("COLLIDE", len(b_struct.vertex))
if ind == 3 and bool_col == False:
print("NO TANGENT 2 FOUND IN ONE BAR COMBINATION")
return None
if bool_col == True:
break
# end_pts_0 = [map(float, p) for p in end_pts_0]
vec_x, vec_y, vec_z = calculate_coord_sys(end_pts_0, pt_mean)
# pt_o = centroid_points(end_pts_0)
if not b_v0_n:
# b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (2*radius, 2.0), vec_z)
b_v0 = b_struct.add_bar(0, end_pts_0, "tube", (25.0, 2.0), vec_z)
else:
b_v0 = b_v0_n
b_struct.vertex[b_v0].update({"axis_endpoints": end_pts_0})
b_struct.vertex[b_v0].update({"index_sol": [ind]})
# b_struct.vertex[b_v0].update({"gripping_plane_no_offset":(pt_o, vec_x, vec_y, vec_z)})
# calculate_gripping_plane(b_struct, b_v0, pt_mean)
b_struct.vertex[b_v0].update({"mean_point": pt_mean})
b2_1.update({"axis_endpoints": pts_b2_1})
b2_2.update({"axis_endpoints": pts_b2_2})
if not b_v0_n:
b_struct.connect_bars(b_v0, b_v2_1)
b_struct.connect_bars(b_v0, b_v2_2)
dpp_1 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v2_1]["axis_endpoints"][0],
b_struct.vertex[b_v2_1]["axis_endpoints"][1])
dpp_2 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v2_2]["axis_endpoints"][0],
b_struct.vertex[b_v2_2]["axis_endpoints"][1])
# b_struct.edge[b_v0][b_v2_1].update({"endpoints":[dpp_1[0], dpp_1[1]]})
# b_struct.edge[b_v0][b_v2_2].update({"endpoints":[dpp_2[0], dpp_2[1]]})
k_1 = list(b_struct.edge[b_v0][b_v2_1]["endpoints"].keys())[0]
k_2 = list(b_struct.edge[b_v0][b_v2_2]["endpoints"].keys())[0]
b_struct.edge[b_v0][b_v2_1]["endpoints"].update(
{k_1: (dpp_1[0], dpp_1[1])})
b_struct.edge[b_v0][b_v2_2]["endpoints"].update(
{k_2: (dpp_2[0], dpp_2[1])})
return b_v0, pt2, end_pts_0
def first_tangent(pt1,
b1_1,
b1_2,
pt_mean_1,
max_len,
b_v1_1,
b_v1_2,
b_struct,
pt_mean,
radius,
b_v0_n=None,
check_collision=False):
"""[summary]
SP disseration P129:
two discrete parameters are used for adjusting the topology in case a collision is found:
1. the connection side of the bar
2. the existing bars in the node that a new bar is connecting to
The process first iterates over the four possible connection sides
then runs through all possible bar pairs that a new bar can connect to in a node
the check is performed sequentially for each of the three bars in a three-bar-group
and stopped once a collision-free solution is found
.. image:: ../images/perpendicular_bar_tangent_to_two_existing_bars.png
:scale: 80 %
:align: center
Parameters
----------
pt1 : point
OverallS new vertex point
b1_1 : dict
BarS vertex
b1_2 : dict
BarS vertex
pt_mean_1 : point
contact pt
max_len : float
max allowable length of bar
b_v1_1 : int
BarS vertex key for bar b1_1
b_v1_2 : int
BarS vertex key for bar b1_2
b_struct : BarStructure
[description]
pt_mean : point
base triangle central point
radius : float
bar radius (millimeter)
b_v0_n : int, optional
index_sol attribute to indicate which tangent plane solution (four solutions in total), by default None
check_collision : bool, optional
[description], by default False
Returns
-------
[type]
[description]
"""
sol_indices = range(4) if check_colisions else [
b_struct.vertex[b_v0_n]["index_sol"][0] if b_v0_n else 0
]
for sol_id in sol_indices:
solutions_1 = tangent_from_point_one(
b1_1["axis_endpoints"][0],
subtract_vectors(b1_1["axis_endpoints"][1],
b1_1["axis_endpoints"][0]),
b1_2["axis_endpoints"][0],
subtract_vectors(b1_2["axis_endpoints"][1],
b1_2["axis_endpoints"][0]), pt1, 2 * radius,
2 * radius, sol_id)
if sol_id == 3 and solutions_1 == None:
print("jumping out")
return None
if solutions_1 == None:
print("no solutions 1", sol_id)
print("ind", sol_id)
continue
ret_cls = check_length_sol_one(solutions_1[0], pt_mean_1, pt1, b1_1,
b1_2, b_v1_1, b_v1_2, b_struct)
vec_sol_1, l1, pts_b1_1, pts_b1_2 = ret_cls
pt1_e = add_vectors(pt1, scale_vector(vec_sol_1, l1))
new_axis_end_pts = (pt1, pt1_e)
if not check_colisions:
break
# add extension
ext_len = 30
new_axis_end_pts = (add_vectors(pt1, scale_vector(normalize_vector(vector_from_points(pt1_e, pt1)), ext_len)), \
add_vectors(pt1_e, scale_vector(normalize_vector(vector_from_points(pt1, pt1_e)), ext_len)))
bool_col = check_colisions(b_struct,
new_axis_end_pts,
radius,
bar_nb=b_v0_n)
if bool_col:
end_pts_check = b_struct.vertex[b_v1_1]["axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v1_1)
if bool_col:
end_pts_check = b_struct.vertex[b_v1_2]["axis_endpoints"]
bool_col = check_colisions(b_struct,
end_pts_check,
radius,
bar_nb=b_v0_n,
bar_checking=b_v1_2)
if not bool_col:
print("COLLIDE", len(b_struct.vertex))
else:
break
if sol_id == 3 and not bool_col:
print("NO TANGENT 1 FOUND IN ONE BAR COMBINATION")
return None
####################################################################
# end_pts_0 = (pt1, add_vectors(pt1, solutions_1[0]))
##################################################################
# end_pts_0 = [map(float, p) for p in end_pts_0]
vec_x, vec_y, vec_z = calculate_coord_sys(new_axis_end_pts, pt_mean)
# pt_o = centroid_points(end_pts_0)
if not b_v0_n:
# pts_e = [map(float, p) for p in end_pts_0]
b_v0 = b_struct.add_bar(0, new_axis_end_pts, "tube", (25.0, 2.0),
vec_z)
else:
b_v0 = b_v0_n
b_struct.vertex[b_v0].update({"axis_endpoints": new_axis_end_pts})
b_struct.vertex[b_v0].update({"index_sol": [sol_id]})
# b_struct.vertex[b_v0].update({"gripping_plane_no_offset":(pt_o, vec_x, vec_y, vec_z)})
# calculate_gripping_plane(b_struct, b_v0, pt_mean)
b_struct.vertex[b_v0].update({"mean_point": pt_mean})
# b1_1.update({"axis_endpoints" : pts_b1_1})
# b1_2.update({"axis_endpoints" : pts_b1_2})
if not b_v0_n:
b_struct.connect_bars(b_v0, b_v1_1)
b_struct.connect_bars(b_v0, b_v1_2)
dpp_1 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v1_1]["axis_endpoints"][0],
b_struct.vertex[b_v1_1]["axis_endpoints"][1])
dpp_2 = dropped_perpendicular_points(
b_struct.vertex[b_v0]["axis_endpoints"][0],
b_struct.vertex[b_v0]["axis_endpoints"][1],
b_struct.vertex[b_v1_2]["axis_endpoints"][0],
b_struct.vertex[b_v1_2]["axis_endpoints"][1])
k_1 = list(b_struct.edge[b_v0][b_v1_1]["endpoints"].keys())[0]
k_2 = list(b_struct.edge[b_v0][b_v1_2]["endpoints"].keys())[0]
b_struct.edge[b_v0][b_v1_1]["endpoints"].update(
{k_1: (dpp_1[0], dpp_1[1])})
b_struct.edge[b_v0][b_v1_2]["endpoints"].update(
{k_2: (dpp_2[0], dpp_2[1])})
return b_v0, new_axis_end_pts
def generate_first_triangle(o_struct, b_struct, radius, base_tri_pts,
base_tri_ids):
"""[summary]
Parameters
----------
o_struct : [type]
to be overwritten
b_struct : [type]
to be overwritten
radius : float
bar radius, in millimeter
base_tri_pts : list of lists of 3-float
[[x, y, z], [x, y, z], [x, y, z]]
base_tri_ids : list of int
point indices for the base triangle, used for bookkeeping indices
in the OverallStructure vertex
Returns
-------
(Bar_Structure, Overall_Structure)
[description]
"""
pt_0, pt_1, pt_2 = base_tri_pts
vec_0 = normalize_vector(vector_from_points(pt_0, pt_1))
vec_1 = normalize_vector(vector_from_points(pt_1, pt_2))
vec_2 = normalize_vector(vector_from_points(pt_2, pt_0))
c_0 = scale_vector(normalize_vector(cross_vectors(vec_0, vec_1)),
2 * radius)
c_1 = scale_vector(normalize_vector(cross_vectors(vec_1, vec_2)),
2 * radius)
c_2 = scale_vector(normalize_vector(cross_vectors(vec_2, vec_0)),
2 * radius)
# bar i: start point to raised end point
end_pts_0 = (pt_0, add_vectors(pt_1, c_0))
end_pts_1 = (pt_1, add_vectors(pt_2, c_1))
end_pts_2 = (pt_2, add_vectors(pt_0, c_2))
# pt_int = centroid_points((end_pts_0[0], end_pts_0[1], end_pts_1[0], end_pts_1[1], end_pts_2[0], end_pts_2[1]))
# local coordinate system for each bar
# _, _, vec_z_0 = calculate_coord_sys(end_pts_0, pt_int)
# _, _, vec_z_1 = calculate_coord_sys(end_pts_1, pt_int)
# _, _, vec_z_2 = calculate_coord_sys(end_pts_2, pt_int)
# ? overwriting the local frame's z axis above ???
vec_z_0 = calculate_bar_z(end_pts_0)
vec_z_1 = calculate_bar_z(end_pts_1)
vec_z_2 = calculate_bar_z(end_pts_2)
# add the three bars to the Bar_Structure as vertices,
bar_type = 0
crosec_type = "tube"
crosec_values = (25.0, 2.0) # ? what does this cross section value mean?
# these are vertex keys in the Bar_Structure network
# * each bar is a vertex in the Bar_Structure
b_v0_key = b_struct.add_bar(bar_type, end_pts_0, crosec_type,
crosec_values, vec_z_0)
b_v1_key = b_struct.add_bar(bar_type, end_pts_1, crosec_type,
crosec_values, vec_z_1)
b_v2_key = b_struct.add_bar(bar_type, end_pts_2, crosec_type,
crosec_values, vec_z_2)
# pt_o_0 = centroid_points(end_pts_0)
# pt_o_1 = centroid_points(end_pts_1)
# pt_o_2 = centroid_points(end_pts_2)
# b_struct.vertex[b_v0].update({"gripping_plane": (pt_o_0, vec_x_0, vec_y_0, vec_z_0)})
# b_struct.vertex[b_v1].update({"gripping_plane": (pt_o_1, vec_x_1, vec_y_1, vec_z_1)})
# b_struct.vertex[b_v2].update({"gripping_plane": (pt_o_2, vec_x_2, vec_y_2, vec_z_2)})
pt_m = [0, 0, -10000000000000]
# calculate_gripping_plane(b_struct, b_v0, pt_m)
# calculate_gripping_plane(b_struct, b_v1, pt_m)
# calculate_gripping_plane(b_struct, b_v2, pt_m)
# ? what does this mean_point mean?
b_struct.vertex[b_v0_key].update({"mean_point": pt_m})
b_struct.vertex[b_v1_key].update({"mean_point": pt_m})
b_struct.vertex[b_v2_key].update({"mean_point": pt_m})
# calculate contact point projected on bar axes, (Pi, P_{ci}) between bar i and bar i+1
epts_0 = dropped_perpendicular_points(
b_struct.vertex[b_v0_key]["axis_endpoints"][0],
b_struct.vertex[b_v0_key]["axis_endpoints"][1],
b_struct.vertex[b_v1_key]["axis_endpoints"][0],
b_struct.vertex[b_v1_key]["axis_endpoints"][1])
epts_1 = dropped_perpendicular_points(
b_struct.vertex[b_v1_key]["axis_endpoints"][0],
b_struct.vertex[b_v1_key]["axis_endpoints"][1],
b_struct.vertex[b_v2_key]["axis_endpoints"][0],
b_struct.vertex[b_v2_key]["axis_endpoints"][1])
epts_2 = dropped_perpendicular_points(
b_struct.vertex[b_v2_key]["axis_endpoints"][0],
b_struct.vertex[b_v2_key]["axis_endpoints"][1],
b_struct.vertex[b_v0_key]["axis_endpoints"][0],
b_struct.vertex[b_v0_key]["axis_endpoints"][1])
b_struct.connect_bars(b_v0_key, b_v1_key, _endpoints=epts_0)
b_struct.connect_bars(b_v1_key, b_v2_key, _endpoints=epts_1)
b_struct.connect_bars(b_v2_key, b_v0_key, _endpoints=epts_2)
# update_edges(b_struct)
b_struct.update_bar_lengths()
tet_id = 0
# these are vertex's index in the Overall_Structure network
o_v0_key = o_struct.add_node(pt_0, v_key=base_tri_ids[0], t_key=tet_id)
o_v1_key = o_struct.add_node(pt_1, v_key=base_tri_ids[1], t_key=tet_id)
o_v2_key = o_struct.add_node(pt_2, v_key=base_tri_ids[2], t_key=tet_id)
print('vertex key: {} added to the OverallStructure as the base triangle, original ids in the list: {}'.format(\
[o_v0_key, o_v1_key, o_v2_key], base_tri_ids))
# ? shouldn't these be assigned to tet #0 as well?
# o_vi and o_vj's connection is "realized" by bar # b_v_key
o_struct.add_bar(o_v0_key, o_v1_key, b_v0_key)
o_struct.add_bar(o_v1_key, o_v2_key, b_v1_key)
o_struct.add_bar(o_v0_key, o_v2_key, b_v2_key)
# calculate and save the contact (tangent) point to each vertex
o_struct.calculate_point(o_v0_key)
o_struct.calculate_point(o_v1_key)
o_struct.calculate_point(o_v2_key)
return b_struct, o_struct
def calculate_offset(o_struct, b_struct, v_key, d_o_1, d_o_2, seq):
"""[summary]
Example usage:
by default, seq = [v for v in b_struct.vertex]
for v in b_struct.vertex:
pts.append(rg.Point3d(*b_struct.vertex[v]["mean_point"]))
calculate_gripping_plane(b_struct, v, b_struct.vertex[v]["mean_point"], nb_rot=nb_rot, nb_trans=nb_trans)
calculate_offset(o_struct, b_struct, v, offset_d1, offset_d2, seq)
calculate_offsets_all(o_struct, b_struct, v, offset_d1, offset_d2, seq)
TODO: we can perform motion planning to solve for local disassembly motion
Parameters
----------
o_struct : [type]
[description]
b_struct : [type]
[description]
v_key : [type]
vertex key in BarStructure, representing a physical bar
d_o_1 : [type]
[description]
d_o_2 : [type]
[description]
seq : [type]
[description]
"""
v_pos = seq.index(v_key)
int_v = 2 - v_pos % 3
v_pos_max = v_pos + int_v
list_verts_con = seq[0:v_pos_max + 1]
# * Find v_key bar's corresponding edge in OverallStructure
o_edge = None
for o_node1 in o_struct.edge:
# OverallStructure's nodes are ideal vertices where multiple bars come together
for o_node2 in o_struct.edge[o_node1]:
# OverallStructure's edges are bars
bar_edge = o_struct.edge[o_node1][o_node2]
if bar_edge["vertex_bar"] == v_key:
# check key correspondance between OverallStructure's edge
# and BarStructure's vertex
o_edge = (o_node1, o_node2)
break
if o_edge: break
# OverallStructure's edges are bars, find this bar's two end points' connected
cons_1 = find_connectors(o_struct, o_edge[0])
cons_2 = find_connectors(o_struct, o_edge[1])
#for c in cons_1:
#cons_all_1 = [c for c in cons_1 if c[0] <= v_key_max and c[1] <= v_key_max and (c[0] == v_key or c[1] == v_key)]
#cons_all_2 = [c for c in cons_2 if c[0] <= v_key_max and c[1] <= v_key_max and (c[0] == v_key or c[1] == v_key)]
cons_all_1 = [
c for c in cons_1
if c[0] in list_verts_con and c[1] in list_verts_con and (
c[0] == v_key or c[1] == v_key)
]
cons_all_2 = [
c for c in cons_2
if c[0] in list_verts_con and c[1] in list_verts_con and (
c[0] == v_key or c[1] == v_key)
]
bar_1 = b_struct.vertex[v_key]["axis_endpoints"]
TOL = 0.1
vecs_con_1 = [] # vectors of all connections to the bar in endpoint 1
pts_con_1 = [] # points of connections on bar axis
for c in cons_all_1:
# ep = b_struct.edge[c[0]][c[1]]["endpoints"][list(b_struct.edge[c[0]][c[1]]["endpoints"].keys())[0]]
ep = list(b_struct.edge[c[0]][c[1]]["endpoints"].values())[0]
if is_point_on_line(ep[0], bar_1, TOL):
vecs_con_1.append(vector_from_points(ep[0], ep[1]))
pts_con_1.append(ep[0])
elif is_point_on_line(ep[1], bar_1, TOL):
vecs_con_1.append(vector_from_points(ep[1], ep[0]))
pts_con_1.append(ep[1])
else:
raise RuntimeError(
"no point found on axis - check function calculate_offset")
vecs_con_2 = [] # vectors of all connections to the bar in endpoint 2
pts_con_2 = [] # points of connections on bar axis
for c in cons_all_2:
# ep = b_struct.edge[c[0]][c[1]]["endpoints"][b_struct.edge[c[0]][c[1]]["endpoints"].keys()[0]]
ep = list(b_struct.edge[c[0]][c[1]]["endpoints"].values())[0]
if is_point_on_line(ep[0], bar_1, 0.1):
vecs_con_2.append(vector_from_points(ep[0], ep[1]))
pts_con_2.append(ep[0])
elif is_point_on_line(ep[1], bar_1, 0.1):
vecs_con_2.append(vector_from_points(ep[1], ep[0]))
pts_con_2.append(ep[1])
else:
raise RuntimeError(
"no point found on axis - check function calculate_offset")
### calculate offset for first three bars (with one neighbour each)
if len(vecs_con_1) == 1 and len(vecs_con_2) == 1:
v1 = normalize_vector(vecs_con_1[0])
v2 = normalize_vector(vecs_con_2[0])
# same_dir = check_dir(v1, v2)
if angle_vectors(v1, v2, deg=True) < 90:
# not locked on the both sides, translation-only
vm = scale_vector(normalize_vector(add_vectors(v1, v2)),
-1. * d_o_1)
# shift gripping plane
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
z_ax = b_struct.vertex[v_key]["gripping_plane"][3]
pt_o_n = translate_points([pt_o], vm)[0]
b_struct.vertex[v_key].update(
{"gripping_plane_offset": (pt_o_n, x_ax, y_ax, z_ax)})
else:
# not locked on the both sides, translation-only
pt_1 = pts_con_1[0]
pt_2 = pts_con_2[0]
pt_o_n, vec_x_n, y_ax, vec_z = calculate_offset_pos_two_side_one_point_locked(
b_struct, v_key, pt_1, pt_2, v1, v2, d_o_1, d_o_2)
#pt_o_n = point_mean([pt_1_n, pt_2_n])
b_struct.vertex[v_key].update(
{"gripping_plane_offset": (pt_o_n, vec_x_n, y_ax, vec_z)})
### calculate offset for bars with neighbours only on one side
if (len(vecs_con_1) == 1
and len(vecs_con_2) == 0) or (len(vecs_con_2) == 1
and len(vecs_con_1) == 0):
if len(vecs_con_1) == 1:
v1 = normalize_vector(vecs_con_1[0])
else:
v1 = normalize_vector(vecs_con_2[0])
vm = scale_vector(v1, -1. * d_o_1)
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
z_ax = b_struct.vertex[v_key]["gripping_plane"][3]
pt_o_n = translate_points([pt_o], vm)[0]
b_struct.vertex[v_key].update(
{"gripping_plane_offset": (pt_o_n, x_ax, y_ax, z_ax)})
if (len(vecs_con_1) == 2
and len(vecs_con_2) == 0) or (len(vecs_con_2) == 2
and len(vecs_con_1) == 0):
# not locked on the both sides, translation-only
if len(vecs_con_1) == 2:
v1 = normalize_vector(vecs_con_1[0])
v2 = normalize_vector(vecs_con_1[1])
else:
v1 = normalize_vector(vecs_con_2[0])
v2 = normalize_vector(vecs_con_2[1])
vm = scale_vector(normalize_vector(add_vectors(v1, v2)), -1. * d_o_1)
# shift gripping plane
pt_o = b_struct.vertex[v_key]["gripping_plane"][0]
x_ax = b_struct.vertex[v_key]["gripping_plane"][1]
y_ax = b_struct.vertex[v_key]["gripping_plane"][2]
z_ax = b_struct.vertex[v_key]["gripping_plane"][3]
pt_o_n = translate_points([pt_o], vm)[0]
b_struct.vertex[v_key].update(
{"gripping_plane_offset": (pt_o_n, x_ax, y_ax, z_ax)})
### calculate offset for other bars (with two neighbours each)
if len(vecs_con_1) == 2 and len(vecs_con_2) == 2:
pt_o_n, vec_x_n, y_ax, vec_z = calculate_offset_pos_two_side_two_point_locked(b_struct, v_key, \
vecs_con_1, vecs_con_2, pts_con_1, pts_con_2, d_o_1, d_o_2)
#pt_o_n = point_mean([pt_1_n, pt_2_n])
b_struct.vertex[v_key].update(
{"gripping_plane_offset": (pt_o_n, vec_x_n, y_ax, vec_z)})
# return pt_o_n, vec_x_n, y_ax, vec_z
# * gripping_planes_all by applying transformation from gripping_plane
gripping_frame = Frame(*b_struct.vertex[v_key]["gripping_plane"][0:3])
gripping_frame_offset = Frame(
*b_struct.vertex[v_key]["gripping_plane_offset"][0:3])
offset_tf = Transformation.from_frame_to_frame(gripping_frame,
gripping_frame_offset)
gripping_frames_all = [
Frame(*plane[0:3]).transformed(offset_tf)
for plane in b_struct.vertex[v_key]["gripping_planes_all"]
]
b_struct.vertex[v_key].update({
"gripping_planes_offset_all":
[Frame_to_plane_data(frame) for frame in gripping_frames_all]
})
# contact point projection on the central axis
# vector connecting projected points on the bars
return pts_con_1, vecs_con_1, pts_con_2, vecs_con_2