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 _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_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 local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
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))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
curr_ell_pos[1]+change_trans_ep[1],
curr_ell_pos[2]+change_trans_ep[2]]
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
goal_ell_pos, curr_ell_quat,
velocity=ELL_LOCAL_VEL, min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos, goal_ell_quat,
velocity=ELL_ROT_VEL, min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos,(0.,0.,0.,1.),
velocity=ELL_ROT_VEL, min_jerk=True)
else:
rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in 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)
self.force_monitor.update_activity()
def run_controller(self, button_press):
start_pos, _ = self.cart_arm.get_ep()
start_time = rospy.get_time()
quat_gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
if button_press in ell_trans_params:
change_trans_ep = ell_trans_params[button_press]
self.controller.execute_ell_move((change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0),
quat_gripper_rot, ELL_LOCAL_VEL)
elif button_press in ell_rot_params:
change_rot_ep = ell_rot_params[button_press]
self.controller.execute_ell_move(((0, 0, 0), change_rot_ep), ((0, 0, 0), 0),
quat_gripper_rot, ELL_ROT_VEL)
elif button_press == "reset_rotation":
self.controller.execute_ell_move(((0, 0, 0), np.mat(np.eye(3))), ((0, 0, 0), 1),
quat_gripper_rot, ELL_ROT_VEL)
end_pos, _ = self.cart_arm.get_ep()
end_time = rospy.get_time()
dist = np.linalg.norm(end_pos - start_pos)
time_diff = end_time - start_time
print "%s, dist: %.3f, time_diff: %.2f" % (button_press, dist, time_diff)
self.button_distances[button_press].append(dist)
self.button_times[button_press].append(time_diff)
def run_controller(self, button_press):
start_pos, _ = self.cart_arm.get_ep()
start_time = rospy.get_time()
quat_gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
if button_press in cart_trans_params:
ell_change_trans_ep = ell_trans_params[button_press]
change_trans_ep = cart_trans_params[button_press]
# get number of samples:
ell_f, rot_mat_f = self._parse_ell_move((ell_change_trans_ep, (0, 0, 0)),
((0, 0, 0), 0),
quat_gripper_rot)
traj = self._create_ell_trajectory(ell_f, rot_mat_f, orient_quat, velocity)
num_samps = len(traj)
# normalization occurs here:
# TODO FIGURE THIS OUT?
ell_dist = self.controller._get_ell_equiv_dist((ell_change_trans_ep, (0, 0, 0)),
((0, 0, 0), 0),
quat_gripper_rot, ELL_LOCAL_VEL)
change_trans_ep = tuple(np.array(change_trans_ep)/np.sum(change_trans_ep) *
ell_dist)
self.controller.execute_cart_move((change_trans_ep, (0, 0, 0)), ((0, 0, 0), 0),
quat_gripper_rot, CART_TRANS_VEL, num_samps=num_samps)
elif button_press in cart_rot_params:
change_rot_ep = cart_rot_params[button_press]
self.controller.execute_cart_move(((0, 0, 0), change_rot_ep), ((0, 0, 0), 0),
quat_gripper_rot, CART_ROT_VEL)
elif button_press == "reset_rotation":
self.controller.execute_cart_move(((0, 0, 0), np.mat(np.eye(3))), ((0, 0, 0), 1),
quat_gripper_rot, CART_ROT_VEL)
end_pos, _ = self.cart_arm.get_ep()
end_time = rospy.get_time()
dist = np.linalg.norm(end_pos - start_pos)
time_diff = end_time - start_time
print "%s, dist: %.3f, time_diff: %.2f" % (button_press, dist, time_diff)
self.button_distances[button_press].append(dist)
self.button_times[button_press].append(time_diff)
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 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 enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
now, rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
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))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [
curr_ell_pos[0] + change_trans_ep[0],
curr_ell_pos[1] + change_trans_ep[1],
curr_ell_pos[2] + change_trans_ep[2]
]
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
goal_ell_pos,
curr_ell_quat,
velocity=ELL_LOCAL_VEL,
min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos,
goal_ell_quat,
velocity=ELL_ROT_VEL,
min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos, (0., 0., 0., 1.),
velocity=ELL_ROT_VEL,
min_jerk=True)
else:
rospy.logerr(
"[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in 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)
self.force_monitor.update_activity()
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 enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side',
'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'],
bounds['height']) #TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id,
'/base_link', now,
rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose(
'/base_link', reg_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure converting ellipse to base frame: %s"
% e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(
self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l',
'%s_arm_controller',
reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
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
