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)