class SawyerImitation(object): def __init__(self, model_path, vgg19_path): self.ctrl = RobotController() self.recorder = RobotRecorder(save_dir='', seq_len=60, use_aux=False, save_video=True, save_actions=False, save_images=False) self.action_interval = 10 #Hz self.action_sequence_length = 10 self.control_rate = rospy.Rate(self.action_interval) self.predictor = setup_predictor(model_path, vgg19_path) self.save_ctr = 0 self.s = 0 self.imp_ctrl_publisher = rospy.Publisher('desired_joint_pos', JointState, queue_size=1) self.imp_ctrl_release_spring_pub = rospy.Publisher('release_spring', Float32, queue_size=10) self.imp_ctrl_active = rospy.Publisher('imp_ctrl_active', Int64, queue_size=10) self.move_neutral() def move_neutral(self): # self.imp_ctrl_active.publish(0) self.ctrl.set_neutral() # self.set_neutral_with_impedance() # self.imp_ctrl_active.publish(1) # rospy.sleep(.2) def query_action(self, robot_configs): image = cv2.resize(self.recorder.ltob.img_cv2[:-150,150:-275,:], (224, 224), interpolation = cv2.INTER_AREA) if image is None or robot_configs is None: return None action, predicted_eep, image = self.predictor(image, robot_configs) self.img_stack.append(image[0, :, :, :]) self.s += 1 # if self.s <=5: # action[np.abs(action) < 0.05] *= 15 # print 'action vector: ', action # print 'predicted end effector pose: ', predicted_eep return action, predicted_eep def move_to(self, des_pos): desired_pose = inverse_kinematics.get_pose_stamped(des_pos[0], des_pos[1], des_pos[2], inverse_kinematics.EXAMPLE_O) start_joints = self.ctrl.limb.joint_angles() try: des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints, use_advanced_options=True) except ValueError: rospy.logerr('no inverse kinematics solution found, ' 'going to reset robot...') current_joints = self.ctrl.limb.joint_angles() self.ctrl.limb.set_joint_positions(current_joints) raise Traj_aborted_except('raising Traj_aborted_except') # self.move_with_impedance(des_joint_angles) self.ctrl.set_joint_positions(des_joint_angles) def move_with_impedance(self, des_joint_angles): """ non-blocking """ js = JointState() js.name = self.ctrl.limb.joint_names() js.position = [des_joint_angles[n] for n in js.name] self.imp_ctrl_publisher.publish(js) def imp_ctrl_release_spring(self, maxstiff): self.imp_ctrl_release_spring_pub.publish(maxstiff) def move_with_impedance_sec(self, cmd, tsec = 2.): """ blocking """ tstart = rospy.get_time() delta_t = 0 while delta_t < tsec: delta_t = rospy.get_time() - tstart self.move_with_impedance(cmd) def set_neutral_with_impedance(self): neutral_jointangles = [0.412271, -0.434908, -1.198768, 1.795462, 1.160788, 1.107675, 2.068076] cmd = dict(zip(self.ctrl.limb.joint_names(), neutral_jointangles)) self.imp_ctrl_release_spring(20) self.move_with_impedance_sec(cmd) def run_trajectory(self): self.start = self.recorder.get_endeffector_pos() print 'actual end eep', self.start self.move_neutral() self.img_stack = [] step = 0 actions = [] current_eep = self.recorder.get_endeffector_pos() eep_diff_action, predicted_eep = self.query_action(current_eep) predicted_eep[2] = max(predicted_eep[2], 0.25) current_eep[:3] = predicted_eep[:3] self.move_to(current_eep) while step < self.action_sequence_length: print step self.control_rate.sleep() current_eep = self.recorder.get_endeffector_pos() eep_diff_action, predicted_eep = self.query_action(current_eep) self.move_to(current_eep[:3] + eep_diff_action[:3]) step += 1 print 'end', self.recorder.get_endeffector_pos() clip = mpy.ImageSequenceClip([cv2.cvtColor(i, cv2.COLOR_BGR2RGB) for i in self.img_stack], fps=20) clip.write_gif('test_frames.gif') self.move_neutral()
class SawyerOneShot(object): EE_STEPS = 2 ACTION_SEQUENCE_LENGTH = 10 SPRING_STIFF = 240 CONTROL_RATE = 10000 PREDICT_RATE = 20 UI_RATE = 1 CROP_H_MIN = 0 CROP_H_MAX = -150 CROP_W_MIN = 190 CROP_W_MAX = -235 TRAJ_LEN = 40 HUMAN_MODEL = 'to_test/model_human_maml/place_sawyer_maml_3_layers_200_dim_1d_conv_ee_3_32_act_2_32_20x1_filters_64_128_3x3_filters_human_light_68k.meta' HUMAN_BIAS = 'to_test/model_human_maml/scale_and_bias_place_sawyer_kinect_view_human.pkl' ROBOT_MODEL = 'to_test/model_maml_kinect2_2/place_sawyer_maml_3_layers_200_dim_ee_2_layers_100_dim_clip_20_kinect_view_47k.meta' ROBOT_BIAS = 'to_test/model_maml_kinect2_2/scale_and_bias_place_sawyer_kinect_view_both.pkl' Z_SAFETY_THRESH = 0.20 def __init__(self): self.ctrl = RobotController(self.CONTROL_RATE) self.recorder = RobotRecorder(save_dir='', use_aux=False, save_actions=False, save_images=False) self.image_publisher = rospy.Publisher('/robot/head_display', Image_msg, queue_size=2) self.load_splashes() self.control_rate = rospy.Rate(self.PREDICT_RATE) self.ui_rate = rospy.Rate(self.UI_RATE) self.robot_predictor = setup_predictor( self.ROBOT_MODEL, self.ROBOT_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.human_predictor = setup_predictor_human( self.HUMAN_MODEL, self.HUMAN_BIAS, self.CROP_H_MIN, self.CROP_H_MAX, self.CROP_W_MIN, self.CROP_W_MAX) self.is_human = False self.predictor = self.robot_predictor rospy.Subscriber("/keyboard/keyup", Key, self.keyboard_up_listener) self.weiss_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10) # rospy.Subscriber("/wsg_50_driver/status", Status, self.weiss_status_listener) self._navigator = intera_interface.Navigator() self.demo_collect = self._navigator.register_callback( self.record_demo, 'right_button_ok') self.swap_key = self._navigator.register_callback( self.swap_sawyer_cuff, 'right_button_back') self.start_key = self._navigator.register_callback( self.start_traj_cuff, 'right_button_square') self.neutral_key = self._navigator.register_callback( self.neutral_cuff, 'right_button_show') self.x_button = self._navigator.register_callback( self.gripper_cuff, 'right_button_triangle') self.calc = EE_Calculator() print 'ONE SHOT READY!' self.running = False self.demo_imgs = None self.move_netural() rospy.spin() def publish_to_head(self, img): num_rows, num_cols = img.shape[:2] rotation_matrix = np.array([[ 0., 1., 0., ], [ -1., 0., 1024., ]]) img = cv2.warpAffine(img, rotation_matrix, (num_rows, num_cols))[:, ::-1, :] img = np.swapaxes(img, 0, 1)[::-1, ::-1, :] #I don't know why the transforms above work either img_message = self.recorder.bridge.cv2_to_imgmsg(img) self.image_publisher.publish(img_message) def gripper_cuff(self, value): if self.running or not value: return print value self.set_weiss_griper(10.) def neutral_cuff(self, value): if self.running or not value: return self.move_netural() def load_splashes(self): self.human_splash = cv2.imread('splashes/human_imitation_start.png') self.robot_splash = cv2.imread('splashes/robot_imitation_start.png') self.demo_splash = cv2.imread('splashes/recording_oneshot.png') #make review splash self.publish_to_head(self.robot_splash) def swap_model(self): if self.is_human: self.publish_to_head(self.robot_splash) self.predictor = self.robot_predictor self.is_human = False else: self.predictor = self.human_predictor self.is_human = True self.publish_to_head(self.human_splash) self.demo_imgs = None def swap_sawyer_cuff(self, value): if not value or self.running: return self.swap_model() def start_traj_cuff(self, value): if not value or self.running: return if self.demo_imgs is None: print "PLEASE COLLECT DEMOS FIRST" return self.run_trajectory() def record_demo(self, value): if not value or self.running: return print "beginning demo!" self.running = True self.demo_imgs = [] traj_ee_pos = np.zeros((self.TRAJ_LEN, 7)) traj_ee_velocity = np.zeros((self.TRAJ_LEN, 6)) full_duration = float(self.TRAJ_LEN) / self.PREDICT_RATE for i in range(4, 0, -1): splash = np.copy(self.demo_splash) if i > 1: cv2.putText(splash, "{}...".format(i - 1), (375, 460), cv2.FONT_HERSHEY_SIMPLEX, 6, (255, 255, 255), 20, cv2.LINE_AA) self.publish_to_head(splash) self.ui_rate.sleep() else: cv2.putText(splash, "GO!", (375, 460), cv2.FONT_HERSHEY_SIMPLEX, 6, (255, 255, 255), 20, cv2.LINE_AA) self.publish_to_head(splash) for i in range(5): self.control_rate.sleep() for i in range(self.TRAJ_LEN): splash = np.copy(self.demo_splash) cv2.putText( splash, "{:.2f}s".format(full_duration - i * 1. / self.PREDICT_RATE), (250, 460), cv2.FONT_HERSHEY_SIMPLEX, 6, (255, 255, 255), 20, cv2.LINE_AA) self.publish_to_head(splash) self.control_rate.sleep() self.demo_imgs.append(self.recorder.ltob.img_cv2) traj_ee_pos[i, :] = self.recorder.get_endeffector_pos() angles = self.recorder.get_joint_angles() velocities = self.recorder.get_joint_angles_velocity() traj_ee_velocity[i, :] = self.calc.jacobian( angles.reshape(-1)).dot(velocities.reshape( (-1, 1))).reshape(-1) splash = np.copy(self.demo_splash) cv2.putText(splash, "{:.2f}s".format(0.00), (250, 460), cv2.FONT_HERSHEY_SIMPLEX, 6, (255, 255, 255), 20, cv2.LINE_AA) self.publish_to_head(splash) final_ee = np.tile(traj_ee_pos[-1, :2], (40, 1)) self.record_state = traj_ee_pos self.record_action = np.concatenate( (traj_ee_velocity[:, :3], final_ee), axis=1) self.demo_imgs = np.stack([ cv2.resize(img[self.CROP_H_MIN:self.CROP_H_MAX, self.CROP_W_MIN:self.CROP_W_MAX, :], (100, 100), interpolation=cv2.INTER_AREA)[:, :, ::-1] for img in self.demo_imgs ], axis=0) for i in self.demo_imgs: splash = np.copy(self.demo_splash) resized_i = cv2.resize(i, (400, 400), interpolation=cv2.INTER_CUBIC) splash[190:590, 316:716, :] = resized_i[:, :, ::-1] self.publish_to_head(splash) cv2.imshow('img', i[:, :, ::-1]) cv2.waitKey(200) cv2.destroyAllWindows() self.running = False print "DEMO DONE" if self.is_human: self.publish_to_head(self.human_splash) else: self.publish_to_head(self.robot_splash) def keyboard_up_listener(self, key_msg): if key_msg.code == 99 and not self.running: #c if self.demo_imgs is None: print "PLEASE COLLECT A DEMO FIRST" return self.run_trajectory() if key_msg.code == 101 and not self.running: #e rospy.signal_shutdown('User shutdown!') if key_msg.code == 110 and not self.running: #n self.move_netural() if key_msg.code == 115 and not self.running: #s self.swap_model() if key_msg.code == 103 and not self.running: #g self.set_weiss_griper(10.) def query_action(self): image = cv2.resize( self.recorder.ltob.img_cv2[self.CROP_H_MIN:self.CROP_H_MAX, self.CROP_W_MIN:self.CROP_W_MAX, :], (100, 100), interpolation=cv2.INTER_AREA)[:, :, ::-1] robot_configs = self.recorder.get_endeffector_pos() if image is None or robot_configs is None: return None action, ee = self.predictor(self.demo_imgs, self.record_state, self.record_action, image, robot_configs) self.img_stack.append(image) return action, ee def move_to(self, des_pos, interp=True): desired_pose = inverse_kinematics.get_pose_stamped( des_pos[0], des_pos[1], des_pos[2], inverse_kinematics.EXAMPLE_O) start_joints = self.ctrl.limb.joint_angles() try: des_joint_angles = inverse_kinematics.get_joint_angles( desired_pose, seed_cmd=start_joints, use_advanced_options=True) except ValueError: rospy.logerr('no inverse kinematics solution found, ' 'going to reset robot...') current_joints = self.recorder.get_joint_angles() des_joint_angles = current_joints if interp: self.ctrl.set_joint_positions_interp(des_joint_angles) else: self.ctrl.limb.set_joint_position_speed(0.15) self.ctrl.set_joint_positions(des_joint_angles) def set_weiss_griper(self, width): cmd = Cmd() cmd.pos = width cmd.speed = 100. self.weiss_pub.publish(cmd) def move_netural(self, gripper_open=True): self.ctrl.set_neutral() if gripper_open: self.set_weiss_griper(100.) def run_trajectory(self): self.running = True self.start = self.recorder.get_endeffector_pos() print 'actual end eep', self.start self.move_netural(gripper_open=False) self.img_stack = [] step = 0 actions = [] for i in range(self.EE_STEPS): current_eep = self.recorder.get_endeffector_pos() eep_diff_action, pred_final = self.query_action() current_eep[:2] = pred_final # current_eep[2] += np.sum(np.abs(current_eep[:2])) * 0.05 self.move_to(current_eep[:3], i > 0) while step < self.ACTION_SEQUENCE_LENGTH: current_eep = self.recorder.get_endeffector_pos() eep_diff_action, pred_final = self.query_action() current_eep[:3] += 0.05 * eep_diff_action current_eep[2] = max(current_eep[2], self.Z_SAFETY_THRESH) self.move_to(current_eep[:3]) step += 1 self.set_weiss_griper(100.) print 'end', self.recorder.get_endeffector_pos() clip = mpy.ImageSequenceClip([i for i in self.img_stack], fps=20) clip.write_gif('test_frames.gif') self.running = False
class SawyerImitation(object): def __init__(self, model_path, vgg19_path): self.ctrl = RobotController() self.recorder = RobotRecorder(save_dir='', seq_len=60, use_aux=False, save_video=True, save_actions=False, save_images=False) self.action_interval = 20 #Hz self.action_sequence_length = 60 self.traj_duration = self.action_sequence_length * self.action_interval self.action_rate = rospy.Rate(self.action_interval) self.control_rate = rospy.Rate(20) self.predictor = setup_predictor(model_path, vgg19_path) self.save_ctr = 0 self.ctrl.set_neutral() self.s = 0 def query_action(self): image = cv2.resize(self.recorder.ltob.img_cv2[:-150,275:-170,:], (224, 224), interpolation=cv2.INTER_AREA) robot_configs = np.concatenate((self.recorder.get_joint_angles(), self.recorder.get_endeffector_pos()[:3])) if image is None or robot_configs is None: return None action, predicted_eep = self.predictor(image, robot_configs) self.img_stack.append(image) self.s += 1 # if self.s <=5: # action[np.abs(action) < 0.05] *= 15 # print 'action vector: ', action # print 'predicted end effector pose: ', predicted_eep return action, predicted_eep def apply_action(self, action): try: self.ctrl.set_joint_velocities(action) except OSError: rospy.logerr('collision detected, stopping trajectory, going to reset robot...') rospy.sleep(.5) if self.ctrl.limb.has_collided(): rospy.logerr('collision detected!!!') rospy.sleep(.5) def move_to(self, des_pos): desired_pose = inverse_kinematics.get_pose_stamped(des_pos[0], des_pos[1], des_pos[2], inverse_kinematics.EXAMPLE_O) start_joints = self.ctrl.limb.joint_angles() try: des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints, use_advanced_options=True) except ValueError: rospy.logerr('no inverse kinematics solution found, ' 'going to reset robot...') current_joints = self.ctrl.limb.joint_angles() self.ctrl.limb.set_joint_positions(current_joints) raise Traj_aborted_except('raising Traj_aborted_except') # self.move_with_impedance(des_joint_angles) self.ctrl.set_joint_positions(des_joint_angles) def run_trajectory(self): start = self.recorder.get_endeffector_pos()[:3] # print 'actual end eep', self.start self.ctrl.set_neutral() self.img_stack = [] step = 0 actions = [] action, predicted_eep = self.query_action() print 'predicted eep', predicted_eep[:2] # print 'diff sqd', np.sum(np.power(predicted_eep - start, 2)) # print 'diff dim', np.abs(predicted_eep - start) start[:2] = predicted_eep[:2] self.move_to(start) while step < self.action_sequence_length: self.control_rate.sleep() action, predicted_eep = self.query_action() action_dict = dict(zip(self.ctrl.joint_names, action)) # for i in range(len(self.ctrl.joint_names)): # action_dict[self.ctrl.joint_names[i]] = action[i] # print 'key', self.ctrl.joint_names[i], 'value', action_dict[self.ctrl.joint_names[i]] actions.append(action) self.apply_action(action_dict) step += 1 print 'end', self.recorder.get_endeffector_pos() clip = mpy.ImageSequenceClip([cv2.cvtColor(i, cv2.COLOR_BGR2RGB) for i in self.img_stack], fps=20) clip.write_gif('test_frames.gif')