def _extract_twist_msg(twist_msg):
pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z]
rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z]
quat = trans.quaternion_from_euler(*rot_euler, axes='sxyz')
homo_mat = np.mat(trans.euler_matrix(*rot_euler))
homo_mat[:3,3] = np.mat([pos]).T
return homo_mat, quat, rot_euler
def _extract_twist_msg(twist_msg):
pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z]
rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z]
quat = trans.quaternion_from_euler(*rot_euler, axes='sxyz')
homo_mat = np.mat(trans.euler_matrix(*rot_euler))
homo_mat[:3,3] = np.mat([pos]).T
return homo_mat, quat, rot_euler
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConv.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
def execute_cart_move(self, change_ep, abs_sel, velocity=0.001,
num_samps=None, blocking=True):
if not self.cmd_lock.acquire(False):
return False
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
change_pos_ep, change_rot_ep = change_ep
abs_cart_ep_sel, is_abs_rot = abs_sel
pos_f = np.where(abs_cart_ep_sel, change_pos_ep,
np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0])
if is_abs_rot:
rot_mat_f = change_rot_ep
else:
rpy = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3]
rot_mat_f = cur_rot * rot_mat
traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps)
retval = self._run_traj(traj, blocking=blocking)
self.cmd_lock.release()
return retval
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 homo_mat_from_2d(x, y, rot):
mat2d = np.mat(trans.euler_matrix(0, 0, rot))
mat2d[0,3] = x
mat2d[1,3] = y
return mat2d
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConv.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
return pos, rot
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
