def place_block(self, placing_pose1=[], placing_pose2=[]): pp1 = placing_pose1 pp2 = placing_pose2 if pp1 != []: # be ready to place & close jaw pos = [pp1[0], pp1[1], self.height_ready] rot = np.deg2rad([pp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_closing self.dvrk_CM.set_pose(pos, rot, jaw) # go down toward peg & open jaw pos = [pp1[0], pp1[1], self.height_drop] rot = np.deg2rad([pp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_opening_drop self.dvrk_CM.set_pose(pos, rot, jaw) # go up pos = [pp1[0], pp1[1], self.height_ready] rot = np.deg2rad([pp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_opening_drop self.dvrk_CM.set_pose(pos, rot, jaw) if pp2 != []: raise NotImplementedError
def pickup_block(self, grasping_pose1=[], grasping_pose2=[]): gp1 = grasping_pose1 gp2 = grasping_pose2 if gp1 != []: # go above block to pickup pos = [gp1[0], gp1[1], self.height_ready] rot = np.deg2rad([gp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_closing self.dvrk_CM.set_pose(pos, rot, jaw) # approach block & open jaw pos = [gp1[0], gp1[1], gp1[2] + self.height_grasp_offset_above] rot = np.deg2rad([gp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_opening self.dvrk_CM.set_pose(pos, rot, jaw) # go down toward block & close jaw pos = [gp1[0], gp1[1], gp1[2] + self.height_grasp_offset_below] rot = np.deg2rad([gp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_closing self.dvrk_CM.set_pose(pos, rot, jaw) # go up with block pos = [gp1[0], gp1[1], self.height_ready] rot = np.deg2rad([gp1[3], 0.0, 0.0]) rot = U.euler_to_quaternion(rot) jaw = self.jaw_closing self.dvrk_CM.set_pose(pos, rot, jaw) if gp2 != []: raise NotImplementedError
def cubic_cartesian(cls, pose0, posef, vel_limit, acc_limit, tf_init=0.5, t_step=0.01): tf = tf_init while True: dt = 0.01 _, pose_traj = dvrkArm.__cubic_time(pose0=pose0, posef=posef, tf=tf, t_step=dt) # pose_traj in Cartesian, q_pos in Joint space q_pos = dvrkKinematics.pose_to_joint(pos=pose_traj[:, :3], rot=U.euler_to_quaternion(pose_traj[:, 3:])) q_pos_prev = np.insert(q_pos, 0, q_pos[0], axis=0) q_pos_prev = np.delete(q_pos_prev, -1, axis=0) q_vel = (q_pos - q_pos_prev) / dt q_vel_prev = np.insert(q_vel, 0, q_vel[0], axis=0) q_vel_prev = np.delete(q_vel_prev, -1, axis=0) q_acc = (q_vel - q_vel_prev) / dt # find maximum values vel_max = np.max(abs(q_vel), axis=0) acc_max = np.max(abs(q_acc), axis=0) if np.any(vel_max > vel_limit) or np.any(acc_max > acc_limit): if tf_init == tf: tf_init += 0.5 tf = tf_init else: tf += 0.02 break else: if tf < 0.1: break tf -= 0.01 _, pose_traj = dvrkArm.__cubic_time(pose0=pose0, posef=posef, tf=tf, t_step=t_step) q_pos = dvrkKinematics.pose_to_joint(pos=pose_traj[:, :3], rot=U.euler_to_quaternion(pose_traj[:, 3:])) return tf, q_pos
def move_above_block(self, pos1, rot1, pos2, rot2): # be ready to place & close jaw t1 = threading.Thread(target=self.arm1.set_pose_interpolate, args=([pos1[0], pos1[1], self.height_ready], U.euler_to_quaternion( np.deg2rad([rot1, 0.0, 0.0])))) t2 = threading.Thread(target=self.arm2.set_pose_interpolate, args=([pos2[0], pos2[1], self.height_ready], U.euler_to_quaternion( np.deg2rad([rot2, 0.0, 0.0])))) t3 = threading.Thread(target=self.arm1.set_jaw, args=[[self.jaw_closing[0]]]) t4 = threading.Thread(target=self.arm2.set_jaw, args=[[self.jaw_closing[1]]]) t1.daemon = True t2.daemon = True t3.daemon = True t4.daemon = True t1.start() t2.start() t3.start() t4.start() t1.join() t2.join() t3.join() t4.join()
def set_pose_interpolate(self, pos=None, rot=None, tf_init=0.5, t_step=0.01): pos0, quat0 = dvrkKinematics.joint_to_pose(self.last_joint_cmd) if len(pos) == 0: posf = pos0 else: posf = pos if len(rot) == 0: quatf = quat0 else: quatf = rot rot0 = U.quaternion_to_euler(quat0) rotf = U.quaternion_to_euler(quatf) pose0 = np.concatenate((pos0, rot0)) posef = np.concatenate((posf, rotf)) # Define trajectory if np.allclose(pose0, posef): return False else: _, q_pos = self.cubic_cartesian(pose0, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=tf_init, t_step=t_step) # Execute trajectory for q in q_pos: start = time.perf_counter() self.set_joint_direct(q) end = time.perf_counter() delta = 0.01 - (end - start) if delta > 0: time.sleep(delta) return True
def pick_block(self, pos, rot, which_arm='PSM1'): if which_arm == 'PSM1' or which_arm == 0: obj = self.arm1 index = 0 elif which_arm == 'PSM2' or which_arm == 1: obj = self.arm2 index = 1 else: raise ValueError # approach block & open jaw t1 = threading.Thread( target=obj.set_pose_interpolate, args=([pos[0], pos[1], pos[2] + self.offset_grasp[which_arm][1]], U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0])))) t2 = threading.Thread(target=obj.set_jaw_interpolate, args=[[self.jaw_opening[index]]]) t1.daemon = True t2.daemon = True t1.start() t2.start() t1.join() t2.join() # go down toward block & close jaw obj.set_pose_interpolate( pos=[pos[0], pos[1], pos[2] + self.offset_grasp[which_arm][0]], rot=U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0]))) obj.set_jaw_interpolate(jaw=[self.jaw_closing[index]])
def fit_model(self, sample): # calculate coefficients from three points p1 = sample[0] p2 = sample[1] p3 = sample[2] p4 = sample[3] A = np.array([[0, 0, 0, 0, 1], [p1[0]**2+p1[1]**2+p1[2]**2, p1[0], p1[1], p1[2], 1], [p2[0]**2+p2[1]**2+p2[2]**2, p2[0], p2[1], p2[2], 1], [p3[0]**2+p3[1]**2+p3[2]**2, p3[0], p3[1], p3[2], 1], [p4[0]**2+p4[1]**2+p4[2]**2, p4[0], p4[1], p4[2], 1]]) M11 = np.linalg.det(U.minor(A,0,0)) if M11 == 0: return [] else: M12 = np.linalg.det(U.minor(A,0,1)) M13 = np.linalg.det(U.minor(A,0,2)) M14 = np.linalg.det(U.minor(A,0,3)) M15 = np.linalg.det(U.minor(A,0,4)) xc = 0.5*M12/M11 yc = -0.5*M13/M11 zc = 0.5*M14/M11 rc = np.sqrt(xc**2 + yc**2 + zc**2 - M15/M11) return xc, yc, zc, rc
def __LSPB(self, q0, qf, t, tf, v): if np.allclose(q0,qf): return q0 elif np.all(v)==0: return q0 elif tf==0: return q0 elif tf<0: return [] elif t<0: return [] else: v_limit = (np.array(qf) - np.array(q0)) / tf if np.allclose(U.normalize(v),U.normalize(v_limit)): if np.linalg.norm(v) < np.linalg.norm(v_limit) or np.linalg.norm(2*v_limit) < np.linalg.norm(v): return [] else: tb = np.linalg.norm(np.array(q0)-np.array(qf)+np.array(v)*tf) / np.linalg.norm(v) a = np.array(v)/tb if 0 <= t and t < tb: q = np.array(q0) + np.array(a)/2*t*t elif tb < t and t <= tf - tb: q = (np.array(qf)+np.array(q0)-np.array(v)*tf)/2 + np.array(v)*t elif tf - tb < t and t <= tf: q = np.array(qf)-np.array(a)*tf*tf/2 + np.array(a)*tf*t - np.array(a)/2*t*t else: return [] return q else: return []
def return_to_origin(self, pos_place, rot_place): # trajectory to place q0 = self.dvrk_model.pose_to_joint( pos=[pos_place[0], pos_place[1], self.height_drop], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) qw = self.dvrk_model.pose_to_joint( pos=[pos_place[0], pos_place[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) qf = self.dvrk_model.pose_to_joint(pos=self.pos_org1, rot=self.rot_org1) J = self.dvrk_model.jacobian(qw) dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) dqw = np.linalg.inv(J).dot(dvw) q_pos, t = self.motion_opt_1wp.optimize(q0, qw, qf, dqw, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, t_step=0.01, print_out=False, visualize=False) for joint in q_pos[:-1]: self.dvrk.set_joint(joint=joint, wait_callback=False) if self.use_controller: time.sleep(0.005) else: time.sleep(0.01) self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True) self.dvrk.set_jaw(jaw=self.jaw_closing)
def go_pick(self, pos_pick, rot_pick): # trajectory to pick q0 = self.dvrk.get_current_joint(wait_callback=True) qw = self.dvrk_model.pose_to_joint( pos=[pos_pick[0], pos_pick[1], self.height_ready - 0.005], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qf = self.dvrk_model.pose_to_joint( pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) J = self.dvrk_model.jacobian(qw) dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) dqw = np.linalg.inv(J).dot(dvw) q_pos, t = self.motion_opt_1wp.optimize(q0, qw, qf, dqw, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, t_step=0.01, print_out=False, visualize=False) for joint in q_pos[:-1]: self.dvrk.set_joint(joint=joint, wait_callback=False) if self.use_controller: time.sleep(0.005) else: time.sleep(0.01) self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True) return q_pos, t
def loop(self): while True: # Low pass filtering fc = 1 # (Hz) dt = self.__interval_ms * 0.001 # (sec) jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5], self.__jaw1_curr_PSM1_prev, fc, dt) self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1 jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6], self.__jaw2_curr_PSM1_prev, fc, dt) self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1 jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5], self.__jaw1_curr_PSM2_prev, fc, dt) self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2 jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6], self.__jaw2_curr_PSM2_prev, fc, dt) self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2 # Current thresholding if (self.__state_jaw_current_PSM1[0] <= 0) and ( jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.threshold_PSM1): self.grasp_detected_PSM1 = True else: self.grasp_detected_PSM1 = False if (self.__state_jaw_current_PSM2[0] <= 0) and ( jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.threshold_PSM2): self.grasp_detected_PSM2 = True else: self.grasp_detected_PSM2 = False self.rate.sleep()
def transfer_block_traj(self, pos_pick, rot_pick, pos_place, rot_place, optimized, optimizer): if optimized: # trajectory to transferring block from peg to peg q0 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qw1 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qw2 = dvrkKinematics.pose_to_joint(pos=[pos_place[0], pos_place[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) qf = dvrkKinematics.pose_to_joint(pos=[pos_place[0], pos_place[1], self.height_drop], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) q0 = np.array(q0) qw1 = np.array(qw1) qw2 = np.array(qw2) qf = np.array(qf) st = time.time() if optimizer == 'cubic': q_pos, _ = self.motion_opt_2wp.optimize(q0, qw1, qw2, qf) # q_pos, _ = self.motion_opt_1wp.optimize(q0, qw1, qw2) elif optimizer == 'qp': # x, H = self.motion_opt.optimize_lift_to_handover_motion(q0[0], qw1[0], qw2[0]) x, H = self.motion_opt.optimize_motion(q0[0], qw1[0], qw2[0], qf[0]) if x is not None: dim = 6 q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)]) elif optimizer == 'mtsqp': x, H = self.motion_opt.optimize_motion(q0[0], qw1[0], qw2[0], qf[0]) if x is not None: dim = 6 q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)]) self.time_computing.append(time.time() - st) else: # trajectory to transferring block from peg to peg pos0 = [pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]] rot0 = [np.deg2rad(rot_pick), 0.0, 0.0] pose0 = np.concatenate((pos0, rot0)) posw1 = [pos_pick[0], pos_pick[1], self.height_ready] rotw1 = [np.deg2rad(rot_pick), 0.0, 0.0] posew1 = np.concatenate((posw1, rotw1)) posw2 = [pos_place[0], pos_place[1], self.height_ready] rotw2 = [np.deg2rad(rot_place), 0.0, 0.0] posew2 = np.concatenate((posw2, rotw2)) # posf = [pos_place[0], pos_place[1], self.height_drop] # rotf = [np.deg2rad(rot_place), 0.0, 0.0] # posef = np.concatenate((posf, rotf)) # Define trajectory _, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew1, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) _, q_pos2 = dvrkArm.cubic_cartesian(posew1, posew2, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) # _, q_pos3 = dvrkArm.cubic_cartesian(posew2, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, # tf_init=0.5, t_step=0.01) # q_pos = np.concatenate((q_pos1, q_pos2, q_pos3)) q_pos = np.concatenate((q_pos1, q_pos2)) return q_pos
def go_handover_traj(self, obj_opt, pos_pick, rot_pick, pos_place, rot_place, optimized, optimizer, which_arm): self.lock.acquire() if optimized: # trajectory to transferring block from peg to peg q0 = dvrkKinematics.pose_to_joint( pos=[ pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[which_arm][0] ], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qw = dvrkKinematics.pose_to_joint( pos=[pos_pick[0], pos_pick[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qf = dvrkKinematics.pose_to_joint( pos=[pos_place[0], pos_place[1], self.height_handover], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) if optimizer == 'cubic': q_pos, _ = obj_opt.optimize(q0, qw, qf) elif optimizer == 'qp': x, H = self.motion_opt.optimize_lift_to_handover_motion( q0[0], qw[0], qf[0]) if x is not None: dim = 6 q_pos = np.array( [x[t * dim:(t + 1) * dim] for t in range(H + 1)]) else: # trajectory to transferring block from peg to peg pos0 = [ pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[which_arm][0] ] rot0 = [np.deg2rad(rot_pick), 0.0, 0.0] pose0 = np.concatenate((pos0, rot0)) posw = [pos_pick[0], pos_pick[1], self.height_ready] rotw = [np.deg2rad(rot_pick), 0.0, 0.0] posew = np.concatenate((posw, rotw)) posf = [pos_place[0], pos_place[1], self.height_handover] rotf = [np.deg2rad(rot_place), 0.0, 0.0] posef = np.concatenate((posf, rotf)) # Define trajectory _, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) _, q_pos2 = dvrkArm.cubic_cartesian(posew, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) q_pos = np.concatenate((q_pos1, q_pos2)) self.lock.release() return q_pos
def servoing_block(self, pos_place, rot_place): # go down toward block & open jaw self.arm1.set_pose_interpolate(pos=[pos_place[0], pos_place[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) pos = [pos_place[0], pos_place[1], self.height_drop] rot = U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])) self.arm1.set_pose_interpolate(pos=pos, rot=rot) self.arm1.set_jaw_interpolate(jaw=self.jaw_opening_drop)
def insertion_sampling(dvrk_model, insertion_number, dvrk_motion): pos_org = [0.080, 0.0, -0.095] rot_org = [0.0, 0.0, 0.0, 1.0] pos_min = [0.172, 0.036, -0.150] pos_max = [0.080, -0.051, -0.110] height_ready = -0.120 height_block = [-0.155, -0.150] traj_pos = [] traj_joint = [] # dvrk_motion.set_jaw(jaw1=[0.0]) for i in range(insertion_number): # pick pos_above = np.random.uniform(pos_min, pos_max) pos_above[2] = height_ready pos_grasp = [ pos_above[0], pos_above[1], np.random.uniform(height_block[0], height_block[1]) ] # action 1 traj_pos.append(pos_org) q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_org, rot_org) traj_joint.append([q1, q2, q3, q4, q5, q6]) # dvrk_motion.set_joint(joint1=[q1,q2,q3,q4,q5,q6]) # action 2 traj_pos.append(pos_above) q4_rand = np.random.uniform(-np.pi / 2, np.pi / 2) rot_q = U.euler_to_quaternion([q4_rand, 0.0, 0.0]) q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q) traj_joint.append([q1, q2, q3, q4, q5, q6]) # dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6]) # action 3 traj_pos.append(pos_grasp) q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_grasp, rot_q) traj_joint.append([q1, q2, q3, q4, q5, q6]) # dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6]) # action 4 traj_pos.append(pos_above) q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q) traj_joint.append([q1, q2, q3, q4, q5, q6]) # dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6]) # place pos_above = np.random.uniform(pos_min, pos_max) pos_above[2] = height_ready # action 5 traj_pos.append(pos_above) rot_q = U.euler_to_quaternion([0.0, 0.0, 0.0]) q1, q2, q3, q4, q5, q6 = dvrk_model.pose_to_joint(pos_above, rot_q) traj_joint.append([q1, q2, q3, q4, q5, q6]) # dvrk_motion.set_joint(joint1=[q1, q2, q3, q4, q5, q6]) print(i, "th insertion traj. sampled") return traj_pos, traj_joint
def run(self, stop): while True: # Resizing images img1 = cv2.resize(self.__img_raw_left, (self.__img_width, self.__img_height)) img2 = cv2.resize(self.__img_raw_right, (self.__img_width, self.__img_height)) # Low pass filtering fc = 1 # (Hz) dt = 0.01 # (ms) jaw1_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[5], self.__jaw1_curr_PSM1_prev, fc, dt) self.__jaw1_curr_PSM1_prev = jaw1_curr_PSM1 jaw2_curr_PSM1 = U.LPF(self.__actuator_current_measured_PSM1[6], self.__jaw2_curr_PSM1_prev, fc, dt) self.__jaw2_curr_PSM1_prev = jaw2_curr_PSM1 jaw1_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[5], self.__jaw1_curr_PSM2_prev, fc, dt) self.__jaw1_curr_PSM2_prev = jaw1_curr_PSM2 jaw2_curr_PSM2 = U.LPF(self.__actuator_current_measured_PSM2[6], self.__jaw2_curr_PSM2_prev, fc, dt) self.__jaw2_curr_PSM2_prev = jaw2_curr_PSM2 # Current thresholding self.__threshold_PSM1 = 0.3 # (A) self.__threshold_PSM2 = 0.25 # (A) if self.__state_jaw_current_PSM1[0] <= 0: if jaw1_curr_PSM1 - jaw2_curr_PSM1 > self.__threshold_PSM1: cv2.circle(img1, (540, 300), 30, (0, 255, 0), -1) else: cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1) else: cv2.circle(img1, (540, 300), 30, (0, 0, 255), -1) if self.__state_jaw_current_PSM2[0] <= 0: if jaw1_curr_PSM2 - jaw2_curr_PSM2 > self.__threshold_PSM2: cv2.circle(img1, (100, 300), 30, (0, 255, 0), -1) else: cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1) else: cv2.circle(img1, (100, 300), 30, (0, 0, 255), -1) cv2.imshow("original1", img1) if self.__video_recording is True: self.out.write(img1) self.rate.sleep() if cv2.waitKey(1) & 0xFF == ord('q'): if self.__video_recording is True: print("finishing recording") self.out.release() cv2.destroyAllWindows() stop() break
def return_to_peg_traj(self, obj, pos_pick, rot_pick, optimized, optimizer): # trajectory of returning to another peg to pick-up if optimized: p0, quat0 = self.arm1.get_current_pose() q0 = self.arm1.get_current_joint() qw1 = dvrkKinematics.pose_to_joint(pos=[p0[0], p0[1], self.height_ready], rot=quat0) qw2 = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qf = dvrkKinematics.pose_to_joint(pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) q0 = np.array(q0) qw1 = np.array(qw1) qw2 = np.array(qw2) qf = np.array(qf) st = time.time() if optimizer == 'cubic': q_pos, _ = self.motion_opt_2wp.optimize(q0, qw1, qw2, qf) # q_pos, _ = self.motion_opt_1wp.optimize(q0, qw1, qw2, dqw1) elif optimizer == 'qp': x, H = self.motion_opt.optimize_motion(q0, qw1[0], qw2[0], qf[0]) if x is not None: dim = 6 q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)]) elif optimizer == 'mtsqp': x, H = self.motion_opt.optimize_motion(q0, qw1[0], qw2[0], qf[0]) if x is not None: dim = 6 q_pos = np.array([x[t * dim:(t + 1) * dim] for t in range(H + 1)]) self.time_computing.append(time.time() - st) else: pos0, quat0 = obj.get_current_pose() rot0 = U.quaternion_to_euler(quat0) pose0 = np.concatenate((pos0, rot0)) posw1 = [pos0[0], pos0[1], self.height_ready] rotw1 = rot0 posew1 = np.concatenate((posw1, rotw1)) posw2 = [pos_pick[0], pos_pick[1], self.height_ready] rotw2 = [np.deg2rad(rot_pick), 0.0, 0.0] posew2 = np.concatenate((posw2, rotw2)) posf = [pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1]] rotf = [np.deg2rad(rot_pick), 0.0, 0.0] posef = np.concatenate((posf, rotf)) # Define trajectory _, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew1, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) _, q_pos2 = dvrkArm.cubic_cartesian(posew1, posew2, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) _, q_pos3 = dvrkArm.cubic_cartesian(posew2, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) q_pos = np.concatenate((q_pos1, q_pos2, q_pos3)) return q_pos
def go_pick_traj(self, obj, pos_pick, rot_pick, optimized, optimizer): if optimized: # trajectory to pick q0 = obj.get_current_joint() qw = dvrkKinematics.pose_to_joint( pos=[pos_pick[0], pos_pick[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qf = dvrkKinematics.pose_to_joint( pos=[ pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1] ], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) if optimizer == 'cubic': q_pos, _ = self.motion_opt_1wp.optimize(q0, qw, qf) elif optimizer == 'qp': x, H = self.motion_opt.optimize_handover_to_drop_motion( q0, qw[0], qf[0]) if x is not None: dim = 6 q_pos = np.array( [x[t * dim:(t + 1) * dim] for t in range(H + 1)]) else: pos0, quat0 = obj.get_current_pose() rot0 = U.quaternion_to_euler(quat0) pose0 = np.concatenate((pos0, rot0)) posw = [pos_pick[0], pos_pick[1], self.height_ready] rotw = [np.deg2rad(rot_pick), 0.0, 0.0] posew = np.concatenate((posw, rotw)) posf = [ pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[1] ] rotf = [np.deg2rad(rot_pick), 0.0, 0.0] posef = np.concatenate((posf, rotf)) # Define trajectory _, q_pos1 = dvrkArm.cubic_cartesian(pose0, posew, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) _, q_pos2 = dvrkArm.cubic_cartesian(posew, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=0.5, t_step=0.01) q_pos = np.concatenate((q_pos1, q_pos2)) return q_pos
def set_pose(self, pos, rot, unit='rad', wait_callback=True): """ :param pos_des: position array [x,y,z] :param rot_des: rotation array [Z,Y,X euler angle] :param unit: 'rad' or 'deg' :param wait_callback: True or False """ if unit == 'deg': rot = U.deg_to_rad(rot) # limiting range of motion limit_angle = 89 * np.pi / 180 if rot[1] > limit_angle: rot[1] = limit_angle elif rot[1] < -limit_angle: rot[1] = -limit_angle if rot[2] > limit_angle: rot[2] = limit_angle elif rot[2] < -limit_angle: rot[2] = -limit_angle # set in position cartesian mode frame = self.NumpyArraytoPyKDLFrame(pos, rot) msg = posemath.toMsg(frame) print msg # go to that position by goal if wait_callback: return self.__set_position_goal_cartesian_publish_and_wait(msg) else: self.goal_reached = False self.__set_position_goal_cartesian_pub.publish(msg) return True
def NumpyArraytoPyKDLFrame(self, pos, rot, unit='rad'): if unit == 'deg': rot = U.deg_to_rad(rot) px, py, pz = pos rz, ry, rx = np.array([-np.pi / 2, np.pi, 0]) - np.array(rot) return PyKDL.Frame(PyKDL.Rotation.EulerZYX(rz, ry, rx), PyKDL.Vector(px, py, pz))
def sampling(nb_pos, nb_rot): pos_min = [0.180, 0.017, -0.150] # workspace for peg transfer pos_max = [0.087, -0.040, -0.117] pos_traj = [] joint_traj = [] roll_st = 0.0 for i in range(nb_pos): pos_rand = np.random.uniform(pos_min, pos_max) for j in range(nb_rot): # select random roll angle roll_ed = np.random.uniform(-90, 90) if roll_st > roll_ed: roll = np.arange(roll_st, roll_ed, step=-10) else: roll = np.arange(roll_st, roll_ed, step=10) rot = [[r, 0.0, 0.0] for r in roll] quat = [U.euler_to_quaternion(r, 'deg') for r in rot] jaw = [0 * np.pi / 180.] for q in quat: joints = dvrk_model.pose_to_joint(pos_rand, q) # dvrk.set_joint(joint1=joints, jaw1=jaw) joints_deg = [ joint * 180. / np.pi if i != 2 else joint for i, joint in enumerate(joints) ] print(i, roll_ed, pos_rand, joints_deg) joint_traj.append(joints) pos_traj.append(pos_rand) return joint_traj, pos_traj
def find_block_servo_handover(self, img_color, img_point): # color masking & transform points to pegboard coordinate pnt_masked = self.mask_color(img_color, img_point, [self.lower_red, self.upper_red]) pnt_masked = self.remove_nan(pnt_masked) pnt_transformed = U.transform(pnt_masked * 0.001, self.Tpc) * 1000 # block image masking by depth pnt_blk = self.mask_depth(pnt_transformed, self.depth_block_servo) # remove outliers # pnt_blk, _ = PCLRegistration.remove_outliers(pnt_blk, nb_points=20, radius=2, visualize=False) pcl_blk, pnt_blk = PCLRegistration.convert(pnt_blk) # registration if len(pnt_blk) < 100: pose_blk = [] pnt_mask = [] else: T = PCLRegistration.registration(pcl_blk, self.pcl_model_servo, downsample=2, use_svr=False, save_image=False, visualize=False) T = np.linalg.inv(T) # transform from model to block blk_ang = self.get_pose(T) pose_blk = [blk_ang, T] pnt_mask = T[:3, :3].dot( self.pnt_model_servo.T).T + T[:3, -1].T # transformed mask points return pose_blk, pnt_blk, pnt_mask
def move_origin(self): # Daniel note: debug positions/orientations by changing origins and exiting after this. print('Moving dvrk to origin, should have closed grippers.') rot_temp1 = (np.array(self.__rot_org1) + np.array(self.__rot_offset1)) * np.pi / 180. rot_temp2 = (np.array(self.__rot_org2) + np.array(self.__rot_offset2)) * np.pi / 180. rot_org1 = U.euler_to_quaternion(rot_temp1) rot_org2 = U.euler_to_quaternion(rot_temp2) jaw_org1 = np.array(self.__jaw_org1) * np.pi / 180. jaw_org2 = np.array(self.__jaw_org2) * np.pi / 180. self.__dvrk.set_pose(self.__pos_org1, rot_org1, self.__pos_org2, rot_org2) print('Changing jaws to: {}, {}'.format(jaw_org1, jaw_org2)) self.__dvrk.set_jaw(jaw_org1, jaw_org2) print('Changed jaws! Proceeding...')
def transfer_block(self, pos_pick, rot_pick, pos_place, rot_place): # open jaw, go down toward block, and close jaw self.dvrk.set_jaw(jaw=self.jaw_opening) self.dvrk.set_pose( pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) self.dvrk.set_jaw(jaw=self.jaw_closing) # trajectory to transferring block from peg to peg q0 = self.dvrk_model.pose_to_joint( pos=[pos_pick[0], pos_pick[1], pos_pick[2] + self.offset_grasp[0]], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qw1 = self.dvrk_model.pose_to_joint( pos=[pos_pick[0], pos_pick[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) qw2 = self.dvrk_model.pose_to_joint( pos=[pos_place[0], pos_place[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) qf = self.dvrk_model.pose_to_joint( pos=[pos_place[0], pos_place[1], self.height_drop], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) J1 = self.dvrk_model.jacobian(qw1) J2 = self.dvrk_model.jacobian(qw2) dvw = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) dqw1 = np.linalg.inv(J1).dot(dvw) dqw2 = np.linalg.inv(J2).dot(dvw) q_pos, t = self.motion_opt_2wp.optimize(q0, qw1, qw2, qf, dqw1, dqw2, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, t_step=0.01, print_out=False, visualize=False) for joint in q_pos[:-1]: self.dvrk.set_joint(joint=joint, wait_callback=False) if self.use_controller: time.sleep(0.005) else: time.sleep(0.01) self.dvrk.set_joint(joint=q_pos[-1], wait_callback=True) self.dvrk.set_jaw(jaw=self.jaw_opening) return q_pos, t
def get_pose(self, transform): if transform == []: pose = [] else: R = transform[:3, :3] euler = np.rad2deg(U.R_to_euler(R)) pose = euler[0] return pose # block angle
def servoing_block(self, pos_place, rot_place, which_arm): if which_arm == 'PSM1' or which_arm == 0: index = 0 obj = self.arm1 elif which_arm == 'PSM2' or which_arm == 1: index = 1 obj = self.arm2 # go down toward block & open jaw obj.set_pose_interpolate( pos=[pos_place[0], pos_place[1], self.height_ready], rot=U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0]))) pos = [pos_place[0], pos_place[1], self.height_drop] rot = U.euler_to_quaternion(np.deg2rad([rot_place, 0.0, 0.0])) obj.set_pose_interpolate(pos=pos, rot=rot) obj.set_jaw_interpolate([self.jaw_opening[index]])
def get_current_jaw(self, unit='rad'): """ :param unit: 'rad' or 'deg' :return: Numpy.float64 """ jaw = np.float64(self.__position_jaw_current) if unit == "deg": jaw = U.rad_to_deg(self.__position_jaw_current) return jaw
def get_poses(self, transform): pose = [] for n, T in enumerate(transform): if T == []: pose.append([n, 0.0, []]) else: R = T[:3, :3] euler = np.rad2deg(U.R_to_euler(R)) pose.append([n, euler[2], T]) return pose # [nb_block, euler angle, T]
def servoing_block(self, pos_place1, rot_place1, pos_place2, rot_place2): # go down toward block & open jaw t1 = threading.Thread( target=self.arm1.set_pose_interpolate, args=([pos_place1[0], pos_place1[1], self.height_ready], U.euler_to_quaternion(np.deg2rad([rot_place1, 0.0, 0.0])))) t2 = threading.Thread( target=self.arm2.set_pose_interpolate, args=([pos_place2[0], pos_place2[1], self.height_ready], U.euler_to_quaternion(np.deg2rad([rot_place2, 0.0, 0.0])))) t1.daemon = True t2.daemon = True t1.start() t2.start() t1.join() t2.join() t3 = threading.Thread( target=self.arm1.set_pose_interpolate, args=([pos_place1[0], pos_place1[1], self.height_drop], U.euler_to_quaternion(np.deg2rad([rot_place1, 0.0, 0.0])))) t4 = threading.Thread( target=self.arm2.set_pose_interpolate, args=([pos_place2[0], pos_place2[1], self.height_drop], U.euler_to_quaternion(np.deg2rad([rot_place2, 0.0, 0.0])))) t3.daemon = True t4.daemon = True t3.start() t4.start() t3.join() t4.join() t5 = threading.Thread(target=self.arm1.set_jaw_interpolate, args=[[self.jaw_opening[0]]]) t6 = threading.Thread(target=self.arm2.set_jaw_interpolate, args=[[self.jaw_opening[1]]]) t5.daemon = True t6.daemon = True t5.start() t6.start() t5.join() t6.join()
def remove_outliers(Rs, Ts): Eulers = np.array([U.R_to_euler(R) for R in Rs]) Ts = np.array(Ts) # for translation clustering = DBSCAN(eps=0.3, min_samples=2).fit(Ts) index = clustering.labels_ # for rotation clustering2 = DBSCAN(eps=0.7, min_samples=2).fit(Eulers) index2 = clustering2.labels_ # cross sectional index index_cross = index + index2 Ts = Ts[index_cross == 0] T = np.average(Ts, axis=0) Euler = Eulers[index_cross == 0] Euler = np.average(Euler, axis=0) R = U.euler_to_R(Euler) return R, T