def _normal_to_ellipse_prolate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(trans.quaternion_matrix(norm_quat_ortho)[:3,:3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_oblate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(-lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_oblate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(-lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1. / (1. + ny * ny / (nz * nz)))
k = -ny * j / nz
norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j],
[-nz, nx * j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi / 2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho,
quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(
norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_prolate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1. / (1. + ny * ny / (nz * nz)))
k = -ny * j / nz
norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j],
[-nz, nx * j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0,
0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(
trans.quaternion_matrix(norm_quat_ortho)[:3, :3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(
norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0, 0],
pose_pos[1, 0], pose_pos[2,
0])
_, ell_rot = PoseConv.to_pos_rot(
self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(
np.mat([0] * 3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None):
vis_pub = rospy.Publisher(name, Marker, queue_size=10)
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.Time()
marker.ns = "my_namespace"
marker.id = 0
marker.type = type
marker.action = Marker.ADD
if type == Marker.LINE_LIST:
for point in points:
p = Point()
p.x = point[0]
p.y = point[1]
p.z = point[2]
marker.points.append(p)
else:
r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1])
rot = pose[0:3,0:3] * r
pose2 = numpy.matrix(numpy.identity(4))
pose2[0:3,0:3] = rot
pose2[0:3,3] = pose[0:3,3]
quat_pose = PoseConv.to_pos_quat(pose2)
marker.pose.position.x = quat_pose[0][0]
marker.pose.position.y = quat_pose[0][1]
marker.pose.position.z = quat_pose[0][2]
marker.pose.orientation.x = quat_pose[1][0]
marker.pose.orientation.y = quat_pose[1][1]
marker.pose.orientation.z = quat_pose[1][2]
marker.pose.orientation.w = quat_pose[1][3]
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.a = .5
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
vis_pub.publish(marker)
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START %msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback('Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
retreat_ell_pos, retreat_ell_quat,
velocity=0.01, min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
approach_ell_pos, approach_ell_quat,
velocity=0.01, min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
final_ell_pos, final_ell_quat,
velocity=0.01, min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0])
_, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def ellipsoidal_to_pose(self, pose):
ell_pos, ell_quat = PoseConv.to_pos_quat(pose)
if not self.is_oblate:
return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat)
else:
return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START % msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id,
'/ellipse_frame', msg.header.stamp,
rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(
goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(
goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg(
'/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback(
'Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(
goal_cart_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s"
% e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot,
[1., 0., 0., 0.])
approach_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT
]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
final_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1],
goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST
]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose(
(retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose(
(approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose(
(final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
retreat_ell_pos,
retreat_ell_quat,
velocity=0.01,
min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(
retreat_ell_pos,
retreat_ell_quat,
approach_ell_pos,
approach_ell_quat,
velocity=0.01,
min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(
approach_ell_pos,
approach_ell_quat,
final_ell_pos,
final_ell_quat,
velocity=0.01,
min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv
if __name__ == "__main__":
rospy.init_node("test_poseconv")
homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
pose_msg = PoseConv.to_pose_msg(homo_mat)
pos, quat = PoseConv.to_pos_quat(pose_msg)
pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg",
"pose_stamped_msg2", "tf_stamped", "p", "R"]:
print "%s: \n" % name, locals()[name]
def ellipsoidal_to_pose(self, pose):
ell_pos, ell_quat = PoseConv.to_pos_quat(pose)
if not self.is_oblate:
return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat)
else:
return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)