#jointangle_frame = frameManager.get_jointangle_frame(angleID)
#corresponding_jointangle_frame = frameManager.get_corresponding_jointangles(frameID)
#theta = frameManager.get_corresponding_jointangles(frameID)
Phi0 = 90 # Rotational axis (Finger 0 + 2) [0, 90]
Phi1 = -15 # Finger 0 proximal [-90, 90]
Phi2 = 15 # Finger 0 distal [-90, 90]
Phi3 = -90 # Finger 1 proximal [-90, 90]
Phi4 = 0 # Finger 1 distal [-90, 90]
Phi5 = -15 # Finger 2 proximal [-90, 90]
Phi6 = 15 # Finger 2 distal [-90, 90]
theta = np.array([Phi0, Phi1, Phi2, Phi3, Phi4, Phi5, Phi6], dtype=np.float64)
# Compute transformation matrices
T01_f0, T02_f0, T03_f0 = DH.create_transformation_matrices_f0(
Phi0, Phi1, Phi2) # Finger 0
T01_f1, T02_f1, T03_f1 = DH.create_transformation_matrices_f1(Phi3,
Phi4) # Finger 1
T01_f2, T02_f2, T03_f2 = DH.create_transformation_matrices_f2(
Phi0, Phi5, Phi6) # Finger 2
# Forward kinematics of active sensor cells
# Finger 0
points_f0_prox = project_active_cells_proximal(tsframe_proximal_f0, T02_f0)
points_f0_dist = project_active_cells_distal(tsframe_distal_f0, T03_f0)
# Finger 1
points_f1_prox = project_active_cells_proximal(tsframe_proximal_f1, T02_f1)
points_f1_dist = project_active_cells_distal(tsframe_distal_f1, T03_f1)
# Finger 3
points_f2_prox = project_active_cells_proximal(tsframe_proximal_f2, T02_f2)
Phi2_list = []
Phi5_list = []
Phi6_list = []
close_ratios = np.linspace(0, 1, 20)
for close_ratio in close_ratios:
Phi0, Phi1, Phi2, Phi3, Phi4, Phi5, Phi6 = preshape_pinch(close_ratio) # Simulated grasp
Phi1_list.append(Phi1)
Phi2_list.append(Phi2)
Phi5_list.append(Phi5)
Phi6_list.append(Phi6)
# Compute transformation matrices
T01_f0, T02_f0, T03_f0 = DH.create_transformation_matrices_f0(Phi0, Phi1, Phi2) # Finger 0
T01_f2, T02_f2, T03_f2 = DH.create_transformation_matrices_f2(Phi0, Phi5, Phi6) # Finger 2
# DH-Transform: finger 0
P_dist = DH.create_P_dist(y)
T_total = T03_f0.dot(P_dist)
p = np.array([y, 0.0, x, 1.0])
xyz_0 = T_total.dot(p)[0:3] # remove w
# DH-Transform: finger 2
P_dist = DH.create_P_dist(y)
T_total = T03_f2.dot(P_dist)
p = np.array([y, 0.0, x, 1.0])
xyz_2 = T_total.dot(p)[0:3] # remove w
# Distance between specified points on finger 0 and 2
