def arTagCallback(self, msg):

        markers = msg.markers

        main_tag_flag  = False
        
        for i in xrange(len(markers)):
            if markers[i].id > self.max_idx: continue
        
            if markers[i].id == self.main_tag_id:
                main_tag_flag = True
                self.main_tag_frame = posemath.fromMsg(markers[i].pose.pose)*self.z_neg90_frame 
                
                if self.main_tag_frame.p.Norm() > 2.0: return
                
        if main_tag_flag == False: return

        for i in xrange(len(markers)):

            if markers[i].id > self.max_idx: continue
        
            if markers[i].id != self.main_tag_id:                
                tag_id    = markers[i].id
                tag_frame = posemath.fromMsg(markers[i].pose.pose)*self.z_neg90_frame

                # position filtering
                if (self.main_tag_frame.p - tag_frame.p).Norm() > self.pos_thres : return
                
                frame_diff = self.main_tag_frame.Inverse()*tag_frame
                self.updateFrames(tag_id, frame_diff)

	def move_rel_to_cartesian_pose_with_contact(self, time_from_start, rel_pose, wrench_constraint):
		print self.BCOLOR+"[IRPOS] Move relative to cartesian trajectory with contact"+self.ENDC
				
		self.conmanSwitch([self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], [], True)
		time.sleep(0.05)

		actual_pose = self.get_cartesian_pose()
		
		# Transform poses to frames.
		actualFrame = pm.fromMsg(actual_pose)
		
		relativeFrame = pm.fromMsg(rel_pose)
		
		desiredFrame = actualFrame * relativeFrame
		
		pose = pm.toMsg(desiredFrame)

		cartesianGoal = CartesianTrajectoryGoal()
		cartesianGoal.wrench_constraint = wrench_constraint
		cartesianGoal.trajectory.points.append(CartesianTrajectoryPoint(rospy.Duration(time_from_start), pose, Twist()))
		cartesianGoal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)

		self.pose_client.send_goal(cartesianGoal)
		self.pose_client.wait_for_result()

		result = self.pose_client.get_result()
		code = self.cartesian_error_code_to_string(result.error_code)
		print self.BCOLOR+"[IRPOS] Result: "+str(code)+self.ENDC

		self.conmanSwitch([], [self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], True)

def get_ik_simple(service, goal, seed, joint_names, tip_frame, tool=None):
    validate_quat(goal.pose.orientation)
    
    # Transforms the goal due to the tip frame
    if tool:
        tool_in_tip = tfl.transformPose(tip_frame, tool)
        tool_in_tip_t = pm.fromMsg(tool_in_tip.pose)
        goal_t = pm.fromMsg(goal.pose)
        #goal.pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t)
        #goal = PoseStamped(header = goal.header, pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t))
        goal = PoseStamped(header = goal.header, pose = pm.toMsg(goal_t * tool_in_tip_t.Inverse()))

    req = PositionIKRequest()
    req.ik_link_name = tip_frame
    req.pose_stamped = goal
    req.ik_seed_state.joint_state.name = joint_names
    req.ik_seed_state.joint_state.position = seed

    error_code = 0
    for i in range(10):
        resp = service(req, rospy.Duration(10.0))
        if resp.error_code.val != 1:
            rospy.logerr("Error in IK: %d" % resp.error_code.val)
        else:
            return list(resp.solution.joint_state.position)
    return []

    def read_tfs(self,req):
        #marker w.r.t. camera\print

        ok=True

        #read target w.r.t. camera

        w_P_ee=self.current_pose(self.robot_ee_frame_name,self.base_frame_name)
        if(w_P_ee==0):
            ok=False
        c_P_m=self.current_pose(self.marker_frame_name,self.camera_frame_name)
        if(c_P_m==0):
            ok=False

        #ee w.r.t. base


        if ok:
            print self.base_frame_name+" -> "+self.robot_ee_frame_name
            print w_P_ee
            print self.camera_frame_name + " -> " + self.marker_frame_name
            print c_P_m
            #save data
            #safe_pose_to_file(self.f,w_P_ee)
            #safe_pose_to_file(self.f,c_P_m)
            self.crc.store_frames(posemath.fromMsg( w_P_ee),posemath.fromMsg(c_P_m))
            print "saved so far"
            print len(self.crc._w_T_ee)
        else:
            print "error in retrieving a frame"

        return EmptyResponse();

                def plug_in_contact(ud):
                    """Returns true if the plug is in contact with something."""

                    pose_base_gripper = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame').pose)
                    pose_outlet_plug = fromMsg(ud.base_to_outlet).Inverse() * pose_base_gripper * fromMsg(ud.gripper_to_plug)

                    # check if difference between desired and measured outlet-plug along x-axis is more than 1 cm
                    ud.outlet_to_plug_contact = toMsg(pose_outlet_plug)
                    if math.fabs(pose_outlet_plug.p[0] - ud.approach_offset) > 0.01:
                        return True
                    return False

 def optitrack_cb(self, msg):
   if msg.header.frame_id != self.global_frame:
     return
   for rigid_body in msg.bodies:
     if rigid_body.tracking_valid:
       if rigid_body.id == self.table_id:
         frame = posemath.fromMsg(rigid_body.pose)
         self.Ttable = posemath.toMatrix(frame)
       if rigid_body.id == self.stick_id:
         frame = posemath.fromMsg(rigid_body.pose)
         self.Tshort = posemath.toMatrix(frame)

