def pose_estimation(pbr, pbg, pbb, pby, use_Trc,
which_arm): # Find tool position, joint angles
pt = []
q_phy = []
if len(pbr) < 2:
pass
else:
pt = bd.find_tool_pitch(pbr[0], pbr[1]) # tool position of pitch axis
pt = np.array(pt) * 0.001 # (m)
if use_Trc:
pt = bd.Rrc.dot(pt) + bd.trc
qp1, qp2, qp3 = dvrkKinematics.ik_position_straight(
pt, L3=0, L4=0) # position of pitch axis
# Find tool orientation, joint angles, and overlay
temp = [pbr[2], pbg, pbb, pby]
if len(pbr) < 3:
qp4 = 0.0
qp5 = 0.0
qp6 = 0.0
elif temp.count([]) >= 2:
qp4 = 0.0
qp5 = 0.0
qp6 = 0.0
else:
Rm = bd.find_tool_orientation(
pbr[2], pbg, pbb, pby,
which_arm) # orientation of the marker
qp4, qp5, qp6 = dvrkKinematics.ik_orientation(qp1, qp2, Rm)
q_phy = [qp1, qp2, qp3, qp4, qp5, qp6]
else:
q_phy = []
return pt, q_phy
def estimate_joint_angles(pbr, pbg, pbb, pby):
# Find tool(wrist) position
pt = BD.find_tool_position(pbr[0], pbr[1]) # tool position of pitch axis
pt = np.array(pt) * 0.001 # (m)
pt = BD.Rrc.dot(pt) + BD.trc
q1, q2, q3 = dvrkKinematics.ik_position_straight(pt)
# Find tool orientation
Rm = BD.find_tool_orientation(pbr[2], pbg, pbb,
pby) # orientation of the marker
q4, q5, q6 = dvrkKinematics.ik_orientation(q1, q2, Rm)
return [q1, q2, q3, q4, q5, q6]
def collect_data_joint(self,
j1,
j2,
j3,
j4,
j5,
j6,
jaw,
transform='known'): # j1, ..., j6: joint trajectory
try:
time_st = time.time() # (sec)
time_stamp = []
q_des = []
q_act = []
pos_des = []
pos_act = []
assert len(j1) == len(j2) == len(j3) == len(j4) == len(j5) == len(
j6)
for qd1, qd2, qd3, qd4, qd5, qd6 in zip(j1, j2, j3, j4, j5, j6):
joint1 = [qd1, qd2, qd3, qd4, qd5, qd6]
self.dvrk.set_joint(joint1=joint1, jaw1=jaw)
# Capture image from Zivid
self.zivid.capture_3Dimage()
img_color, img_depth, img_point = self.BD.img_crop(
self.zivid.color, self.zivid.depth, self.zivid.point)
img_color = cv2.cvtColor(img_color, cv2.COLOR_RGB2BGR)
img_color_org = np.copy(img_color)
# Find balls
pbs = self.BD.find_balls(img_color_org, img_depth, img_point)
img_color = self.BD.overlay_balls(img_color, pbs)
# Find tool position, joint angles, and overlay
if pbs[0] == [] or pbs[1] == []:
qa1 = 0.0
qa2 = 0.0
qa3 = 0.0
qa4 = 0.0
qa5 = 0.0
qa6 = 0.0
else:
# Find tool position, joint angles, and overlay
pt = self.BD.find_tool_position(
pbs[0], pbs[1]) # tool position of pitch axis
pt = np.array(pt) * 0.001 # (m)
if transform == 'known':
pt = self.BD.Rrc.dot(pt) + self.BD.trc
qa1, qa2, qa3 = dvrkKinematics.ik_position(pt)
# Find tool orientation, joint angles, and overlay
count_pbs = [pbs[2], pbs[3], pbs[4], pbs[5]]
if count_pbs.count([]) >= 2:
qa4 = 0.0
qa5 = 0.0
qa6 = 0.0
else:
Rm = self.BD.find_tool_orientation(
pbs[2], pbs[3], pbs[4],
pbs[5]) # orientation of the marker
qa4, qa5, qa6 = dvrkKinematics.ik_orientation(
qa1, qa2, Rm)
img_color = self.BD.overlay_tool(
img_color, [qa1, qa2, qa3, qa4, qa5, qa6],
(0, 255, 0))
# Append data pairs
if transform == 'known':
# joint angles
q_des.append([qd1, qd2, qd3, qd4, qd5, qd6])
q_act.append([qa1, qa2, qa3, qa4, qa5, qa6])
time_stamp.append(time.time() - time_st)
print('index: ', len(q_des), '/', len(j1))
print('t_stamp: ', time.time() - time_st)
print('q_des: ', [qd1, qd2, qd3, qd4, qd5, qd6])
print('q_act: ', [qa1, qa2, qa3, qa4, qa5, qa6])
print(' ')
elif transform == 'unknown':
# positions
pos_des_temp = self.BD.fk_position(q1=qd1,
q2=qd2,
q3=qd3,
q4=0,
q5=0,
q6=0,
L1=self.BD.L1,
L2=self.BD.L2,
L3=0,
L4=0)
pos_des.append(pos_des_temp)
pos_act.append(pt)
print('index: ', len(pos_des), '/', len(j1))
print('pos_des: ', pos_des_temp)
print('pos_act: ', pt)
print(' ')
# Visualize
cv2.imshow("images", img_color)
cv2.waitKey(1) & 0xFF
# cv2.waitKey(0)
finally:
# Save data to a file
if transform == 'known':
np.save('q_des_raw', q_des)
np.save('q_act_raw', q_act)
elif transform == 'unknown':
# Get transform from robot to camera
np.save('pos_des', pos_des)
np.save('pos_act', pos_act)
T = U.get_rigid_transform(np.array(pos_act), np.array(pos_des))
np.save('Trc', T)
np.save('t_stamp_raw', time_stamp)
print("Data is successfully saved")
# Find tool position, joint angles, and overlay
if pbr[0] == [] or pbr[1] == []:
pass
else:
pt = BD.find_tool_position(pbr[0],
pbr[1]) # tool position of pitch axis
pt = np.array(pt) * 0.001 # (m)
pt = BD.Rrc.dot(pt) + BD.trc
q1, q2, q3 = dvrkKinematics.ik_position(pt)
# print(q0*180/np.pi, q2*180/np.pi, q3)
color = BD.overlay_tool_position(color, [q1, q2, q3], (0, 255, 0))
# Find tool orientation, joint angles, and overlay
if len(pbr) < 3:
pass
elif [pbr[2], pbg, pbb, pby].count([]) < 3:
pass
else:
Rm = BD.find_tool_orientation(pbr[2], pbg, pbb,
pby) # orientation of the marker
q4, q5, q6 = dvrkKinematics.ik_orientation(q1, q2, Rm)
# print(q4*180/np.pi,q5*180/np.pi,q6*180/np.pi)
# print(q5*180/np.pi)
color = BD.overlay_tool(color, [q1, q2, q3, q4, q5, q6],
(0, 255, 0))
cv2.imshow("images", color)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
break