def project_active_cells_distal(tsframe, T03):
points = np.empty([0, 3])
for y in range(13):
for x in range(6):
if (tsframe[y, x] > 0.0):
vertex = DH.get_xyz_distal(x, y, T03)
points = np.vstack((points, vertex))
return points
def project_active_cells_distal(tsframe, T03):
points = np.empty([0,3])
for y in range(13):
for x in range(6):
if(tsframe[y,x] > 0.0):
vertex = DH.get_xyz_distal(x, y, T03)
points = np.vstack((points, vertex))
return points
def plot_cells_proximal(ax, tsframe, T02, scalarMap):
span = 0.5
for y in range(14):
for x in range(6):
vertex = np.empty([4,3])
vertex[0,:] = DH.get_xyz_proximal(x-span, y+span, T02)
vertex[1,:] = DH.get_xyz_proximal(x+span, y+span, T02)
vertex[2,:] = DH.get_xyz_proximal(x+span, y-span, T02)
vertex[3,:] = DH.get_xyz_proximal(x-span, y-span, T02)
quad = Axes3d.art3d.Poly3DCollection([vertex])
color = scalarMap.to_rgba(tsframe[y,x])
if(tsframe[y,x] > 0.0):
quad.set_color([color[0], color[1], color[2], 1.0]) # Active cell
else:
quad.set_color([0.0, 0.0, 0.0, 1.0])
quad.set_edgecolor([0.3, 0.3, 0.3, 1.0])
ax.add_collection3d(quad)
def plot_cells_distal(ax, tsframe, T03, scalarMap):
span = 0.5
for y in range(13):
for x in range(6):
vertex = np.empty([4, 3])
vertex[0, :] = DH.get_xyz_distal(x - span, y + span, T03)
vertex[1, :] = DH.get_xyz_distal(x + span, y + span, T03)
vertex[2, :] = DH.get_xyz_distal(x + span, y - span, T03)
vertex[3, :] = DH.get_xyz_distal(x - span, y - span, T03)
quad = Axes3d.art3d.Poly3DCollection([vertex])
color = scalarMap.to_rgba(tsframe[y, x])
if (tsframe[y, x] > 0.0):
quad.set_color([color[0], color[1], color[2],
1.0]) # Active cell
else:
quad.set_color([0.0, 0.0, 0.0, 1.0])
quad.set_edgecolor([0.3, 0.3, 0.3, 1.0])
ax.add_collection3d(quad)
#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)
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
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
#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) points_f2_dist = project_active_cells_distal(tsframe_distal_f2, T03_f2)
