def main():
rospy.init_node("visualize_poses")
pose_file = file(sys.argv[1], 'r')
params = yaml.load(pose_file)
ell_reg_bag = rosbag.Bag(sys.argv[2], 'r')
for topic, ell_reg, ts in ell_reg_bag.read_messages():
pass
ell_space = EllipsoidSpace(E=ell_reg.E)
pub_head_pose = rospy.Publisher("/head_center_test", PoseStamped)
pub_arrows = rospy.Publisher("visualization_markers_array", MarkerArray)
def create_tool_arrow():
arrows = MarkerArray()
color = ColorRGBA(0., 0., 1., 1.)
for i, param in enumerate(params):
ell_pos, ell_rot = params[param]
_, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
arrow = create_arrow_marker(cart_pose, i, color)
arrow.header.stamp = rospy.Time.now()
arrows.markers.append(arrow)
return arrows
r = rospy.Rate(10)
while not rospy.is_shutdown():
pub_head_pose.publish(PoseConv.to_pose_stamped_msg("/base_link", [0]*3, [0]*3))
arrows = create_tool_arrow()
pub_arrows.publish(arrows)
r.sleep()
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)
frame = "/ellipse_frame"
pose_stamped = PoseConv.to_pose_stamped_msg(frame, pose)
#self.tmp_pub.publish(pose_stamped)
return pose_stamped
def load_params(self, params):
kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
base_B_kinect = self.kin_head.forward(base_link="base_link",
end_link=self.kinect_frame)
base_B_head = base_B_kinect * kinect_B_head
self.head_center = PoseConv.to_pose_stamped_msg("/base_link",base_B_head)
self.ell_space = EllipsoidSpace()
self.ell_space.load_ell_params(params.E, params.is_oblate, params.height)
rospy.loginfo("Loaded ellispoidal parameters.")
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("/face_side", 'r')
bag_str = self.reg_dir + '/' + '_'.join(
[self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf
is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback(
"Registration failed: Error loading saved registration.")
rospy.logerr(
"[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo(
"[%s] Requesting head registration for %s at pixel (%d, %d)." %
(rospy.get_name(), self.subject, req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0))
and (self.head_pc_reg.pose.orientation == Quaternion(
0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" % se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(
Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 *
head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback(
"Registered head using %s cheek model, please check and confirm." %
side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
def load_params(self, params):
kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
base_B_kinect = self.kin_head.forward(base_link="base_link",
end_link=self.kinect_frame)
base_B_head = base_B_kinect * kinect_B_head
self.head_center = PoseConv.to_pose_stamped_msg(
"/base_link", base_B_head)
self.ell_space = EllipsoidSpace()
self.ell_space.load_ell_params(params.E, params.is_oblate,
params.height)
rospy.loginfo("Loaded ellispoidal parameters.")
def _run_traj(self, traj, blocking=True):
self.start_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0]))
self.end_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1]))
# make sure traj beginning is close to current end effector position
init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05)
init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12)
ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose())
_, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T))
pos_diff = np.linalg.norm(ee_pos - traj[0][0])
if pos_diff > init_pos_tolerance:
rospy.logwarn("[controller_base] End effector too far from current position. " +
"Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance))
return False
if np.linalg.norm(rot_diff) > init_rot_tolerance:
rospy.logwarn("[controller_base] End effector too far from current rotation. " +
"Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff),
init_rot_tolerance))
return False
return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)
def __init__(self):
# Parse the yaml file to obtain the trajectory
time_step = float(read_parameter('~time_step', 0.05))
points = read_parameter('~points', list())
frame_id = read_parameter('~frame_id', 'base_link')
# Set-up publishers/subscribers
cmd_pub = rospy.Publisher('/grips/ik_command', PoseStamped)
# Give it time to create the publisher
rospy.sleep(2.0)
for point in points:
pose = PoseConv.to_pose_stamped_msg(frame_id, point[4:7], point[:4])
cmd_pub.publish(pose)
try:
rospy.sleep(time_step)
except:
pass
def init_reg_cb(self, req):
# TODO REMOVE THIS FACE SIDE MESS
self.publish_feedback("Performing Head Registration. Please Wait.")
self.face_side = rospy.get_param("~face_side1", 'r')
bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
try:
bag = rosbag.Bag(bag_str, 'r')
for topic, msg, ts in bag.read_messages():
head_tf = msg
assert (head_tf is not None), "Error reading head transform bagfile"
bag.close()
except Exception as e:
self.publish_feedback("Registration failed: Error loading saved registration.")
rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
(rospy.get_name(), bag_str, e))
return (False, PoseStamped())
if self.face_side == 'r':
head_registration = self.head_registration_r
else:
head_registration = self.head_registration_l
try:
rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
self.subject,
req.u, req.v))
self.head_pc_reg = head_registration(req.u, req.v).reg_pose
if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
(self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
raise rospy.ServiceException("Unable to find a good match.")
self.head_pc_reg = None
except rospy.ServiceException as se:
self.publish_feedback("Registration failed: %s" %se)
return (False, PoseStamped())
pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
head_tf.transform.rotation))
self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
self.head_pose.header.stamp = self.head_pc_reg.header.stamp
side = "right" if (self.face_side == 'r') else "left"
self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
# rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
self.test_pose.publish(self.head_pose)
return (True, self.head_pose)
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 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)
#!/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 set_ep(self, pose):
cep_pose_stmp = PoseConv.to_pose_stamped_msg('torso_lift_link', pose)
self.command_pose_pub.publish(cep_pose_stmp)
def main():
if len(sys.argv) < 3:
print 'grab_cbs_auto cb_config.yaml output_bag.bag'
return
rospy.init_node("grab_cbs")
f = file(sys.argv[1], 'r')
cb_config = yaml.safe_load(f.read())
print cb_config
f.close()
is_kinect = rospy.get_param("/grab_cbs/is_kinect", True)
# load cb stuff
chessboard = ChessboardInfo()
chessboard.n_cols = cb_config['cols'] # 6
chessboard.n_rows = cb_config['rows'] # 7
chessboard.dim = cb_config['dim'] # 0.0258
calib = Calibrator([chessboard])
bridge = CvBridge()
l = DataListener(is_kinect, bridge, calib)
tf_list = tf.TransformListener()
cb_knowns = []
for j in range(chessboard.n_cols):
for i in range(chessboard.n_rows):
cb_knowns.append((chessboard.dim*i, chessboard.dim*j, 0.0))
bag = rosbag.Bag(sys.argv[2], 'w')
i = 0
while not rospy.is_shutdown():
if raw_input("Press enter to take CB, type 'q' to quit: ") == "q":
break
tries = 0
while not rospy.is_shutdown() and tries < 3:
corners = l.wait_for_new(5.)
if corners is None:
print "No corners detected"
tries += 1
continue
corners_2d = np.uint32(np.round(corners)).tolist()
corners_3d = []
if is_kinect:
for x,y,z in read_points(l.cur_pc, field_names=['x', 'y', 'z'], uvs=corners_2d):
corners_3d.append((x,y,z))
frame_id = l.cur_pc.header
else:
obj_pts = cv.fromarray(np.array(cb_knowns))
img_pts = cv.fromarray(np.array(corners))
K = cv.fromarray(np.reshape(l.cam_info.K,(3,3)))
D = cv.fromarray(np.array([l.cam_info.D]))
R_vec = cv.fromarray(np.zeros((3,1)))
t = cv.fromarray(np.zeros((3,1)))
cv.FindExtrinsicCameraParams2(obj_pts, img_pts, K, D, R_vec, t)
R_mat = cv.fromarray(np.zeros((3,3)))
cv.Rodrigues2(R_vec, R_mat)
T = PoseConv.to_homo_mat(np.mat(np.asarray(t)).T.A.tolist(),
np.mat(np.asarray(R_mat)).A.tolist())
cb_knowns_mat = np.vstack((np.mat(cb_knowns).T, np.mat(np.ones((1, len(cb_knowns))))))
corners_3d = np.array((T * cb_knowns_mat)[:3,:].T)
frame_id = l.cur_img.header
print corners_3d
if np.any(np.isnan(corners_3d)):
print "Pointcloud malformed"
tries += 1
continue
now = rospy.Time.now()
corners_pc = create_cloud_xyz32(frame_id, corners_3d)
try:
pose = tf_list.lookupTransform('/base_link', '/ee_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "TF screwed up..."
continue
bag.write("/pose", PoseConv.to_pose_stamped_msg('/base_link', pose), now)
bag.write("/pc", corners_pc, now)
print "Wrote pose/CB to bag file"
break
i += 1
bag.close()
