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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 transfer_block(self, pos_pick, rot_pick, pos_place, rot_place): # open jaw self.arm1.set_jaw_interpolate(self.jaw_opening) # go down toward block & close jaw self.arm1.set_pose_interpolate(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.arm1.set_jaw_interpolate(self.jaw_closing) # transfer block from peg to peg q_pos = self.transfer_block_traj(pos_pick, rot_pick, pos_place, rot_place, self.use_optimization, self.optimizer) self.move_trajectory(self.arm1, q_pos)
def drop_block(self, pos1, rot1, pos2, rot2): # be ready to place with jaw closing 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])))) t1.daemon = True t2.daemon = True t1.start() t2.start() t1.join() t2.join() # go down toward peg t3 = threading.Thread(target=self.arm1.set_pose_interpolate, args=([pos1[0], pos1[1], self.height_drop], U.euler_to_quaternion( np.deg2rad([rot1, 0.0, 0.0])))) t4 = threading.Thread(target=self.arm2.set_pose_interpolate, args=([pos2[0], pos2[1], self.height_drop], U.euler_to_quaternion( np.deg2rad([rot2, 0.0, 0.0])))) t3.daemon = True t4.daemon = True t3.start() t4.start() t3.join() t4.join() # # open jaw t5 = threading.Thread(target=self.arm1.set_jaw, args=[[self.jaw_opening[0]]]) t6 = threading.Thread(target=self.arm2.set_jaw, args=[[self.jaw_opening[1]]]) t5.daemon = True t6.daemon = True t5.start() t6.start() t5.join() t6.join() # go up t7 = 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])))) t8 = 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])))) t7.daemon = True t8.daemon = True t7.start() t8.start() t7.join() t8.join()
def move_block(self, grasping_pose1, placing_pose1, pick_number1, place_number1, grasping_pose2, placing_pose2, pick_number2, place_number2): pos_pick1 = grasping_pose1[pick_number1 - 1][1:] rot_pick1 = [ -grasping_pose1[pick_number1 - 1][0], 0, 0 ] # (-) is added because the direction of pose perception and motion is oppoiste. pos_place1 = placing_pose1[place_number1 - 1][1:] rot_place1 = [-placing_pose1[place_number1 - 1][0], 0, 0] pos_pick2 = grasping_pose2[pick_number2 - 1][1:] rot_pick2 = [-grasping_pose2[pick_number2 - 1][0], 0, 0] pos_place2 = placing_pose2[place_number2 - 1][1:] rot_place2 = [-placing_pose2[place_number2 - 1][0], 0, 0] # conversion to quaternion q_pick1 = U.euler_to_quaternion(rot_pick1) q_place1 = U.euler_to_quaternion(rot_place1) q_pick2 = U.euler_to_quaternion(rot_pick2) q_place2 = U.euler_to_quaternion(rot_place2) self.__dvrk.pickup(pos_pick1, q_pick1, pos_pick2, q_pick2, 'PSM2') self.__dvrk.place(pos_place1, q_place1, pos_place2, q_place2, 'PSM2')
def return_to_peg(self, pos_pick, rot_pick): p0, quat0 = self.dvrk.get_current_pose() self.dvrk.set_jaw(jaw=self.jaw_closing) # trajectory of returning to another peg to pick-up q0 = self.dvrk.get_current_joint(wait_callback=True) qw1 = self.dvrk_model.pose_to_joint( pos=[p0[0], p0[1], self.height_ready - 0.01], rot=quat0) qw2 = 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]))) 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) return q_pos, t
def go_handover(self, pos_pick, rot_pick, pos_place, rot_place, which_arm): if which_arm == 'PSM1': obj = self.arm1 if self.optimizer == 'cubic': obj_opt = self.motion_opt_1wp_PSM1 elif self.optimizer == 'qp': obj_opt = self.motion_opt index = 0 elif which_arm == 'PSM2': obj = self.arm2 if self.optimizer == 'cubic': obj_opt = self.motion_opt_1wp_PSM2 elif self.optimizer == 'qp': obj_opt = self.motion_opt index = 1 else: raise ValueError # open jaw obj.set_jaw_interpolate([self.jaw_opening[index]]) # go down toward block & close jaw obj.set_pose_interpolate(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]))) obj.set_jaw_interpolate([self.jaw_closing[index]]) # pick-up and handover q_pos = self.go_handover_traj(obj_opt, pos_pick, rot_pick, pos_place, rot_place, self.use_optimization, self.optimizer, which_arm) if which_arm == 'PSM1': t1 = threading.Thread(target=self.move_trajectory, args=(obj, q_pos)) elif which_arm == 'PSM2': t1 = threading.Thread(target=self.move_trajectory2, args=(obj, q_pos)) t2 = threading.Thread(target=obj.set_jaw_interpolate, args=[[self.jaw_closing[index]]]) t1.daemon = True t2.daemon = True t1.start() t2.start() t1.join() t2.join()
def insertion_sampling(dvrk_model, insertion_number): # pos_low_min_z = -0.170 # pos_low_max_z = -0.135 # pos_high_min = [0.02, -0.08, -0.115] # pos_high_max = [0.18, 0.08, -0.095] pos_org = [0.055, 0.0, -0.113] rot_org = [0.0, 0.0, 0.0, 1.0] pos_min = [0.170, 0.027, -0.145] pos_max = [0.082, -0.051, -0.106] height_ready = [-0.115, -0.095] height_block = [-0.155, -0.135] traj_pos = [] traj_joint = [] for i in range(insertion_number): pos_above = np.random.uniform(pos_min, pos_max) pos_above[2] = np.random.uniform(height_ready[0], height_ready[1]) 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]) # 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]) # 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]) # 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]) return traj_pos, traj_joint
def move_upright(self, pos, rot, jaw='close', which_arm='PSM1', interpolate=False): 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 pos = [pos[0], pos[1], pos[2]] if rot == []: pass else: rot = U.euler_to_quaternion(np.deg2rad([rot, 0.0, 0.0])) if jaw == 'close': jaw = self.jaw_closing[index] elif jaw == 'open': jaw = self.jaw_opening[index] else: raise ValueError if interpolate: t1 = threading.Thread(target=obj.set_pose_interpolate, args=(pos, rot)) else: t1 = threading.Thread(target=obj.set_pose_interpolate, args=(pos, rot)) t2 = threading.Thread(target=obj.set_jaw_interpolate, args=[[jaw]]) t1.daemon = True t2.daemon = True t1.start() t2.start() t1.join() t2.join()
def transfer_block(self, pos_pick1, rot_pick1, pos_place1, rot_place1, pos_pick2, rot_pick2, pos_place2, rot_place2): # open jaw t3 = threading.Thread(target=self.arm1.set_jaw_interpolate, args=[[self.jaw_opening[0]]]) t4 = threading.Thread(target=self.arm2.set_jaw_interpolate, args=[[self.jaw_opening[1]]]) t3.daemon = True t4.daemon = True t3.start() t4.start() t3.join() t4.join() # go down toward block & close jaw t5 = threading.Thread( target=self.arm1.set_pose_interpolate, args=([ pos_pick1[0], pos_pick1[1], pos_pick1[2] + self.offset_grasp[0] ], U.euler_to_quaternion(np.deg2rad([rot_pick1, 0.0, 0.0])))) t6 = threading.Thread( target=self.arm2.set_pose_interpolate, args=([ pos_pick2[0], pos_pick2[1], pos_pick2[2] + self.offset_grasp[0] ], U.euler_to_quaternion(np.deg2rad([rot_pick2, 0.0, 0.0])))) t5.daemon = True t6.daemon = True t5.start() t6.start() t5.join() t6.join() t7 = threading.Thread(target=self.arm1.set_jaw_interpolate, args=[[self.jaw_closing[0]]]) t8 = threading.Thread(target=self.arm2.set_jaw_interpolate, args=[[self.jaw_closing[1]]]) t7.daemon = True t8.daemon = True t7.start() t8.start() t7.join() t8.join() # transfer block from peg to peg q_pos1 = self.transfer_block_traj(pos_pick1, rot_pick1, pos_place1, rot_place1, self.use_optimization, self.optimizer) q_pos2 = self.transfer_block_traj(pos_pick2, rot_pick2, pos_place2, rot_place2, self.use_optimization, self.optimizer) t1 = threading.Thread(target=self.move_trajectory, args=(self.arm1, q_pos1)) t2 = threading.Thread(target=self.move_trajectory, args=(self.arm2, q_pos2)) t3 = threading.Thread(target=self.arm1.set_jaw_interpolate, args=[[self.jaw_closing[0]]]) t4 = threading.Thread(target=self.arm2.set_jaw_interpolate, 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()
self.des_pos1 = [] self.des_rot1 = [] self.des_jaw1 = [] self.des_pos2 = [] self.des_rot2 = [] self.des_jaw2 = [] self.des_joint1 = [] self.des_joint2 = [] self.input_flag = [True, True, True, True, True, True, True, True] if __name__ == "__main__": perception = dvrkMotionBridgeP() pos1 = [0.1, 0.1, -0.13] rot1 = [-0.3, -0.4, -0.3] q1 = U.euler_to_quaternion(rot1, unit='deg') jaw1 = [5 * np.pi / 180.] # perception.set_pose(pos1=pos1, rot1=q1, jaw1=jaw1) perception.set_pose(jaw1=jaw1) pos2 = [0.0, 0.0, -0.13] rot2 = [0.0, 0.0, 0.0] q2 = U.euler_to_quaternion(rot2, unit='deg') jaw2 = [0.0 * np.pi / 180.] # perception.set_pose(jaw1=jaw1) # joint1 = [0.8, .7, 0.12, 0.0, -0.5, 0.0] # joint2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # perception.set_joint(joint1=joint1) # t = 0.0 # import time # while True:
from FLSpegtransfer.motion.dvrkKinematics import dvrkKinematics 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)
10) and self.arm2._dvrkArm__goal_reached_event.wait( 10) # 10 seconds at most else: self.arm1._dvrkArm__set_position_goal_joint_pub.publish(msg1) self.arm2._dvrkArm__set_position_goal_joint_pub.publish(msg2) return True if __name__ == "__main__": p = dvrkDualArm() import numpy as np while True: pos11 = [-0.1, -0.1, -0.14] rot11 = np.array([0, -60, -60 ]) * np.pi / 180. # ZYX Euler angle in (deg) q11 = U.euler_to_quaternion(rot11) jaw11 = [0] pos12 = [-0.05, -0.05, -0.14] rot12 = np.array([0, -60, -60 ]) * np.pi / 180. # ZYX Euler angle in (deg) q12 = U.euler_to_quaternion(rot12) jaw12 = [0] p.set_pose(pos1=pos11, rot1=q11, pos2=pos12, rot2=q12) # p.set_pose(pos1=pos11, rot1=q11) p.set_jaw(jaw1=jaw11, jaw2=jaw12) # p.set_jaw(jaw2=jaw12) pos21 = [0.1, 0.1, -0.14] rot21 = np.array([0, 60, 60 ]) * np.pi / 180. # ZYX Euler angle in (deg) q21 = U.euler_to_quaternion(rot21)
pos3 = pose3[:3] opt = PegMotionOptimizer() pos, vel, acc, t = opt.optimize_motion(pose1, pose2, pose3, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, t_step=0.01, horizon=50, print_out=True, visualize=False) from FLSpegtransfer.motion.dvrkDualArm import dvrkDualArm import time dvrk = dvrkDualArm() while True: q = U.euler_to_quaternion([pose1[3], 0.0, 0.0]) dvrk.set_pose(pos1=pose1[:3], rot1=q) dvrk.set_jaw(jaw1=[0.5]) # time.sleep(2) for pose in pos: q = U.euler_to_quaternion([pose[3], 0.0, 0.0]) dvrk.set_pose(pos1=pose[:3], rot1=q, wait_callback=False) time.sleep(0.01) # time.sleep(2) q = U.euler_to_quaternion([pose3[3], 0.0, 0.0]) dvrk.set_pose(pos1=pose3[:3], rot1=q) dvrk.set_jaw(jaw1=[0.5])