def get_outlet_to_plug_ik_goal(ud, pose):
    """Get an IK goal for a pose in the frame of the outlet"""
    pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link", 
                                                        rospy.Time.now(), rospy.Duration(2.0)).pose)

    goal = ArmMoveIKGoal()
    goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
    goal.pose.pose = toMsg(fromMsg(ud.base_to_outlet) * pose * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist)
    goal.pose.header.stamp = rospy.Time.now()
    goal.pose.header.frame_id = 'base_link'

    return goal

def oplus(cur_estimate, step):
    result = deepcopy(cur_estimate)

    # loop over camera's
    for camera, res, camera_index in zip(cur_estimate.cameras, result.cameras, [r*pose_width for r in range(len(cur_estimate.cameras))]):
        res.pose = posemath.toMsg(pose_oplus(posemath.fromMsg(camera.pose), step[camera_index:camera_index+pose_width]))
    # loop over targets
    for i, target in enumerate(cur_estimate.targets): 
        target_index = (len(cur_estimate.cameras) + i) * pose_width
        result.targets[i] = posemath.toMsg(pose_oplus(posemath.fromMsg(target), step[target_index:target_index+pose_width]))

    return result

def calculate_residual_and_jacobian(cal_samples, cur_estimate):
    """
    returns the full residual vector and jacobian
    """
    # Compute the total number of rows. This is the number of poses * 6
    num_cols = len(cur_estimate.cameras) * pose_width + len(cur_estimate.targets) * pose_width
    num_rows = sum ([ sum([  len(cam.features.image_points) for cam in cur_sample.M_cam]) for cur_sample in cal_samples]) * feature_width

    J = matrix(zeros([num_rows, num_cols]))
    residual = matrix(zeros([num_rows, 1]))

    cam_pose_dict  = dict( [ (cur_camera.camera_id, posemath.fromMsg(cur_camera.pose))  for cur_camera in cur_estimate.cameras] )
    cam_index_dict = dict( [ (cur_camera.camera_id, cur_index)        for cur_camera, cur_index  in zip ( cur_estimate.cameras, range(len(cur_estimate.cameras)) )] )

    # Start filling in the jacobian
    cur_row = 0;
    # Loop over each observation
    for cur_sample, target_pose_msg, target_index in zip(cal_samples, cur_estimate.targets, range(len(cur_estimate.targets))):
        for cam_measurement in cur_sample.M_cam:
            # Find the index of this camera
            try:
                cam_pose = cam_pose_dict[cam_measurement.camera_id]
                cam_index     = cam_index_dict[cam_measurement.camera_id]
            except KeyError:
                print "Couldn't find current camera_id in cam_pose dictionary"
                print "  camera_id = %s", cam_measurement.camera_id
                print "  %s", cam_pose_dict.keys()
                raise

            # ROS Poses  -> KDL Poses
            target_pose = posemath.fromMsg(target_pose_msg)

            end_row = cur_row + len(cam_measurement.features.image_points)*feature_width

            # ROS Target Points -> (4xN) Homogenous coords
            target_pts = matrix([ [pt.x, pt.y, pt.z, 1.0] for pt in cam_measurement.features.object_points ]).transpose()

            # Save the residual for this cam measurement
            measurement_vec = matrix( concatenate([ [cur_pt.x, cur_pt.y] for cur_pt in cam_measurement.features.image_points]) ).T
            expected_measurement = sub_h(cam_pose, target_pose, target_pts, cam_measurement.cam_info)
            residual[cur_row:end_row, 0] =  measurement_vec - expected_measurement 

            # Compute jacobian for this cam measurement
            camera_J = calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_measurement.cam_info, use_cam = True)
            #print "Camera Jacobian [%s]]" % cam_measurement.camera_id
            #print camera_J
            target_J = calculate_sub_jacobian(cam_pose, target_pose, target_pts, cam_measurement.cam_info, use_cam = False)
            J[cur_row:end_row, cam_index*pose_width:((cam_index+1)*pose_width)] = camera_J
            col_start = (len(cur_estimate.cameras) + target_index)*pose_width
            J[cur_row:end_row, col_start:(col_start+pose_width)] = target_J

            cur_row = end_row
    return residual, J

	def map_to_world(self, x, y):
		if not self.map:
			raise NotEnoughDataException('no map message received.')
		stamped = PoseStamped()
		stamped.header.frame_id = self.map.header.frame_id
		stamped.pose.position.x = x * self.map.info.resolution
		stamped.pose.position.y = y * self.map.info.resolution
		stamped.pose.orientation.w = 1
		origin_transform = pm.fromMsg(self.map.info.origin)
		center_transform = pm.fromMsg(stamped.pose)
		stamped.pose = pm.toMsg(origin_transform * center_transform)
		return stamped

        def get_straighten_goal(ud, goal):
            """Generate an ik goal to straighten the plug in the outlet."""
            pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)

            pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist

            goal = ArmMoveIKGoal()
            goal.pose.pose = toMsg(pose_base_wrist)
            goal.pose.header.stamp = rospy.Time.now()
            goal.pose.header.frame_id = 'base_link'
            goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
            goal.move_duration = rospy.Duration(0.5)
            return goal

	def is_good_approach_pose(self, stamped, banner):
		robot_frame = pm.fromMsg(stamped.pose)
		banner_frame = pm.fromMsg(banner.pose.pose)
		view_ray = banner_frame.p - robot_frame.p
		distance = view_ray.Norm()
		if distance > Params().approach_distance_tolerance:
			rospy.loginfo('bad approach pose: distance {}'.format(distance))
			return False
		angle = self._get_viewing_angle(robot_frame, banner_frame)
		if abs(angle) > Params().max_approach_angle:
			rospy.loginfo('bad approach pose: angle {}'.format(math.degrees(angle)))
			return False
		return True

	def is_good_verification_pose(self, stamped, banner):
		robot_frame = pm.fromMsg(stamped.pose)
		banner_frame = pm.fromMsg(banner.pose.pose)
		view_ray = banner_frame.p - robot_frame.p
		distance = view_ray.Norm()
		if distance < Params().min_verification_distance or distance > Params().max_verification_distance*1.5:
			rospy.loginfo('bad verification pose: distance {}'.format(distance))
			return False
