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):
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()
self.use_tf = False
self.observations_stale = True
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):
# 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)
