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:
