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 go_pick_traj(self, obj, obj_opt, pos_pick, rot_pick, optimized, optimizer, which_arm): self.lock.acquire() 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[which_arm][1] ], rot=U.euler_to_quaternion(np.deg2rad([rot_pick, 0.0, 0.0]))) if optimizer == 'cubic': q_pos, _ = obj_opt.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[which_arm][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)) self.lock.release() return q_pos
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 set_pose_interpolate(self, pos, rot, tf_init=0.5, t_step=0.01): """ :param pos: [x,y,z] :param rot: [qx,qy,qz,qw] :param tf_init: initial guess to be minimized :param t_step: :return: """ assert not np.isnan(np.sum(pos)) assert not np.isnan(np.sum(rot)) # Define q0 and qf pos0, rot0 = self.get_current_pose(wait_callback=True) if len(pos)==0: posf = pos0 else: posf = pos if len(rot)==0: rotf = rot0 else: rotf = rot rot0 = U.quaternion_to_euler(rot0) rotf = U.quaternion_to_euler(rotf) pose0 = np.concatenate((pos0, rot0)) posef = np.concatenate((posf, rotf)) # Define trajectory if np.allclose(pose0, posef): return False else: _, traj = self.cubic_cartesian(pose0, posef, vel_limit=dvrkVar.v_max, acc_limit=dvrkVar.a_max, tf_init=tf_init, t_step=0.01) # Execute trajectory for q in traj: self.set_joint_direct(q) rospy.sleep(t_step) return True
def go_place_traj(self, obj, obj_opt, pos_place, rot_place, optimized): self.lock.acquire() # if optimized: # # trajectory to pick # q0 = obj.get_current_joint() # qw = 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]))) # J = dvrkKinematics.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, _ = obj_opt.optimize(q0, qw, qf, dqw, max_vel=dvrkVar.v_max, max_acc=dvrkVar.a_max, # t_step=0.01, print_out=False, visualize=False) # else: pos0, quat0 = obj.get_current_pose() rot0 = U.quaternion_to_euler(quat0) pose0 = np.concatenate((pos0, rot0)) posw = [pos_place[0], pos_place[1], self.height_ready] rotw = [np.deg2rad(rot_place), 0.0, 0.0] posew = np.concatenate((posw, rotw)) # 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, 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)) q_pos = q_pos1 self.lock.release() return q_pos
def set_pose_interpolate(self, pos1=[], rot1=[], pos2=[], rot2=[], method='cubic'): # Define q0 and qf for arm 1 pos10, rot10 = self.arm1.get_current_pose(wait_callback=True) if len(pos1) == 0: pos1f = pos10 else: pos1f = pos1 if len(rot1) == 0: rot1f = rot10 else: rot1f = rot1 rot10 = U.quaternion_to_euler(rot10) rot1f = U.quaternion_to_euler(rot1f) q10 = np.concatenate((pos10, rot10)) q1f = np.concatenate((pos1f, rot1f)) # Define q0 and qf for arm 2 pos20, rot20 = self.arm2.get_current_pose(wait_callback=True) if len(pos2) == 0: pos2f = pos20 else: pos2f = pos2 if len(rot2) == 0: rot2f = rot20 else: rot2f = rot2 rot20 = U.quaternion_to_euler(rot20) rot2f = U.quaternion_to_euler(rot2f) q20 = np.concatenate((pos20, rot20)) q2f = np.concatenate((pos2f, rot2f)) # Define trajectory v_max = [0.1, 0.1, 0.1, 1.0, 1.0, 1.0] a_max = [0.1, 0.1, 0.1, 1.0, 1.0, 1.0] if method == 'cubic': time1, traj1 = self.arm1.cubic(q10, q1f, v_max=v_max, a_max=a_max, t_step=0.01) time2, traj2 = self.arm2.cubic(q20, q2f, v_max=v_max, a_max=a_max, t_step=0.01) elif method == 'LSPB': time1, traj1 = self.arm1.LSPB(q10, q1f, v_max=v_max, a_max=a_max, t_step=0.01) time2, traj2 = self.arm2.LSPB(q20, q2f, v_max=v_max, a_max=a_max, t_step=0.01) else: raise IndexError # Execute trajectory len1 = len(traj1) len2 = len(traj2) len_max = max(len1, len2) traj_add1 = np.repeat([traj1[-1]], len_max-len1, axis=0) traj_add2 = np.repeat([traj2[-1]], len_max-len2, axis=0) traj1 = np.concatenate((traj1, traj_add1), axis=0) traj2 = np.concatenate((traj2, traj_add2), axis=0) for i, (q1, q2) in enumerate(zip(traj1, traj2)): super().set_pose(pos1=q1[:3], rot1=U.euler_to_quaternion(q1[3:]), pos2=q2[:3], rot2=U.euler_to_quaternion(q2[3:]), wait_callback=False) if self.stop_collision: if self.detect_collision(): # when collide, temporary make the threshold higher to escape in next move. self.curr_threshold = self.curr_threshold_high return False time.sleep(0.01) # wait until goal reached super().set_pose(pos1=traj1[-1][:3], rot1=U.euler_to_quaternion(traj1[-1][3:]), pos2=traj2[-1][:3], rot2=U.euler_to_quaternion(traj2[-1][3:]), wait_callback=True) self.curr_threshold = self.curr_threshold_low return True