def publish_transform():
optitrak_params = rosparam.get_param("optitrak_calibration")
remap_mat = PoseConverter.to_homo_mat(optitrak_params["position"], optitrak_params["orientation"]) ** -1
tf_list = tf.TransformListener()
tf_broad = tf.TransformBroadcaster()
small_dur = rospy.Duration(0.001)
robot_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
opti_mat = PoseConverter.to_homo_mat([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0])
while not rospy.is_shutdown():
try:
now = rospy.Time(0.0)
(opti_pos, opti_rot) = tf_list.lookupTransform("/optitrak", "/pr2_antenna", now)
opti_mat = PoseConverter.to_homo_mat(opti_pos, opti_rot)
now = rospy.Time(0.0)
(robot_pos, robot_rot) = tf_list.lookupTransform("/wide_stereo_link", "/base_footprint", now)
robot_mat = PoseConverter.to_homo_mat(robot_pos, robot_rot)
odom_mat = opti_mat * remap_mat * robot_mat
odom_pos, odom_rot = PoseConverter.to_pos_quat(odom_mat)
tf_broad.sendTransform(odom_pos, odom_rot, rospy.Time.now(), "/base_footprint", "/optitrak")
rospy.sleep(0.001)
except Exception as e:
print e
def main():
rospy.init_node("tf_link_flipper")
child_frame = rospy.get_param("~child_frame")
parent_frame = rospy.get_param("~parent_frame")
link_frame = rospy.get_param("~link_frame")
rate = rospy.get_param("~rate", 100)
link_trans = rospy.get_param("~link_transform")
l_B_c = PoseConverter.to_homo_mat(link_trans['pos'], link_trans['quat'])
tf_broad = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
rospy.sleep(1)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
time = rospy.Time.now()
tf_listener.waitForTransform(child_frame, parent_frame, rospy.Time(0),
rospy.Duration(1))
pos, quat = tf_listener.lookupTransform(child_frame, parent_frame,
rospy.Time(0))
c_B_p = PoseConverter.to_homo_mat(pos, quat)
l_B_p = l_B_c * c_B_p
tf_pos, tf_quat = PoseConverter.to_pos_quat(l_B_p)
tf_broad.sendTransform(tf_pos, tf_quat, time, parent_frame, link_frame)
r.sleep()
def generate_trajectory(ud):
b_B_s = PoseConverter.to_homo_mat(ud.start_click)
b_B_e = PoseConverter.to_homo_mat(ud.end_click)
s_B_e = (b_B_s**-1) * b_B_e
b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2])
s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T
s_vel = s_vel / np.linalg.norm(s_vel)
b_vel = b_B_s[:3, :3].T * s_vel
b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T
b_ortho = b_ortho / np.linalg.norm(b_ortho)
b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T])
b_p_start = b_B_s[:3, 3]
b_p_end = b_B_e[:3, 3]
b_p_end = 3 #TODO TODO
self.start_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_start, b_R_traj)))
self.end_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_end, b_R_traj)))
ud.start_traj_frame = (b_p_start, b_R_traj)
ud.end_traj_frame = (b_p_end, b_R_traj)
return 'succeeded'
def generate_trajectory(ud):
b_B_s = PoseConverter.to_homo_mat(ud.start_click)
b_B_e = PoseConverter.to_homo_mat(ud.end_click)
s_B_e = (b_B_s ** -1) * b_B_e
b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2])
s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T
s_vel = s_vel / np.linalg.norm(s_vel)
b_vel = b_B_s[:3, :3].T * s_vel
b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T
b_ortho = b_ortho / np.linalg.norm(b_ortho)
b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T])
b_p_start = b_B_s[:3, 3]
b_p_end = b_B_e[:3, 3]
b_p_end = 3 #TODO TODO
self.start_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id,
(b_p_start, b_R_traj)))
self.end_frame_pub.publish(PoseConverter.to_pose_stamped_msg(ud.start_click.header.frame_id,
(b_p_end, b_R_traj)))
ud.start_traj_frame = (b_p_start, b_R_traj)
ud.end_traj_frame = (b_p_end, b_R_traj)
return 'succeeded'
def capture_data():
tf_list = tf.TransformListener()
opti_mat, robot_mat = [], []
print "Hit enter to collect a data sample"
i = 0
while not rospy.is_shutdown():
rospy.sleep(0.3)
# if raw_input(".") == 'q':
# break
now = rospy.Time(0.0)
(opti_pos, opti_rot) = tf_list.lookupTransform("/pr2_antenna", "/optitrak", now)
(robot_pos, robot_rot) = tf_list.lookupTransform("/wide_stereo_link", "/base_footprint", now)
opti_mat.append(opti_pos)
opti_rot_foot = np.mat(
[
[-0.17496687, 0.03719102, -0.98387165],
[-0.98098458, -0.09183928, 0.17098186],
[-0.08399907, 0.99507908, 0.05255265],
]
).T
opti_pos_foot = np.mat([[1.276], [-0.81755], [-0.06905]])
foot_B_opti = PoseConverter.to_homo_mat(opti_pos_foot, opti_rot_foot) ** -1
opti_B_ant = PoseConverter.to_homo_mat(opti_pos, opti_rot) ** -1
wide_B_foot = PoseConverter.to_homo_mat(robot_pos, robot_rot)
print wide_B_foot * foot_B_opti * opti_B_ant
offset_robot_pos = (wide_B_foot * foot_B_opti)[:3, 3]
print offset_robot_pos, opti_pos
robot_mat.append(offset_robot_pos.T.A[0].tolist())
i += 1
if i % 10 == 0:
print i, "samples collected"
return opti_mat, robot_mat
def publish_transform():
optitrak_params = rosparam.get_param("optitrak_calibration")
remap_mat = PoseConverter.to_homo_mat(optitrak_params["position"],
optitrak_params["orientation"])**-1
tf_list = tf.TransformListener()
tf_broad = tf.TransformBroadcaster()
small_dur = rospy.Duration(0.001)
robot_mat = PoseConverter.to_homo_mat([0., 0., 0.], [0., 0., 0., 1.])
opti_mat = PoseConverter.to_homo_mat([0., 0., 0.], [0., 0., 0., 1.])
while not rospy.is_shutdown():
try:
now = rospy.Time(0.)
(opti_pos,
opti_rot) = tf_list.lookupTransform("/optitrak", "/pr2_antenna",
now)
opti_mat = PoseConverter.to_homo_mat(opti_pos, opti_rot)
now = rospy.Time(0.)
(robot_pos,
robot_rot) = tf_list.lookupTransform("/wide_stereo_link",
"/base_footprint", now)
robot_mat = PoseConverter.to_homo_mat(robot_pos, robot_rot)
odom_mat = opti_mat * remap_mat * robot_mat
odom_pos, odom_rot = PoseConverter.to_pos_quat(odom_mat)
tf_broad.sendTransform(odom_pos, odom_rot, rospy.Time.now(),
"/base_footprint", "/optitrak")
rospy.sleep(0.001)
except Exception as e:
print e
def capture_data():
tf_list = tf.TransformListener()
opti_mat, robot_mat = [], []
print "Hit enter to collect a data sample"
i = 0
while not rospy.is_shutdown():
rospy.sleep(0.3)
#if raw_input(".") == 'q':
# break
now = rospy.Time(0.)
(opti_pos, opti_rot) = tf_list.lookupTransform("/pr2_antenna",
"/optitrak", now)
(robot_pos,
robot_rot) = tf_list.lookupTransform("/wide_stereo_link",
"/base_footprint", now)
opti_mat.append(opti_pos)
opti_rot_foot = np.mat([[-0.17496687, 0.03719102, -0.98387165],
[-0.98098458, -0.09183928, 0.17098186],
[-0.08399907, 0.99507908, 0.05255265]]).T
opti_pos_foot = np.mat([[1.276], [-0.81755], [-0.06905]])
foot_B_opti = PoseConverter.to_homo_mat(opti_pos_foot,
opti_rot_foot)**-1
opti_B_ant = PoseConverter.to_homo_mat(opti_pos, opti_rot)**-1
wide_B_foot = PoseConverter.to_homo_mat(robot_pos, robot_rot)
print wide_B_foot * foot_B_opti * opti_B_ant
offset_robot_pos = (wide_B_foot * foot_B_opti)[:3, 3]
print offset_robot_pos, opti_pos
robot_mat.append(offset_robot_pos.T.A[0].tolist())
i += 1
if i % 10 == 0:
print i, "samples collected"
return opti_mat, robot_mat
def pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose(
"/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(
tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,
0:3] = tf_trans.euler_matrix(0, 0, head_rot,
'sxyz')[0:3, 0:3]
self.cheek_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(
np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame",
cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [
self.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1, i],
ell_traj[2, i], gripper_rot,
ell_frame_mat)
for i in range(ell_traj.shape[1])
]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(),
ell_pose_traj[-1], t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1])
for i in range(num_samps)]
self.start_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[0]))
self.end_pub.publish(
PoseConverter.to_pose_stamped_msg("/torso_lift_link",
cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time,
rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)
def ar_sub(self, msg):
cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id, msg.pose.pose)
cur_ar_ps.header.stamp = msg.header.stamp
try:
cur_ar_in_base = self.tf_list.transformPose("/base_link", cur_ar_ps)
except tf.Exception as e:
return
self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base)
self.ar_pose_updated = True
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot, ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot,
ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def ar_sub(self, msg):
cur_ar_ps = PoseConverter.to_pose_stamped_msg(msg.header.frame_id,
msg.pose.pose)
cur_ar_ps.header.stamp = msg.header.stamp
try:
cur_ar_in_base = self.tf_list.transformPose(
"/base_link", cur_ar_ps)
except tf.Exception as e:
return
self.cur_ar_pose = PoseConverter.to_homo_mat(cur_ar_in_base)
self.ar_pose_updated = True
def ar_sub(self, msg):
if self.kin_arm == None:
self.kin_arm = kin_from_param(base_link="base_link",
end_link=msg.header.frame_id)
base_B_camera = self.kin_arm.forward_filled()
camera_B_tag = PoseConverter.to_homo_mat(msg.pose.pose)
cur_ar_pose = base_B_camera * camera_B_tag
# check to see if the tag is in front of the robot
if cur_ar_pose[0, 3] < 0.:
#rospy.logwarn("Strange AR toolkit bug!")
return
self.cur_ar_pose = cur_ar_pose
self.ar_pose_updated = True
def execute_trajectory(self, ell_f, gripper_rot, duration=5.):
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
num_samps = int(duration / self.time_step)
t_vals = min_jerk_traj(num_samps)
self.arm.reset_ep()
self.reset_ell_ep()
ell_init = np.mat(self.ell_ep).T
ell_final = np.mat(ell_f).T
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
print "wrapping; ell_f:", ell_f
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
print "wrapping; ell_f:", ell_f
print "init", ell_init, "final", ell_final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
# find the current ellipsoid frame
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_frame_mat = PoseConverter.to_homo_mat(
self.tf_list.lookupTransform("/torso_lift_link",
"/ellipse_frame", cur_time))
print ell_frame_mat
print ell_traj.shape
ell_pose_traj = [self.robot_ellipsoidal_pose(ell_traj[0,i], ell_traj[1,i], ell_traj[2,i],
gripper_rot, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# replace the rotation matricies with a simple cartesian slerp
cart_interp_traj = self.arm.interpolate_ep(self.arm.get_ep(), ell_pose_traj[-1],
t_vals)
fixed_traj = [(ell_pose_traj[i][0], cart_interp_traj[i][1]) for i in range(num_samps)]
self.start_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[0]))
self.end_pub.publish(PoseConverter.to_pose_stamped_msg("/torso_lift_link", cart_interp_traj[-1]))
#ep_traj_control = EPTrajectoryControl(self.arm, cart_interp_traj)
print_trajectory_stats(ell_traj, fixed_traj, t_vals)
ep_traj_control = EPTrajectoryControl(self.arm, fixed_traj)
self.action_preempted = False
self.ell_traj_behavior.stop_epc = False
monitor_timer = rospy.Timer(rospy.Duration(0.1), self._check_preempt)
self.ell_traj_behavior.epc_motion(ep_traj_control, self.time_step)
monitor_timer.shutdown()
if self.action_preempted:
self.reset_ell_ep()
else:
self.ell_ep = ell_f
def main():
rospy.init_node("tf_link_flipper")
child_frame = rospy.get_param("~child_frame")
parent_frame = rospy.get_param("~parent_frame")
link_frame = rospy.get_param("~link_frame")
rate = rospy.get_param("~rate", 100)
link_trans = rospy.get_param("~link_transform")
l_B_c = PoseConverter.to_homo_mat(link_trans['pos'], link_trans['quat'])
tf_broad = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
rospy.sleep(1)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
time = rospy.Time.now()
tf_listener.waitForTransform(child_frame, parent_frame, rospy.Time(0), rospy.Duration(1))
pos, quat = tf_listener.lookupTransform(child_frame, parent_frame, rospy.Time(0))
c_B_p = PoseConverter.to_homo_mat(pos, quat)
l_B_p = l_B_c * c_B_p
tf_pos, tf_quat = PoseConverter.to_pos_quat(l_B_p)
tf_broad.sendTransform(tf_pos, tf_quat, time, parent_frame, link_frame)
r.sleep()
def pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3]
self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
def update_ellipse_pose(self):
cur_time = rospy.Time.now()
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
cur_tf = self.tf_list.lookupTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
self.ell_tf = PoseConverter.to_homo_mat(cur_tf)