T_rightarmmount2rightgripper[:3,
3] += correction_length * T_rightarmmount2rightgripper[:
3,
2]
T_base2rightgripper = np.dot(robot_parameters.T_base2rightarmmount,
T_rightarmmount2rightgripper)
# Compute peg frame wrt base frame
T_base2hole = T_base2rightgripper.copy()
T_base2hole[:3, 3] += robot_parameters.l_hole * T_base2hole[:3, 2]
############################################################################
############################ PART 4 ########################################
################ Move peg along its z-axis towards hole ##################
############################################################################
dist = kinematic_utilities.norm(T_base2peg[:3, 3] -
T_base2hole[:3, 3])
# Update the position of peg frame after sliding it along z-axis by dist
peg_final_position = T_base2peg[:3, 3] + dist * T_base2peg[:3, 2]
P_hole2pegfinal = np.dot(T_base2hole[:3, :3].T,
peg_final_position - T_base2hole[:3, 3])
R_hole2pegfinal = np.dot(T_base2hole[:3, :3].T, T_base2peg[:3, :3])
############################################################################
############################# PART 5 #######################################
########### Visualize peg and hole cross sections in hole frame XY plane ###
############################################################################
if want_visual:
animate_assembly_square.visualize_all(T_base2leftgripper,
T_base2rightgripper,
T_base2peg, T_base2hole,
T_rightarmmount2rightgripper)
# Compute peg frame wrt base frame
T_base2hole = T_base2rightgripper.copy()
T_base2hole[:3, 3] += robot_parameters.l_hole * T_base2hole[:3, 2]
print("T_base2hole: ")
print(T_base2hole)
#
# print("Relative pose of hole wrt pe frame: ")
# print(np.dot(inv(T_base2peg), T_base2hole))
############################################################################
############################ PART 4 ########################################
################ Move peg along its z-axis towards hole ##################
############################################################################
dist = kinematic_utilities.norm(T_base2peg[:3, 3] - T_base2hole[:3, 3])
print("Distance between peg and hole frames before motion starts: %2.6f" %
dist)
# Update the position of peg frame after sliding it along z-axis by dist
peg_final_position = T_base2peg[:3, 3] + dist * T_base2peg[:3, 2]
P_hole2pegfinal = np.dot(T_base2hole[:3, :3].T,
peg_final_position - T_base2hole[:3, 3])
print("P_hole2pegfinal: ")
print(P_hole2pegfinal)
############################################################################
############################# PART 5 #######################################
########### Visualize peg and hole cross sections in hole frame XY plane ###
############################################################################
def view_animation(T_leftgripper, T_rightgripper, T_peg, T_hole, dist_peg_hole):
# Settings (newly added for video)
video_file = "myvid.mp4"
clear_frames = True # Should it clear the figure between each frame?
fps = 5
# Output video writer
FFMpegWriter = animation.writers['ffmpeg']
metadata = dict(title='Movie Test', artist='Matplotlib', comment='Movie support!')
writer = FFMpegWriter(fps=fps, metadata=metadata)
#########################################
fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111, projection='3d')
lin_vel = 0.002
num_iteration = dist_peg_hole // lin_vel
with writer.saving(fig, video_file, 200):
Xh, Yh, Zh = generate_cylinder_pts(T_rightgripper, robot_parameters.rad_hole, robot_parameters.l_hole)
h2 = ax.plot_surface(Xh, Yh, Zh, alpha=0.5, color='b')
# ax.set_xlim(0.0, 0.85)
# ax.set_ylim(-0.20, -0.01)
# ax.set_zlim(0.0, 0.45)
ax.set_xlim(0.6, 0.9)
ax.set_ylim(-0.01, 0.2)
ax.set_zlim(0.3, 0.5)
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
ax.set_title('Peg-in-Hole Assembly Simulation')
ax.view_init(30, 160)
# Plot hole frame axes
ax.quiver(T_hole[0, 3], T_hole[1, 3], T_hole[2, 3], T_hole[0, 0], T_hole[1, 0],
T_hole[2, 0], length=0.05, normalize=False, color='r')
ax.quiver(T_hole[0, 3], T_hole[1, 3], T_hole[2, 3], T_hole[0, 1], T_hole[1, 1],
T_hole[2, 1], length=0.05, normalize=False, color='g')
ax.quiver(T_hole[0, 3], T_hole[1, 3], T_hole[2, 3], T_hole[0, 2], T_hole[1, 2],
T_hole[2, 2], length=0.05, normalize=False, color='b')
# Compute Euclidean distance between peg and hole
print(T_hole[:3, 3])
print(T_peg[:3, 3])
peg_hole_euclidean_dist = kinematic_utilities.norm(T_hole[:3, 3] - T_peg[:3, 3])
print(peg_hole_euclidean_dist)
itr = 0
while peg_hole_euclidean_dist > 1e-3 and itr <= num_iteration:
Xp, Yp, Zp = generate_cylinder_pts(T_leftgripper, robot_parameters.rad_peg, robot_parameters.l_peg)
h1 = ax.plot_surface(Xp, Yp, Zp, alpha=0.5, color='r')
# Plot hole frame axes
peg_x = ax.quiver(T_peg[0, 3], T_peg[1, 3], T_peg[2, 3], T_peg[0, 0], T_peg[1, 0],
T_peg[2, 0], length=0.05, normalize=False, color='r')
peg_y = ax.quiver(T_peg[0, 3], T_peg[1, 3], T_peg[2, 3], T_peg[0, 1], T_peg[1, 1],
T_peg[2, 1], length=0.05, normalize=False, color='g')
peg_z = ax.quiver(T_peg[0, 3], T_peg[1, 3], T_peg[2, 3], T_peg[0, 2], T_peg[1, 2],
T_peg[2, 2], length=0.05, normalize=False, color='b')
# Plot number of iterations and current Euclidean distance
itr_text = ax.text(0.75, 0.075, 0.4, "iteration: " + str(itr), color='red')
# dist_str = "2.4f" % peg_hole_euclidean_dist
dist_text = ax.text(0.75, 0.075, 0.3, "Euclidean dist: %2.4f" % peg_hole_euclidean_dist, color='blue')
# write the frame to a file
writer.grab_frame()
if peg_hole_euclidean_dist <= 5e-3:
print("Peg and hole are close enough to each other")
break
# Move the gripper along the Z-axis
T_leftgripper[:3, 3] += lin_vel * T_leftgripper[:3, 2]
T_peg[:3, 3] += lin_vel * T_peg[:3, 2]
peg_hole_euclidean_dist = kinematic_utilities.norm(T_hole[:3, 3] - T_peg[:3, 3])
print(peg_hole_euclidean_dist)
# Hold the plot for 0.5s
plt.pause(0.01)
# Remove old peg plot
h1.remove()
peg_x.remove()
peg_y.remove()
peg_z.remove()
itr_text.remove()
dist_text.remove()
# increament iteration counter
itr += 1
T_base2leftgripper)
print("T_leftarmmount2leftgripper: ")
print(T_leftarmmount2leftgripper, "\n")
# Compute transformation of right_gripper frame wrt right_arm_mount frame
T_rightarmmount2rightgripper = np.dot(
kinematic_utilities.inv(robot_parameters.T_base2rightarmmount),
T_base2rightgripper)
print("T_rightarmmount2rightgripper: ")
print(T_rightarmmount2rightgripper, "\n")
###################################################################################################
######## Transformations of left and right grippers wrt corresponding arm_mount frames ############
###################################################################################################
print("Following can be used for IK-Fast input for computing IK-solutions")
print("pd_l: ", T_leftarmmount2leftgripper[:3, 3])
print("qd_l: ",
kinematic_utilities.rotm2quat(T_leftarmmount2leftgripper[:3, :3]))
print("pd_r: ", T_rightarmmount2rightgripper[:3, 3])
print("qd_r: ",
kinematic_utilities.rotm2quat(T_rightarmmount2rightgripper[:3, :3]))
###################################################################################################
######################## Visualize ################################################################
###################################################################################################
dist = kinematic_utilities.norm(T_base2leftgripper[:3, 3] -
T_base2rightgripper[:3, 3])
animate_assembly.view_animation(T_base2leftgripper, T_base2rightgripper,
T_base2peg, T_base2hole, dist)
def view_animation(T_leftgripper, T_rightgripper, T_peg, T_hole,
dist_peg_hole):
fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111, projection='3d')
lin_vel = 0.002
num_iteration = dist_peg_hole // lin_vel
Xh, Yh, Zh = generate_cylinder_pts(T_rightgripper,
robot_parameters_sl.rad_hole,
robot_parameters_sl.l_hole)
h2 = ax.plot_surface(Xh, Yh, Zh, alpha=0.5, color='b')
ax.set_xlim(0.6, 0.9)
ax.set_ylim(0.15, -0.25)
ax.set_zlim(0.0, 0.375)
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
ax.set_title('Peg-in-Hole Assembly Simulation')
ax.view_init(28, -25)
# Plot hole frame axes
ax.quiver(T_hole[0, 3],
T_hole[1, 3],
T_hole[2, 3],
T_hole[0, 0],
T_hole[1, 0],
T_hole[2, 0],
length=0.05,
normalize=False,
color='r')
ax.quiver(T_hole[0, 3],
T_hole[1, 3],
T_hole[2, 3],
T_hole[0, 1],
T_hole[1, 1],
T_hole[2, 1],
length=0.05,
normalize=False,
color='g')
ax.quiver(T_hole[0, 3],
T_hole[1, 3],
T_hole[2, 3],
T_hole[0, 2],
T_hole[1, 2],
T_hole[2, 2],
length=0.05,
normalize=False,
color='b')
# Compute Euclidean distance between peg and hole
print(T_hole[:3, 3])
print(T_peg[:3, 3])
peg_hole_euclidean_dist = kinematic_utilities.norm(T_hole[:3, 3] -
T_peg[:3, 3])
print(peg_hole_euclidean_dist)
itr = 0
while peg_hole_euclidean_dist > 1e-3 and itr <= num_iteration:
Xp, Yp, Zp = generate_cylinder_pts(T_leftgripper,
robot_parameters_sl.rad_peg,
robot_parameters_sl.l_peg)
h1 = ax.plot_surface(Xp, Yp, Zp, alpha=0.5, color='r')
# Plot hole frame axes
peg_x = ax.quiver(T_peg[0, 3],
T_peg[1, 3],
T_peg[2, 3],
T_peg[0, 0],
T_peg[1, 0],
T_peg[2, 0],
length=0.05,
normalize=False,
color='r')
peg_y = ax.quiver(T_peg[0, 3],
T_peg[1, 3],
T_peg[2, 3],
T_peg[0, 1],
T_peg[1, 1],
T_peg[2, 1],
length=0.05,
normalize=False,
color='g')
peg_z = ax.quiver(T_peg[0, 3],
T_peg[1, 3],
T_peg[2, 3],
T_peg[0, 2],
T_peg[1, 2],
T_peg[2, 2],
length=0.05,
normalize=False,
color='b')
# Plot number of iterations and current Euclidean distance
itr_text = ax.text(0.75,
0.075,
0.4,
"iteration: " + str(itr),
color='red')
# dist_str = "2.4f" % peg_hole_euclidean_dist
dist_text = ax.text(0.75,
0.075,
0.3,
"Euclidean dist: %2.4f" % peg_hole_euclidean_dist,
color='blue')
if peg_hole_euclidean_dist <= 5e-3:
print("Peg and hole are close enough to each other")
break
# Move the gripper along the Z-axis
T_leftgripper[:3, 3] += lin_vel * T_leftgripper[:3, 2]
T_peg[:3, 3] += lin_vel * T_peg[:3, 2]
peg_hole_euclidean_dist = kinematic_utilities.norm(T_hole[:3, 3] -
T_peg[:3, 3])
print(peg_hole_euclidean_dist)
# Hold the plot for 0.5s
plt.pause(0.01)
# Remove old peg plot
h1.remove()
peg_x.remove()
peg_y.remove()
peg_z.remove()
itr_text.remove()
dist_text.remove()
# increament iteration counter
itr += 1
