def __init__(self, hyperparams): config = copy.deepcopy(AGENT_OWN) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world()
def __init__(self, hyperparams): config = copy.deepcopy(AGENT_MUJOCO) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world(hyperparams['env_name'])
def __init__(self, hyperparams): config = deepcopy(AGENT_BOX2D) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world(hyperparams["world"], hyperparams["target_state"])
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): config = copy.deepcopy(AGENT_MUJOCO) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world(config['filename']) self._linear = config['point_linear']
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, 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): # Get configs, hyperparameters & initialize agent self.ptconf, self.dconf, self.camconf = hyperparams[ 'ptconf'], hyperparams['dconf'], hyperparams['camconf'] self.env_name = hyperparams['env'] self.gui_on = hyperparams['bullet_gui_on'] config = copy(AGENT_BULLET) config.update(hyperparams) Agent.__init__(self, config) self.hyperparams = hyperparams # Setup bullet environment self._setup_conditions() self._setup_world(hyperparams['filename']) self.setup_bullet() self.setup_inference_camera() # Get demo data self.demo_vid = imageio.get_reader( join(self.dconf.DEMO_DIR, self.dconf.DEMO_NAME, 'rgb/{}.mp4'.format(self.dconf.SEQNAME))) self.demo_frames = [] self.reset_condition = hyperparams['reset_condition'] for im in self.demo_vid: self.demo_frames.append(im) # Setup feature embedding network if enabled if self.ptconf.COMPUTE_FEATURES: self.tcn = hyperparams['vf_model'] # print("model path: {}".format(self.ptconf.MODEL_PATH)) else: self.tcn = None self.vid_seqname = 0 self.feature_fn = hyperparams['feature_fn']
def __init__(self, hyperparams): # Get configs, hyperparameters & initialize agent self.ptconf, self.dconf, self.camconf = hyperparams[ 'ptconf'], hyperparams['dconf'], hyperparams['camconf'] self.env_name = hyperparams['env'] self.gui_on = hyperparams['bullet_gui_on'] self.hyperparams = hyperparams config = copy(AGENT_BULLET) config.update(hyperparams) Agent.__init__(self, config) # Setup bullet environment self._setup_conditions() self._setup_world(hyperparams['filename']) self.setup_bullet() self.setup_inference_camera() # Get demo data self.demo_vid = imageio.get_reader( join(self.dconf.DEMO_DIR, self.dconf.DEMO_NAME, 'rgb/{}.mp4'.format(self.dconf.SEQNAME))) self.demo_frames = [] self.reset_condition = hyperparams['reset_condition'] for im in self.demo_vid: self.demo_frames.append(im) # self.cost_tgt_mean = hyperparams['cost_tgt_mean'] # self.cost_tgt_std = hyperparams['cost_tgt_std'] self.objects_centroids_mrcnn = hyperparams['objects_centroids_mrcnn'] # Setup feature embedding network if enabled self.tcn = None # Setup Mask RCNN inference_config = InferenceConfig() with tf.device('/device:GPU:1'): self.mrcnn = modellib.MaskRCNN( mode='inference', model_dir=join(self.ptconf.EXP_DIR, self.ptconf.EXP_NAME, 'mrcnn_logs'), config=inference_config) self.mrcnn.load_weights(self.ptconf.WEIGHTS_FILE_PATH_MRCNN, by_name=True) self.class_names = gconf.CLASS_NAMES_W_BG # self.target_ids = gconf.CLASS_IDS self.target_ids = [1, 2] self.colors = visualize.random_colors(7) self.plotting_on = hyperparams['plotting_on'] if self.plotting_on: self.fig, self.ax = visualize.get_ax() self.mrcnn_centroids_last_known = { key: None for key in self.target_ids } st() # Run MRCNN on one image because first detection always takes long to initialize self.vid_seqname = 0 rgb_crop, depth_crop = self.get_images(0) results = self.mrcnn.detect([rgb_crop], verbose=0) self.blob_detector = BlobDetector()
def __init__(self, hyperparams): config = deepcopy(AGENT_GYM): config.update(hyperparams) Agent.__init__(self, config) self.reach_start = None self.reach_end = None self.finishing = None self.finishing_time = None
def __init__(self, hyperparams): print('in the file Agent Box 2D') config = deepcopy(AGENT_BOX2D) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world(self._hyperparams["world"], self._hyperparams["target_state"], self._hyperparams["render"])
def __init__(self, hyperparams): config = deepcopy(AGENT_VEH) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() #TODO: customize world for veh self._setup_world(self._hyperparams["world"], self._hyperparams["target_state"], self._hyperparams["render"])
def __init__(self, hyperparams): config = deepcopy(AGENT_VREP) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self.jointHandles = np.array([-1]) self.tipHandle = -1 self._connect_to_vrep()
def __init__(self, hyperparams): config = copy.deepcopy(AGENT_BAXTER) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self.set_initial_state() # self._setup_world(hyperparams['filename']) self.baxter = baxter_methods.BaxterMethods() if RGB_IMAGE in self.obs_data_types: self.baxter.use_camera = True self.baxter._setup_baxter_world()
def __init__(self, hyperparams): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. """ Agent.__init__(self, hyperparams) self.x0 = self._hyperparams['x0'] self.record = False self.render = self._hyperparams['render'] self.scaler = self._hyperparams.get('scaler', None) self.__init_gym()
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 __init__(self, hyperparams): config = deepcopy(AGENT_BOX2D) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self._setup_world(self._hyperparams["world"], self._hyperparams["target_state"], self._hyperparams["render"]) ## realsense self.pipeline = rs.pipeline() cam_config = rs.config() cam_config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) self.pipeline.start(cam_config)
def __init__(self, hyperparams): config = deepcopy(AGENT_BUS) config.update(hyperparams) Agent.__init__(self, config) self._setup_conditions() self.reach_start = None self.reach_end = None self.finishing = None self.finishing_time = None self._setup_world(self._hyperparams["world"], self._hyperparams["target_state"], self._hyperparams["render"], self._hyperparams["polygons"], self._hyperparams["map_size"], self._hyperparams["map_state"], self._hyperparams["display_center"],)
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_TURTLEBOT) config.update(hyperparams) Agent.__init__(self, config) if init_node: rospy.init_node('gps_agent_turtlebot_node') self._init_pubs_and_subs() self._seq_id = 0 # Used for setting seq in ROS commands. self.x0 = self._hyperparams['x0'] r = rospy.Rate(1) r.sleep()
def __init__(self, hyperparams): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ config = copy.deepcopy(AGENT) config.update(hyperparams) Agent.__init__(self, config) self._env = env_search_control(step_max=200, fuzzy=False, add_noise=False) self.x0 = self._hyperparams['x0'] self.use_tf = False self.observations_stale = True
def __init__(self, hyperparams): config = copy.deepcopy(agent_superball) config.update(hyperparams) Agent.__init__(self, config) self._sensor_types = set(self.x_data_types + self.obs_data_types) self.x0 = None self._sensor_readings = {} self._prev_sensor_readings = {} self._prev2_sensor_readings = {} if self._hyperparams['constraint']: import superball_kinematic_tool as skt self._constraint = skt.KinematicMotorConstraints( self._hyperparams['constraint_file'], **self._hyperparams['constraint_params']) rospy.init_node('superball_agent', disable_signals=True, log_level=rospy.DEBUG) self._state_update = False self._state_update_cv = threading.Condition() if 'state_estimator' in self._hyperparams and self._hyperparams[ 'state_estimator']: self._state_sub = rospy.Subscriber('/superball/state', SUPERballStateArray, callback=self._handle_state_msg, queue_size=1) else: self._state_sub = rospy.Subscriber('/superball/state_sim', SUPERballStateArray, callback=self._handle_state_msg, queue_size=1) self._ctrl_pub = rospy.Publisher('/superball/control', String, queue_size=1) self._timestep_pub = rospy.Publisher('/superball/timestep', UInt16, queue_size=1) self._init_motor_pubs() self._compute_rel_pos = True self.reset(0)
def __init__(self, hyperparams): """ Initialize agent. Args: hyperparams: Dictionary of hyperparameters. init_node: Whether or not to initialize a new ROS node. """ Agent.__init__(self, hyperparams) self.x0 = None self.dt = self._hyperparams['dt'] self.scaler = self._hyperparams['scaler'] self.actuator_overrides = self._hyperparams['override_actuator'] self.sensor_overrides = self._hyperparams['override_sensor'] self.signals = self._hyperparams['send_signal'] self.sensors = self.x_data_types self.actuators = self.u_data_types #self.__init_opcua() self.client = None self.pool = ThreadPool(len(self.sensors)) self.debug = True
def __init__(self, hyperparams): config = copy.deepcopy(AGENT_INDY) config.update(hyperparams) Agent.__init__(self, config) rospy.init_node('gps_agent_indy') self.period = self._hyperparams['dt'] self.r = rospy.Rate(1. / self.period) self._setup_conditions() # self._setup_world(hyperparams['filename']) self._set_initial_state() ## TODO : decide to use 'use_camera' or not. robot_ip = “xxx.xxx.x.x” robot_name = “xxxx” self.indy = client.IndyDCPClient(robot_ip, robot_name) # if RGB_IMAGE in self.obs_data_types: self.pipeline = rs.pipeline() cam_config = rs.config() cam_config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) self.pipeline.start(cam_config)
def __init__(self, hyperparams): config = copy.deepcopy(AGENT_PANDA) config.update(hyperparams) Agent.__init__(self, config) rospy.init_node('gps_agent_panda') self.period = self._hyperparams['dt'] self.r = rospy.Rate(1. / self.period) self._setup_conditions() # self._setup_world(hyperparams['filename']) self._set_initial_state() ## TODO : decide to use 'use_camera' or not. self.panda = PandaArm() # if RGB_IMAGE in self.obs_data_types: # self.panda.use_camera = True self.pipeline = rs.pipeline() self.config = rs.config() self.config.enable_stream(rs.stream.color, self._hyperparams['image_width'], self._hyperparams['image_height'], rs.format.bgr8, 30) self.pipeline.start(config)
def __init__(self, hyperparams): Agent.__init__(self, hyperparams) self._setup_conditions() self._setup_worlds()
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=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 = []
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)