# 		angle = self._get_facing_angle(robot_frame, banner_frame)
# 		if abs(angle) > Params().max_verification_angle:
# 			rospy.loginfo('bad verification pose: angle {}'.format(math.degrees(angle)))
# 			return False
		return True

	def sample_approach_from_averaged(self, line1, line2):
		line1_frame = pm.fromMsg(line1)
		line2_frame = pm.fromMsg(line2)
		averaged_frame = PyKDL.Frame()
		averaged_frame.p = (line1_frame.p + line2_frame.p) * 0.5
		[roll, pitch, yaw1] = line1_frame.M.GetRPY()
		[roll, pitch, yaw2] = line2_frame.M.GetRPY()
		averaged_frame.M = PyKDL.Rotation.RPY(0, 0, (yaw1+yaw2)/2.0)
		approach_frame = PyKDL.Frame()
		approach_frame.p = averaged_frame.p + averaged_frame.M * PyKDL.Vector(Params().approach_distance * 1.5, 0, 0)
		[roll, pitch, yaw] = averaged_frame.M.GetRPY()
		approach_frame.M = PyKDL.Rotation.RPY(0, 0, yaw+math.pi)
		return pm.toMsg(approach_frame)

                def get_twist_goal(ud, goal):
                    """Generate an ik goal to rotate the plug"""
                    pose_contact_plug = PyKDL.Frame(PyKDL.Rotation.RPY(ud.twist_angle, 0, 0))
                    pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)

                    pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * pose_contact_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist

                    goal = ArmMoveIKGoal()
                    goal.pose.pose = toMsg(pose_base_wrist)
                    goal.pose.header.stamp = rospy.Time.now()
                    goal.pose.header.frame_id = 'base_link'
                    goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
                    goal.move_duration = rospy.Duration(1.0)
                    return goal

        def get_pull_back_goal(ud, goal):
            """Generate an ik goal to move along the local x axis of the outlet."""
            pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0))
            pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)

            pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist

            goal = ArmMoveIKGoal()
            goal.pose.pose = toMsg(pose_base_wrist)
            goal.pose.header.stamp = rospy.Time.now()
            goal.pose.header.frame_id = 'base_link'
            goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
            goal.move_duration = rospy.Duration(3.0)
            return goal

        def get_grasp_plug_goal(ud, goal):
            """Get the ik goal for grasping the plug."""
            pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)

            goal = ArmMoveIKGoal()
            goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose)
                                   * fromMsg(ud.gripper_plug_grasp_pose).Inverse()
                                   * pose_gripper_wrist)

            goal.pose.header.stamp = rospy.Time.now()
            goal.pose.header.frame_id = 'base_link'
            goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed')
            goal.move_duration = rospy.Duration(3.0)

            return goal

