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 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 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 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 pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input("Type the name of the pose and hit enter to save it (Type 'q' to quit) ")
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input(
"Type the name of the pose and hit enter to save it (Type 'q' to quit) "
)
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame", cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
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 reset_ell_ep(self):
ee_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", self.arm.get_end_effector_pose())
cur_time = rospy.Time.now()
ee_pose.header.stamp = cur_time
self.tf_list.waitForTransform("/torso_lift_link", "/ellipse_frame",
cur_time, rospy.Duration(3))
ell_pose = self.tf_list.transformPose("/ellipse_frame", ee_pose)
pos, quat = PoseConverter.to_pos_quat(ell_pose)
self.ell_ep = list(self.ell_space.pos_to_ellipsoidal(*pos))
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 get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
# self.tmp_pub.publish(pose_stamped)
return pose_stamped
def get_head_pose_srv(self, req):
if req.name not in head_poses:
pose = (np.mat([-9999, -9999, -9999]).T, np.mat(np.zeros((3, 3))))
else:
pose = self.get_head_pose(req.name, req.gripper_rot)
if USE_ELLIPSE_FRAME:
frame = "/ellipse_frame"
else:
frame = "/base_link"
pose_stamped = PoseConverter.to_pose_stamped_msg(frame, pose)
#self.tmp_pub.publish(pose_stamped)
return pose_stamped
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 publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos+df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi/4.
pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
def publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(
0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos + df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi / 4.
pos, rot = self.sspace.spheroidal_to_pose((u, v, p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
def set_ep(self, cep, duration, delay=0.0):
cep_pose_stmp = PoseConverter.to_pose_stamped_msg(
'torso_lift_link', cep)
cep_pose_stmp.header.stamp = rospy.Time.now()
self.command_pose_pub.publish(cep_pose_stmp)
def set_ep(self, cep, duration, delay=0.0):
cep_pose_stmp = PoseConverter.to_pose_stamped_msg('torso_lift_link', cep)
cep_pose_stmp.header.stamp = rospy.Time.now()
self.command_pose_pub.publish(cep_pose_stmp)
