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_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 _extract_tf_msg(tf_msg):
px = tf_msg.translation.x; py = tf_msg.translation.y; pz = tf_msg.translation.z
ox = tf_msg.rotation.x; oy = tf_msg.rotation.y
oz = tf_msg.rotation.z; ow = tf_msg.rotation.w
quat = [ox, oy, oz, ow]
rot_euler = trans.euler_from_quaternion(quat)
homo_mat = np.mat(trans.quaternion_matrix(quat))
homo_mat[:3,3] = np.mat([[px, py, pz]]).T
return homo_mat, quat, rot_euler
def _extract_pose_msg(pose):
px = pose.position.x; py = pose.position.y; pz = pose.position.z
ox = pose.orientation.x; oy = pose.orientation.y
oz = pose.orientation.z; ow = pose.orientation.w
quat = [ox, oy, oz, ow]
rot_euler = trans.euler_from_quaternion(quat)
homo_mat = np.mat(trans.quaternion_matrix(quat))
homo_mat[:3,3] = np.mat([[px, py, pz]]).T
return homo_mat, quat, rot_euler
def _extract_tf_msg(tf_msg):
px = tf_msg.translation.x; py = tf_msg.translation.y; pz = tf_msg.translation.z
ox = tf_msg.rotation.x; oy = tf_msg.rotation.y
oz = tf_msg.rotation.z; ow = tf_msg.rotation.w
quat = [ox, oy, oz, ow]
rot_euler = trans.euler_from_quaternion(quat)
homo_mat = np.mat(trans.quaternion_matrix(quat))
homo_mat[:3,3] = np.mat([[px, py, pz]]).T
return homo_mat, quat, rot_euler
def _extract_pose_msg(pose):
px = pose.position.x; py = pose.position.y; pz = pose.position.z
ox = pose.orientation.x; oy = pose.orientation.y
oz = pose.orientation.z; ow = pose.orientation.w
quat = [ox, oy, oz, ow]
rot_euler = trans.euler_from_quaternion(quat)
homo_mat = np.mat(trans.quaternion_matrix(quat))
homo_mat[:3,3] = np.mat([[px, py, pz]]).T
return homo_mat, quat, rot_euler
def interpolate_ep(self, ep_a, ep_b, t_vals):
pos_a, rot_a = ep_a
pos_b, rot_b = ep_b
num_samps = len(t_vals)
pos_waypoints = (np.array(pos_a) +
np.array(np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals))
pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
rot_homo_a[:3,:3] = rot_a
rot_homo_b[:3,:3] = rot_b
quat_a = trans.quaternion_from_matrix(rot_homo_a)
quat_b = trans.quaternion_from_matrix(rot_homo_b)
rot_waypoints = []
for t in t_vals:
cur_quat = trans.quaternion_slerp(quat_a, quat_b, t)
rot_waypoints.append(np.mat(trans.quaternion_matrix(cur_quat))[:3,:3])
return zip(pos_waypoints, rot_waypoints)
def _parse_ell_move(self, change_ep, abs_sel, orient_quat):
#change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat)
#Provides new position and orientation goals (in relative or abs manner, determined next).
change_ell_ep, change_rot_ep = change_ep
#abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool)
#abs_ell_ep_sel: selects lat, long, height absolute vs relative position command
#change_rot_ep: selects abs vs relative use of orientation.
abs_ell_ep_sel, is_abs_rot = abs_sel
ell_f = np.where(abs_ell_ep_sel,
change_ell_ep,
np.array(self.get_ell_ep()) + np.array(change_ell_ep))
print "old", ell_f
if ell_f[0] > np.pi:
ell_f[0] = 2 * np.pi - ell_f[0]
ell_f[1] *= -1
if ell_f[0] < 0.:
ell_f[0] *= -1
ell_f[1] *= -1
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
ell_f = self._clip_ell_ep(ell_f)
print "new", ell_f
if is_abs_rot:
#Apply rotation from change_rot_ep as final orientation of pose
#Expects rotation matrix in change_rot_ep
rot_change_mat = change_rot_ep
_, ell_final_rot = self.ell_server.robot_ellipsoidal_pose(ell_f[0], ell_f[1], ell_f[2],
orient_quat)
rot_mat_f = ell_final_rot * rot_change_mat
else:
#Apply rotation from change_rot_ep as change to current rotation
#Expects quaternion in change_rot_ep
quat = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.quaternion_matrix(quat))[:3,:3]
rot_mat_f = cur_rot * rot_mat
return ell_f, rot_mat_f
def _parse_ell_move(self, change_ep, abs_sel, orient_quat):
#change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat)
#Provides new position and orientation goals (in relative or abs manner, determined next).
change_ell_ep, change_rot_ep = change_ep
#abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool)
#abs_ell_ep_sel: selects lat, long, height absolute vs relative position command
#change_rot_ep: selects abs vs relative use of orientation.
abs_ell_ep_sel, is_abs_rot = abs_sel
ell_f = np.where(abs_ell_ep_sel, change_ell_ep,
np.array(self.get_ell_ep()) + np.array(change_ell_ep))
print "old", ell_f
if ell_f[0] > np.pi:
ell_f[0] = 2 * np.pi - ell_f[0]
ell_f[1] *= -1
if ell_f[0] < 0.:
ell_f[0] *= -1
ell_f[1] *= -1
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
ell_f = self._clip_ell_ep(ell_f)
print "new", ell_f
if is_abs_rot:
#Apply rotation from change_rot_ep as final orientation of pose
#Expects rotation matrix in change_rot_ep
rot_change_mat = change_rot_ep
_, ell_final_rot = self.ell_server.robot_ellipsoidal_pose(
ell_f[0], ell_f[1], ell_f[2], orient_quat)
rot_mat_f = ell_final_rot * rot_change_mat
else:
#Apply rotation from change_rot_ep as change to current rotation
#Expects quaternion in change_rot_ep
quat = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.quaternion_matrix(quat))[:3, :3]
rot_mat_f = cur_rot * rot_mat
return ell_f, rot_mat_f
def _make_generic(args):
try:
if type(args[0]) == str:
frame_id = args[0]
header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic(
args[1:])
if homo_mat is None:
return None, None, None, None
if header is None:
header = [0, rospy.Time.now(), '']
header[2] = frame_id
return header, homo_mat, rot_quat, rot_euler
if len(args) == 2:
return PoseConv._make_generic(((args[0], args[1]), ))
elif len(args) == 1:
pose_type = PoseConv.get_type(*args)
if pose_type is None:
return None, None, None, None
if pose_type == 'pose_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'pose_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(
args[0].pose)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(
args[0].transform)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'point_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'point_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(
args[0].point)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(
args[0].twist)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp,
frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'homo_mat':
return (None, np.mat(args[0]),
trans.quaternion_from_matrix(args[0]).tolist(),
trans.euler_from_matrix(args[0]))
elif pose_type in [
'pos_rot', 'pos_euler', 'pos_quat', 'pos_axis_angle'
]:
pos_arg = np.mat(args[0][0])
if pos_arg.shape == (1, 3):
# matrix is row, convert to column
pos = pos_arg.T
elif pos_arg.shape == (3, 1):
pos = pos_arg
if pose_type == 'pos_axis_angle':
homo_mat = np.mat(np.eye(4))
homo_mat[:3, :3] = axis_angle_to_rot_mat(
args[0][1][0], args[0][1][1])
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
elif pose_type == 'pos_rot':
# rotation matrix
homo_mat = np.mat(np.eye(4))
homo_mat[:3, :3] = np.mat(args[0][1])
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
else:
rot_arg = np.mat(args[0][1])
if rot_arg.shape[1] == 1:
rot_arg = rot_arg.T
rot_list = rot_arg.tolist()[0]
if pose_type == 'pos_euler':
# Euler angles rotation
homo_mat = np.mat(trans.euler_matrix(*rot_list))
quat = trans.quaternion_from_euler(*rot_list)
rot_euler = rot_list
elif pose_type == 'pos_quat':
# quaternion rotation
homo_mat = np.mat(
trans.quaternion_matrix(rot_list))
quat = rot_list
rot_euler = trans.euler_from_quaternion(quat)
homo_mat[:3, 3] = pos
return None, homo_mat, np.array(quat), rot_euler
except:
pass
return None, None, None, None
def _make_generic(args):
try:
if type(args[0]) == str:
frame_id = args[0]
header, homo_mat, rot_quat, rot_euler = PoseConv._make_generic(args[1:])
if homo_mat is None:
return None, None, None, None
if header is None:
header = [0, rospy.Time.now(), '']
header[2] = frame_id
return header, homo_mat, rot_quat, rot_euler
if len(args) == 2:
return PoseConv._make_generic(((args[0], args[1]),))
elif len(args) == 1:
pose_type = PoseConv.get_type(*args)
if pose_type is None:
return None, None, None, None
if pose_type == 'pose_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'pose_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_pose_msg(args[0].pose)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'tf_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_tf_msg(args[0].transform)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'point_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'point_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_point_msg(args[0].point)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0])
return None, homo_mat, rot_quat, rot_euler
elif pose_type == 'twist_stamped_msg':
homo_mat, rot_quat, rot_euler = PoseConv._extract_twist_msg(args[0].twist)
seq = args[0].header.seq
stamp = args[0].header.stamp
frame_id = args[0].header.frame_id
return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler
elif pose_type == 'homo_mat':
return (None, np.mat(args[0]), trans.quaternion_from_matrix(args[0]).tolist(),
trans.euler_from_matrix(args[0]))
elif pose_type in ['pos_rot', 'pos_euler', 'pos_quat']:
pos_arg = np.mat(args[0][0])
rot_arg = np.mat(args[0][1])
if pos_arg.shape == (1, 3):
# matrix is row, convert to column
pos = pos_arg.T
elif pos_arg.shape == (3, 1):
pos = pos_arg
if pose_type == 'pos_rot':
# rotation matrix
homo_mat = np.mat(np.eye(4))
homo_mat[:3,:3] = rot_arg
quat = trans.quaternion_from_matrix(homo_mat)
rot_euler = trans.euler_from_matrix(homo_mat)
else:
if rot_arg.shape[1] == 1:
rot_arg = rot_arg.T
rot_list = rot_arg.tolist()[0]
if pose_type == 'pos_euler':
# Euler angles rotation
homo_mat = np.mat(trans.euler_matrix(*rot_list))
quat = trans.quaternion_from_euler(*rot_list)
rot_euler = rot_list
elif pose_type == 'pos_quat':
# quaternion rotation
homo_mat = np.mat(trans.quaternion_matrix(rot_list))
quat = rot_list
rot_euler = trans.euler_from_quaternion(quat)
homo_mat[:3, 3] = pos
return None, homo_mat, np.array(quat), rot_euler
except:
pass
return None, None, None, None