def processFeedback(feedback):

    global action_trajectory_client

    if action_trajectory_client == None:
        return

#    print "feedback", feedback.marker_name, feedback.control_name, feedback.event_type

    if ( feedback.marker_name == 'head_position_marker' ) and ( feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK ) and ( feedback.control_name == "button" ):
        T_B_Td = pm.fromMsg(feedback.pose)
        rz, ry, rx = T_B_Td.M.GetEulerZYX()
        duration = 3.0

        action_trajectory_goal = control_msgs.msg.FollowJointTrajectoryGoal()
        
        action_trajectory_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
        action_trajectory_goal.trajectory.joint_names = ['head_pan_joint', 'head_tilt_joint']
        pt = trajectory_msgs.msg.JointTrajectoryPoint()
        pt.positions = [rz, ry]
        pt.time_from_start = rospy.Duration(duration)
        action_trajectory_goal.trajectory.points.append(pt)

        action_trajectory_client.send_goal(action_trajectory_goal)

        print "moving the head in %s seconds..."%(duration)

 def markerCallback(data):
     marker_lock.acquire()
     self.visible_markers = []
     for m in data.markers:
         if m.id in marker_list:
             self.visible_markers.append( (m.id, pm.fromMsg(m.pose.pose)) )
     marker_lock.release()

    def servo_to_pose_call(self,req): 
        #rospy.loginfo('Recieved servo to pose request')
        #print req
        if self.driver_status == 'SERVO':
            T = pm.fromMsg(req.target)

            # Check acceleration and velocity limits
            (acceleration, velocity) = self.check_req_speed_params(req) 

            # inverse kinematics
            traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter)
            if len(traj.points) == 0:
                (code,res) = self.planner.getPlan(req.target,self.q0) # find a non-local movement
                if not res is None:
                    traj = res.planned_trajectory.joint_trajectory

            #if len(traj) == 0:
            #    traj.append(self.planner.ik(T,self.q0))

            # Send command
            if len(traj.points) > 0:
                rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
                return self.send_trajectory(traj,acceleration,velocity)
            else:
                rospy.logerr('SIMPLE DRIVER -- IK failed')
                return 'FAILURE - not in servo mode'
        else:
            rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
            return 'FAILURE - not in servo mode'