def lie_algebras_to_pose(self, pred_lie_algebras, num_joints):
"""
arrange lie algebras and forward kinematics to pose
"""
lie_algebras = np.zeros([num_joints, 6])
i = 0
for idx, chain in enumerate(self.chains_idx):
if idx == 0:
lie_algebras[chain, 3:6] = pred_lie_algebras[0:3]
i += 3
else:
n = 4 * len(chain)
lie_algebras[chain,
0:4] = pred_lie_algebras[i:i + n].reshape([-1, 4])
i += n
pose = utils.forward_kinematics(lie_algebras, self.chains_idx)
return pose
def arrange_initial_batch_examples(_idx, pose, num_joints, chains_idx,
norm_bbx, predefined_bbx, target_pose):
# resize ratio
x_min, x_max, y_min, y_max, z_min, z_max = norm_bbx
predefined_bbx = np.asarray([predefined_bbx])
point1 = np.array([[x_min, y_min, z_min]])
point2 = np.array([[x_max, y_max, z_max]])
_resize_ratio = predefined_bbx / (point2 - point1)
# lie group of init pose
assert pose.shape == (num_joints, 3)
_lie_algebras = utils.inverse_kinematics(pose, chains_idx)
joint_xyz_f, _lie_group = utils.forward_kinematics(
_lie_algebras, chains_idx)
assert len(_lie_group) == (len(chains_idx) - 1)
# max error in current distance measurement
max_error = np.max(np.linalg.norm(target_pose - pose, axis=1))
return _idx, _resize_ratio, _lie_group, max_error
time_passed = '{:>.2f} seconds passed'.format(1./25.*num)
fig_text.set_text(time_passed)
# create the animation object, for animation to work reference to this object must be kept
line_ani = animation.FuncAnimation(fig, update_frame, seq_length,
fargs=(pos, all_lines, parents, colors + [colors[0]]),
interval=int(round(1000.0 / 25.0)), blit=False)
plt.show()
def visualize_joint_angles(joint_angles, change_color_after_frame=None):
"""
Visualize motion given joint angles in exponential map format.
:param positions: list of np arrays in shape (seq_length, n_joints*3) giving the 3D positions per joint and frame.
:param change_color_after_frame: after this frame id, the color of the plot is changed
"""
positions = [forward_kinematics(ja) for ja in joint_angles]
visualize_positions(positions, change_color_after_frame)
if __name__ == '__main__':
# load a random training sequence
# TODO change data path here
train_data = np.load('../data/train.npz')['data']
data = train_data[np.random.randint(len(train_data))]['angles']
positions = forward_kinematics(data)
visualize_positions([positions])
# 3 - Working Area
# 4 - Robot Animation
print("Modes: \n")
print("1) Forward Kinematics \n")
print("2) Inverse Kinematics \n")
print("3) Working Area \n")
print("4) Path finding \n")
mode = int(input("Please select a mode: "))
if (mode == 1):
l_1 = int(input("Please input link 1 length: "))
theta_1 = int(input("Please input link 1 angle in degrees: "))
l_2 = int(input("Please input link 2 length: "))
theta_2 = int(input("Please input link 2 angle in degrees: "))
[x, y] = forward_kinematics(l_1, l_2, theta_1, theta_2)
print(round(x, 3))
print(round(y, 3))
elif (mode == 2):
l_1 = int(input("Please input link 1 length: "))
l_2 = int(input("Please input link 2 length: "))
x = int(input("Please input x: "))
y = int(input("Please input y: "))
[theta_1, theta_2] = inverse_kinematics(l_1, l_2, x, y)
print("Theta 1")
print(theta_1)
print("Theta 2")
print(theta_2)
elif (mode == 3):
l_1 = int(input("Please input link 1 length: "))
l_2 = int(input("Please input link 2 length: "))
def plot_working_area(l_1,
l_2,
theta_1_min,
theta_1_max,
theta_2_min,
theta_2_max,
plot=True):
################################################################
################ Transform angles to radians ################
################################################################
theta_1_min = math.radians(theta_1_min)
theta_1_max = math.radians(theta_1_max)
theta_2_min = math.radians(theta_2_min)
theta_2_max = math.radians(theta_2_max)
################################################################
################ Calculate Working Area ################
################################################################
wa = l_1 * l_2 * (math.cos(theta_2_min) -
math.cos(theta_2_max)) * (theta_1_max - theta_1_min)
################################################################
################ Setup Graph ################
################################################################
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.spines['left'].set_position('center')
ax.spines['bottom'].set_position('center')
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.autoscale(True, 'both')
################################################################
################ Common Variables ################
################################################################
o = [0, 0]
################################################################
################ Link 2 at minimum ################
################################################################
start_point = forward_kinematics(l_1, l_2, theta_1_min, theta_2_min, True)
end_point = forward_kinematics(l_1, l_2, theta_1_max, theta_2_min, True)
radius = math.dist(end_point, o)
start_angle = angle(start_point, [1, 0])
end_angle = angle(end_point, [1, 0])
################################################################
current_path = Path.arc(start_angle, end_angle)
new_verts = current_path.deepcopy().vertices
for new_vert in new_verts:
new_vert[0] = new_vert[0] * radius
new_vert[1] = new_vert[1] * radius
new_verts = new_verts[:]
current_path = Path(new_verts, current_path.deepcopy().codes[:])
paths.append(current_path)
patches.append(PathPatch(current_path))
ax.add_patch(patches[0])
################################################################
################ Link 2 at maximum ################
################################################################
end_point = forward_kinematics(l_1, l_2, theta_1_min, theta_2_max, True)
start_point = forward_kinematics(l_1, l_2, theta_1_max, theta_2_max, True)
radius = math.dist(end_point, o)
start_angle = angle_clockwise(start_point, [1, 0])
end_angle = angle_clockwise(end_point, [1, 0])
################################################################
current_path = Path.arc(start_angle, end_angle)
new_verts = current_path.deepcopy().vertices
for new_vert in new_verts:
new_vert[0] = new_vert[0] * radius
new_vert[1] = new_vert[1] * radius * -1
new_verts = new_verts[:]
current_path = Path(new_verts, current_path.deepcopy().codes[:])
paths.append(current_path)
patches.append(PathPatch(current_path))
ax.add_patch(patches[1])
################################################################
################ Link 1 at minimum ################
################################################################
center = (l_1 * math.cos(theta_1_min), l_1 * math.sin(theta_1_min))
end_point = forward_kinematics(l_1, l_2, theta_1_min, theta_2_min, True)
end_point = [end_point[0] - center[0], end_point[1] - center[1]]
start_point = forward_kinematics(l_1, l_2, theta_1_min, theta_2_max, True)
start_point = [start_point[0] - center[0], start_point[1] - center[1]]
radius = l_2
start_angle = angle_clockwise(start_point, [1, 0])
end_angle = angle_clockwise(end_point, [1, 0])
################################################################
current_path = Path.arc(start_angle, end_angle)
new_verts = current_path.deepcopy().vertices
for new_vert in new_verts:
new_vert[0] = (new_vert[0] * radius + center[0])
new_vert[1] = (new_vert[1] * radius + center[1]) * -1
new_verts[np.shape(new_verts)[0] - 1] = paths[0].vertices[0]
new_verts = new_verts[:]
current_path = Path(new_verts, current_path.deepcopy().codes[:])
paths.append(current_path)
patches.append(PathPatch(current_path))
ax.add_patch(patches[2])
################################################################
################ Link 1 at maximum ################
################################################################
center = (l_1 * math.cos(theta_1_max), l_1 * math.sin(theta_1_max))
start_point = forward_kinematics(l_1, l_2, theta_1_max, theta_2_min, True)
start_point = [start_point[0] - center[0], start_point[1] - center[1]]
end_point = forward_kinematics(l_1, l_2, theta_1_max, theta_2_max, True)
end_point = [end_point[0] - center[0], end_point[1] - center[1]]
radius = l_2
start_angle = angle(start_point, [1, 0])
end_angle = angle(end_point, [1, 0])
################################################################
current_path = Path.arc(start_angle, end_angle)
new_verts = current_path.deepcopy().vertices
for new_vert in new_verts:
new_vert[0] = new_vert[0] * radius + center[0]
new_vert[1] = new_vert[1] * radius + center[1]
new_verts = new_verts[:]
current_path = Path(new_verts, current_path.deepcopy().codes[:])
paths.append(current_path)
patches.append(PathPatch(current_path))
ax.add_patch(patches[3])
################################################################
################ Plot curves ################
################################################################
ax.set_title("Working Area = {wa:.3f}".format(wa=wa))
new_path = Path.make_compound_path(
paths[0],
paths[3],
paths[1],
paths[2],
)
old_vertices = new_path.deepcopy().vertices
old_codes = new_path.deepcopy().codes
new_path_vertices = []
new_path_codes = []
for i, old_vertex in enumerate(old_vertices, start=0):
if (i == 0):
new_path_vertices.append(old_vertex)
new_path_codes.append(old_codes[i])
else:
if (old_codes[i] != 1):
new_path_vertices.append(old_vertex)
new_path_codes.append(old_codes[i])
new_path = Path(new_path_vertices, new_path_codes)
patches[0].remove()
patches[1].remove()
patches[2].remove()
patches[3].remove()
path_patch = PathPatch(new_path, fill=False, hatch='/', clip_on=True)
patches.append(path_patch)
ax.add_patch(path_patch)
if (plot == True):
plt.show()
return [ax, plt, fig]