def __init__(self, hyperparams, init_node=True): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT_ROS) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_ros_node') self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] r = rospy.Rate(1) r.sleep()
def __init__(self, hyperparams, init_node=True): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT_ROS) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_ros_node') self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] r = rospy.Rate(1) r.sleep() self.use_tf = False self.observations_stale = True
def __init__(self, hyperparams, init_node=True): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT_LAIKA_ROS) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_Laika_agent_ros_node') self.pub_cmd = rospy.Publisher('cmd', LaikaCommand, queue_size=1) self.pub_act = rospy.Publisher('action', LaikaAction, queue_size=1) self.sub_state = rospy.Subscriber('state', LaikaStateArray, callback=self.handle_state_msg, queue_size=1) self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] self._hyperparams['x0'] = setup(self._hyperparams['x0'], conditions) self.x0 = self._hyperparams['x0'] r = rospy.Rate(100) r.sleep() self.curr_state = np.zeros(140) #hardcoded dimensions for now, change later self.prev_state = np.zeros(140) self.prev2_state = np.zeros(140) self.state_update = False self.last_state_msg_time = 0 self.threading_cond = threading.Condition()
def __init__(self, hyperparams): """Initializes the agent. Args: hyperparams: Dictionary of hyperparameters. """ config = copy.deepcopy(AGENT_ROS_JACO) config.update(hyperparams) Agent.__init__(self, config) self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] self.dt = self._hyperparams['dt'] self.ee_points = hyperparams["ee_points"] self.ee_points_tgt = self._hyperparams['ee_points_tgt'] self.scaler = self._hyperparams.get('scaler', None) # EE Jacobian self.jac = JAC if self.scaler is not None: # Scale jacobians self.jac[:3] *= self.scaler.scale_[:6].reshape(1, 6) self.jac[:3] /= self.scaler.scale_[-9:-6].reshape(3, 1)
def _setup_conditions(self): """ Helper method for setting some hyperparameters that may vary by condition. """ conds = self._hyperparams['conditions'] for field in ('x0', 'x0var', 'pos_body_idx', 'pos_body_offset', 'noisy_body_idx', 'noisy_body_var'): self._hyperparams[field] = setup(self._hyperparams[field], conds)
def _setup_conditions(self): """ Helper method for setting some hyperparameters that may vary by condition. """ conds = self._hyperparams['conditions'] for field in ('x0', 'x0var', 'pos_body_idx', 'pos_body_offset', 'noisy_body_idx', 'noisy_body_var', 'filename'): self._hyperparams[field] = setup(self._hyperparams[field], conds)
def __init__(self, hyperparams, init_node=True): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT_ROS_JACO) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_ros_node') self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] #self.x_tgt = self._hyperparams['exp_x_tgts'] self.target_state = np.zeros(self.dX) self.dt = self._hyperparams['dt'] self.gui = None self.condition = None self.policy = None r = rospy.Rate(1) r.sleep() self.stf_policy = None self.init_tf = False self.use_tf = False self.observations_stale = True self.noise = None self.cur_timestep = None self.vision_enabled = False img_width = self._hyperparams['rgb_shape'][0] img_height = self._hyperparams['rgb_shape'][1] img_channel = self._hyperparams['rgb_shape'][2] self.rgb_image = np.empty([img_height, img_width, img_channel]) self.rgb_image_seq = np.empty( [self.T, img_height, img_width, img_channel], dtype=np.uint8) self.sample_processing = False self.sample_save = False self._cv_bridge = CvBridge()
def __init__(self, hyperparams): """Initialized Agent. hyperparams: Dictionary of hyperparameters.""" # Pull parameters from hyperparams file. config = {} config.update(hyperparams) Agent.__init__(self, config) conditions = self._hyperparams['conditions'] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] self.color_log = utils.TextColors()
def _setup_conditions(self): """ Helper method for setting some hyperparameters that may vary by condition. """ # TODO: modify the cond fields conds = self._hyperparams['conditions'] self._hyperparams['x0'] = setup(self._hyperparams['x0'], conds) def _setup_env(self, env, target_state, render, polygons, map_size, bodies): """ Helper method for handling setup of the Box2D world. """ self.x0 = self._hyperparams["x0"] # initial state self._envs = [env(self.x0[i], target_state, render, polygons, map_size, bodies) for i in range(self._hyperparams['conditions'])]
def __init__(self, hyperparams): """Initializes the agent. Args: hyperparams: Dictionary of hyperparameters. """ config = copy.deepcopy(AGENT_PANDA) config.update(hyperparams) Agent.__init__(self, config) self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] self.dt = self._hyperparams['dt'] self.ee_points = hyperparams["ee_points"] self.ee_points_tgt = self._hyperparams['ee_points_tgt']
def __init__(self, hyperparams, init_node=False): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ # plt.ion() plt.ioff() config = copy.deepcopy(AGENT_ROS) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_ros_node', anonymous=True) # self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] self.use_tf = False self.observations_stale = True # Baxter specific limb = 'left' self._trial_limb = limb self._trial_arm = baxter_interface.Limb(self._trial_limb) self._trial_gripper = baxter_interface.Gripper( self._trial_limb, baxter_interface.CHECK_VERSION) self._auxiliary_limb = 'right' self._auxiliary_arm = baxter_interface.Limb(self._auxiliary_limb) self._auxiliary_gripper = baxter_interface.Gripper( self._auxiliary_limb, baxter_interface.CHECK_VERSION) print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) self.joint_names = self._trial_arm.joint_names() print(self.joint_names) self.n_joints = len(self.joint_names) self.n_emb = self._hyperparams['sensor_dims'][TCN_EMBEDDING] self._joint_idx = list(range(self.n_joints)) self._vel_idx = [i + self.n_joints for i in self._joint_idx] self._pos_idx = [i + 2 * self.n_joints for i in range(3)] self._emb_idx = [i + 3 + 2 * self.n_joints for i in range(self.n_emb)] self.planner = BaxterInterfacePlanner(limb) self._rate = 1.0 / self._hyperparams['dt'] self._control_rate = rospy.Rate(self._rate) self._missed_cmds = 20.0 self._trial_arm.set_command_timeout( (1.0 / self._rate) * self._missed_cmds) self._kin_aux = baxter_kinematics('right') self._kin_trial = baxter_kinematics('left') self.views_ids = self._hyperparams['views'] self.taskspace_deltas = self._hyperparams['taskspace_deltas'] topic_img_list = [ '/camera' + view_id + '/color/image_raw' for view_id in self.views_ids ] topic_depth_list = [ '/camera' + view_id + '/aligned_depth_to_color/image_raw' for view_id in self.views_ids ] # pub = rospy.Publisher('/tcn/embedding', numpy_msg(Floats), queue_size=3) self.img_subs_list = [ img_subscriber(topic=topic_img) for topic_img in topic_img_list ] self.depth_subs_list = [ depth_subscriber(topic=topic_depth) for topic_depth in topic_depth_list ] # Initialize Mask RCNN model self.debug_mode = self._hyperparams['debug_mode'] self.plot_mode = True model_path = self._hyperparams['model_path'] MODEL_DIR = '/home/msieb/projects/gps/experiments/baxter_reaching/data_files' class InferenceConfig(BaxterConfig): GPU_COUNT = 1 IMAGES_PER_GPU = 1 inference_config = InferenceConfig() with tf.device('/device:GPU:1'): self.rcnn = modellib.MaskRCNN(mode='inference', model_dir=MODEL_DIR, config=inference_config) self.rcnn.load_weights(model_path, by_name=True) self.class_names = [ 'BG', 'blue_ring', 'green_ring', 'yellow_ring', 'tower', 'hand', 'robot' ] self.visual_idx_1 = self._hyperparams['visual_indices_1'] self.visual_idx_2 = self._hyperparams['visual_indices_2'] self.target_ids = [1, 4] # Take some ramp images to allow cams to adjust for brightness etc. img_subs = self.img_subs_list[0] depth_subs = self.depth_subs_list[0] for i in range(100): # Get frameset of color and depth color_image = img_subs.img depth_image = depth_subs.img resized_image = resize_frame(color_image, IMAGE_SIZE)[None, :] print('Taking ramp image %d.' % i) # DEBUG PICTURE: image = plt.imread('/home/msieb/projects/Mask_RCNN/datasets/baxter/test/2_view0/00000.jpg') results = self.rcnn.detect([color_image], verbose=0) r = results[0] self.colors = visualize.random_colors(7)
def __init__(self, hyperparams, init_node=True): """Initialized Agent. hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node.""" # Setup the main node. if init_node: rospy.init_node('gps_agent_ur_ros_node') # Pull parameters from hyperparams file. config = copy.deepcopy(AGENT_UR_ROS) config.update(hyperparams) Agent.__init__(self, config) conditions = self._hyperparams['conditions'] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) # x0 is not used by the agent, but it needs to be set on the agent # object because the GPS algorithm implementation gets the value it # uses for x0 from the agent object. (This should probably be changed.) self.x0 = self._hyperparams['x0'] # Used by subscriber to get the present time and by main thread to # get buffered positions. self.tf = TransformListener() # Times when EE positions were most recently recorded. Set by the # subscriber thread when it collects the EE positions from ROS and # stores them for later use on the main thread. self._ee_time = None self._previous_ee_time = None self._ref_time = None # Lock used to make sure the subscriber does not update the EE times # while they are being used by the main thread. # The subscriber thread acquires this lock with blocking=False to # skip an update of the protected data if the lock is held elsewhere # (ie, the main thread). The main thread acquires this lock with # blocking=True to block until the lock has been acquired. self._time_lock = threading.Lock() # Flag denoting if observations read in from ROS messages are fresh # or have already been used in processing. Used to make sure that the # observations have been updated by the subscriber before the main # thread will try to use them. self._observations_stale = True # Message sent via the joint_subscriber topic indicating the # newly observed state of the UR. self._observation_msg = None # set dimensions of action self.dU = self._hyperparams['dU'] self._valid_joint_set = set(hyperparams['joint_order']) self._valid_joint_index = { joint: index for joint, index in enumerate(hyperparams['joint_order']) } # Initialize the publisher and subscriber. The subscriber begins # checking the time every 40 msec by default. self._pub = rospy.Publisher(self._hyperparams['joint_publisher'], JointTrajectory) self._sub = rospy.Subscriber(self._hyperparams['joint_subscriber'], JointTrajectoryControllerState, self._observation_callback) # Used for enforcing the period specified in hyperparameters. self.period = self._hyperparams['dt'] self.r = rospy.Rate(1. / self.period) self.r.sleep() self._currently_resetting = False self._reset_cv = threading.Condition(self._time_lock)
def __init__(self, hyperparams, init_node=False): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT_ROS) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_ros_node', disable_signals=True) # self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. self.ptconf, self.dconf = hyperparams['ptconf'], hyperparams['dconf'] conditions = self._hyperparams['conditions'] self.x0 = [] for field in ('x0', 'ee_points_tgt', 'reset_conditions'): self._hyperparams[field] = setup(self._hyperparams[field], conditions) self.x0 = self._hyperparams['x0'] self.use_tf = False self.observations_stale = True # Baxter specific limb = self._hyperparams['trial_arm'] self._trial_limb = limb self._trial_arm = baxter_interface.Limb(self._trial_limb) self._trial_gripper = baxter_interface.Gripper( self._trial_limb, baxter_interface.CHECK_VERSION) # self._trial_gripper.set_moving_force(30) # self._trial_gripper.set_holding_force(30) # self._trial_gripper.command_position(100) print('\nplace object in gripper!!!\n') rospy.sleep(1) # self._trial_gripper.set_moving_force(0.1) # self._trial_gripper.set_holding_force(0.1) self._auxiliary_limb = 'right' self._auxiliary_arm = baxter_interface.Limb(self._auxiliary_limb) self._auxiliary_gripper = baxter_interface.Gripper( self._auxiliary_limb, baxter_interface.CHECK_VERSION) print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(100) ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) rospy.wait_for_service(ns, 5.0) self.joint_names = self._trial_arm.joint_names() self.n_joints = len(self.joint_names) # self.planner = BaxterInterfacePlanner(limb) self._rate = 1.0 / self._hyperparams['dt'] self._control_rate = rospy.Rate(self._rate) self._missed_cmds = 20.0 self._trial_arm.set_command_timeout( (1.0 / self._rate) * self._missed_cmds) self._kin_aux = baxter_kinematics('right') self._kin_trial = baxter_kinematics('left') self.idx_curr_rollout = 0 self.idx_curr_itr = 0 self.take_video = False # self.reset_condition = hyperparams['reset_condition'] # Reset auxiliary arm # self.reset_arm(arm=self._auxiliary_arm, mode='ANGLE', data=self._hyperparams['aux_arm_joint_vals']) # pos = list(self._auxiliary_arm.endpoint_pose()['position']) # orn = list(self._auxiliary_arm.endpoint_pose()['orientation']) # joint_names = self._auxiliary_arm.joint_names() # q = [self._auxiliary_arm.joint_angle(j) for j in joint_names] # dq = [self._auxiliary_arm.joint_velocity(j) for j in joint_names] # vals = { # 'limb': limb, # 'joint_angles': q, # 'joint_vels': dq, # 'ee_pos': pos, # 'ee_orn': orn, # } # with open('/media/zhouxian/ed854110-6801-4dcd-9acf-c4f904955d71/iccv2019/demos/pour/baxter_init_vals/lacan.json', 'w') as fp: # json.dump(vals, fp) # import ipdb; ipdb.set_trace() # self.views_ids = self._hyperparams['views'] topic_img_list = [ '/camera' + device_id + '/color/image_raw' for device_id in self.views_ids ] topic_dimg_list = [ '/camera' + device_id + '/aligned_depth_to_color/image_raw' for device_id in self.views_ids ] # pub = rospy.Publisher('/tcn/embedding', numpy_msg(Floats), queue_size=3) self.img_subs_list = [ img_subscriber(topic=topic_img) for topic_img in topic_img_list ] self.dimg_subs_list = [ img_subscriber(topic=topic_dimg) for topic_dimg in topic_dimg_list ] self.finger_listener = EEFingerListener() # Jacobian # Jacobian Pseudo-Inverse (Moore-Penrose) self.gripper_movement_cnt = 0 # MRCNN variables self.mrcnn = self._hyperparams['mrcnn_model'] self.class_names = self._hyperparams['class_names'] self.target_ids = self._hyperparams['target_ids'] # self.fig, self.ax = visualize.get_ax() self.fig, self.axes = self._hyperparams['figure_axes'] self.centroids_last_known = {id: None for id in self.target_ids} self.all_object_traj = {id: np.empty((0, 3)) for id in self.target_ids} self.total_runs = 0 self.listener = tf.TransformListener() self.start_orn0 = list(self._trial_arm.endpoint_pose()['orientation']) # DON Features self.feature_model = self._hyperparams['feature_model'] self.feature_fn = self._hyperparams['feature_fn'] self.last_known_feature = None self.feature_traj = [] self.last_known_mask = None cv2.namedWindow('target') cv2.namedWindow('query') cv2.namedWindow('mask1') # Metrics self.geom_dist_ee_to_anchor_traj = [] self.geom_dist_object2_to_anchor_traj = []
self._setup_conditions() #TODO: customize world for veh self._setup_world(self._hyperparams["world"], self._hyperparams["target_state"], self._hyperparams["render"]) def _setup_conditions(self): """ Helper method for setting some hyperparameters that may vary by condition. """ # TODO: specify the fields conds = self._hyperparams['conditions'] for field in ('xxxx'): self._hyperparams[field] = setup(self._hyperparams[field], conds) def _setup_world(self): """ Helper method for handling setup of the Box2D world. """ self.x0 = self._hyperparams["x0"] # initial state self._worlds = [world(self.x0[i], target, render) for i in range(self._hyperparams['conditions'])] def sample(self, policy, condition, verbose=False, save=True, noisy=True): """ Runs a trial and constructs a new sample containing information about the trial. Args: