def overlay_tool(self, img_color, joint_angles, color):
try:
# 3D points w.r.t camera frame
pb = self.Rrc.T.dot(np.array([0, 0, 0]) -
self.trc) * 1000 # base position
p5 = np.array(
dvrkKinematics.fk(joint_angles,
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=0,
L4=0))[:3, 3]
p5 = self.Rrc.T.dot(np.array(p5) - self.trc) * 1000 # pitch axis
p6 = np.array(
dvrkKinematics.fk(joint_angles,
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=dvrkVar.L3,
L4=0))[:3, 3]
p6 = self.Rrc.T.dot(np.array(p6) - self.trc) * 1000 # yaw axis
p7 = np.array(
dvrkKinematics.fk(joint_angles,
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=dvrkVar.L3,
L4=dvrkVar.L4 + 0.005))[:3, 3]
p7 = self.Rrc.T.dot(np.array(p7) - self.trc) * 1000 # tip
pb_img = self.world2pixel(pb[0], pb[1], pb[2])
p5_img = self.world2pixel(p5[0], p5[1], p5[2])
p6_img = self.world2pixel(p6[0], p6[1], p6[2])
p7_img = self.world2pixel(p7[0], p7[1], p7[2])
overlayed = img_color.copy()
self.drawline(overlayed,
pb_img[0:2],
p5_img[0:2], (0, 255, 0),
1,
style='dotted',
gap=8)
cv2.circle(overlayed, p5_img[0:2], 2, color, 2)
self.drawline(overlayed,
p5_img[0:2],
p6_img[0:2], (0, 255, 0),
1,
style='dotted',
gap=8)
cv2.circle(overlayed, p6_img[0:2], 2, color, 2)
self.drawline(overlayed,
p6_img[0:2],
p7_img[0:2], (0, 255, 0),
1,
style='dotted',
gap=8)
cv2.circle(overlayed, p7_img[0:2], 2, color, 2)
return overlayed
except:
return img_color
def overlay_tool_position(self, img_color, joint_angles, color):
q1, q2, q3 = joint_angles
# 3D points w.r.t camera frame
pb = self.Rrc.T.dot(np.array([0, 0, 0]) -
self.trc) * 1000 # base position
p5 = np.array(
dvrkKinematics.fk([q1, q2, q3, 0, 0, 0],
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=0,
L4=0))[:3, 3]
p5 = self.Rrc.T.dot(np.array(p5) - self.trc) * 1000 # pitch axis
pb_img = self.world2pixel(pb[0], pb[1], pb[2])
p5_img = self.world2pixel(p5[0], p5[1], p5[2])
overlayed = img_color.copy()
self.drawline(overlayed,
pb_img[0:2],
p5_img[0:2], (0, 255, 0),
1,
style='dotted',
gap=8)
cv2.circle(overlayed, p5_img[0:2], 2, color, 2)
return overlayed
# Append data pairs
if use_Trc:
# joint angles
q_cmd_.append(q_cmd)
q_phy_.append(q_phy)
time_stamp.append(time.time() - time_st)
print('index: ', len(q_cmd_), '/', len(joint_traj))
print('t_stamp: ', time.time() - time_st)
print('q_cmd: ', q_cmd)
print('q_phy: ', q_phy)
print(' ')
else:
# positions of pitch axis
pos_cmd_temp = dvrkKinematics.fk(
[q_cmd[0], q_cmd[1], q_cmd[2], 0, 0, 0],
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=0,
L4=0)[:3, -1]
pos_cmd_.append(pos_cmd_temp)
pos_phy_.append(pt)
print('index: ', len(pos_cmd_), '/', len(joint_traj))
print('pos_des: ', pos_cmd_temp)
print('pos_act: ', pt)
print(' ')
# Visualize
cv2.imshow("images", color)
cv2.waitKey(1) & 0xFF
# cv2.waitKey(0)
# Save data to a file
import numpy as np
from FLSpegtransfer.path import *
from sklearn.metrics import mean_squared_error
from FLSpegtransfer.motion.dvrkKinematics import dvrkKinematics
import FLSpegtransfer.motion.dvrkVariables as dvrkVar
# Load experimental data
q_cmd = np.load(root + "dataset/insertion_sampled_grey1/q_cmd.npy")
q_phy = np.load(root + "dataset/insertion_sampled_grey1/q_phy.npy")
# cartesian position
pos_cmd = [
dvrkKinematics.fk(q,
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=dvrkVar.L3,
L4=dvrkVar.L4)[:3, 3] for q in q_cmd
]
pos_phy = [
dvrkKinematics.fk(q,
L1=dvrkVar.L1,
L2=dvrkVar.L2,
L3=dvrkVar.L3,
L4=dvrkVar.L4)[:3, 3] for q in q_phy
]
pos_cmd = np.array(pos_cmd)
pos_phy = np.array(pos_phy)
# Evaluate data
RMSE = np.sqrt(np.sum((pos_cmd - pos_phy)**2) / len(pos_cmd))
RMSE_axis = np.sqrt(np.sum((pos_cmd - pos_phy)**2, axis=0) / len(pos_cmd))
import numpy as np import FLSpegtransfer.utils.CmnUtil as U model = dvrkKinematics() pos = [0.3, 1.03, -0.15] rot = [0.0, -20.0, -3.0] quat = U.euler_to_quaternion(rot, 'deg') import time st = time.time() ans = model.pose_to_joint(pos, quat, method='analytic') ans2 = model.pose_to_joint(pos, quat, method='numerical') # print (ans) # print (ans2) L1 = 0.1 L2 = 0.2 L3 = 0.02 L4 = 0.05 st = time.time() a = dvrkKinematics.fk(ans, L1, L2, L3, L4) print(time.time() - st) st = time.time() b = dvrkKinematics.fk_orientation(ans) print(time.time() - st) print(a) print(b)