def _get_points_and_vels(self, t, t_last, target, source, debug=False):
"""
Helper function to _run_trial that gets the cartesian positions
and velocities from ROS. Uses tf to convert from the Base to
End Effector and and get cartesian coordinates. Draws positions
from a 'buffer' of past positions by checking recent times.
t: the 'present' time. This is the time of the most recent observation,
so slightly earlier than the actual present.
t_last: the 'present' time from last time this function was called
target: the End Effector link
source: the Base Link
debug: activate extra print statements for debugging"""
# Assert that time has passed since the last step.
assert self.delta_t > 0.
# Listen to the cartesian coordinate of the target link.
pos_last = get_position(self.tf, target, source, t_last)
pos_now = get_position(self.tf, target, source, t)
# Use the past position to get the present velocity.
velocity = (pos_now - pos_last) / self.delta_t
# Shift the present poistion by the End Effector target.
# Since we subtract the target point from the current position, the optimal
# value for this will be 0.
position = np.asarray(pos_now) - np.asarray(
self._hyperparams['ee_points_tgt'])[0, 0, :]
if debug:
print 'VELOCITY:', velocity
print 'POSITION:', position
print 'BEFORE :', t_last.to_sec(), pos_last
print 'PRESENT :', t.to_sec(), pos_now
return position, velocity
# Initialized to start position and inital velocities are 0
x0 = np.zeros(state_space)
x0[:SENSOR_DIMS[JOINT_ANGLES]] = ja_x0
# Need for this node will go away upon migration to KDL
rospy.init_node('gps_agent_ur_ros_node')
# Set starting end effector position using TF
tf = TransformListener()
# Sleep for .1 secs to give the node a chance to kick off
rospy.sleep(1)
time = tf.getLatestCommonTime(WRIST_3_LINK, BASE_LINK)
x0[joint_dim:(joint_dim +
NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position(
tf, WRIST_3_LINK, BASE_LINK, time)
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(
get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
reset_condition = {JOINT_ANGLES: INITIAL_JOINTS, JOINT_VELOCITIES: []}
x0s.append(x0)
ee_tgts.append(ee_tgt)
reset_conditions.append(reset_condition)
if not os.path.exists(common['data_files_dir']):
os.makedirs(common['data_files_dir'])
agent = {
# Initialized to start position and inital velocities are 0
x0 = np.zeros(state_space)
x0[:SENSOR_DIMS[JOINT_ANGLES]] = ja_x0
# Need for this node will go away upon migration to KDL
rospy.init_node('gps_agent_ur_ros_node')
# Set starting end effector position using TF
tf = TransformListener()
# Sleep for .1 secs to give the node a chance to kick off
rospy.sleep(1)
time = tf.getLatestCommonTime(WRIST_3_LINK, BASE_LINK)
x0[joint_dim:(joint_dim + NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position(tf, WRIST_3_LINK, BASE_LINK, time)
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(
get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T
)
reset_condition = {
JOINT_ANGLES: INITIAL_JOINTS,
JOINT_VELOCITIES: []
}
x0s.append(x0)
ee_tgts.append(ee_tgt)
reset_conditions.append(reset_condition)
# Initialized to start position and inital velocities are 0
x0 = np.zeros(state_space)
x0[:SENSOR_DIMS[JOINT_ANGLES]] = ja_x0
# Need for this node will go away upon migration to KDL
rospy.init_node('gps_agent_aubo_ros_node')
# Set starting end effector position using TF
tf = TransformListener()
# Sleep for .1 secs to give the node a chance to kick off
rospy.sleep(1)
time = tf.getLatestCommonTime(WRIST3_LINK, PEDESTAL_LINK)
x0[joint_dim:(joint_dim +
NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position(
tf, WRIST3_LINK, PEDESTAL_LINK, time)
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(
get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
reset_condition = {JOINT_ANGLES: INITIAL_JOINTS, JOINT_VELOCITIES: []}
x0s.append(x0)
ee_tgts.append(ee_tgt)
reset_conditions.append(reset_condition)
if not os.path.exists(common['data_files_dir']):
os.makedirs(common['data_files_dir'])
agent = {
x0[:SENSOR_DIMS[JOINT_ANGLES]] = ja_x0
# In[ ]:
# Need for this node will go away upon migration to KDL
rospy.init_node('gps_agent_pepper_node')
# Set starting end effector position using TF
tf = TransformListener()
# Sleep for .1 secs to give the node a chance to kick off
rospy.sleep(1)
time = tf.getLatestCommonTime(L_WRIST, TORSO)
x0[joint_dim:(joint_dim +
NUM_EE_POINTS * EE_POINTS.shape[1])] = get_position(
tf, L_WRIST, TORSO, time)
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(
get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
reset_condition = {JOINT_ANGLES: INITIAL_JOINTS, JOINT_VELOCITIES: []}
x0s.append(x0)
ee_tgts.append(ee_tgt)
reset_conditions.append(reset_condition)
if not os.path.exists(common['data_files_dir']):
os.makedirs(common['data_files_dir'])
agent = {