def get_size_and_build_walls(self):
        # Get size of the ogrid ==============================================
        # Get some useful vectors
        between_vector = self.left_f - self.right_f
        mid_point = self.right_f + between_vector / 2
        target_vector = self.target - mid_point
        self.mid_point = mid_point

        # For rotations of the `between_vector` and the enu x-axis
        b_theta = np.arctan2(between_vector[1], between_vector[0])
        b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]

        # Make the endpoints
        rot_buffer = b_rot_mat.dot([20, 0, 0])
        endpoint_left_f = self.left_f + rot_buffer
        endpoint_right_f = self.right_f - rot_buffer

        # Now lets build some wall points ======================================

        if self.left_b is None:
            # For rotations of the `target_vector` and the enu x-axis
            t_theta = np.arctan2(target_vector[1], target_vector[0])
            t_rot_mat = trns.euler_matrix(0, 0, t_theta)[:3, :3]

            rot_channel = t_rot_mat.dot([self.channel_length, 0, 0])
            self.left_b = self.left_f + rot_channel
            self.right_b = self.right_f + rot_channel

        # Now draw contours from the boat to the start gate ====================
        # Get vector from boat to mid_point
        mid_point_vector = self.boat_pos - self.mid_point

        b_theta = np.arctan2(mid_point_vector[1], mid_point_vector[0])
        b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]

        rot_buffer = b_rot_mat.dot([np.linalg.norm(mid_point_vector) *  1.5, 0, 0])
        left_cone_point = self.left_f + rot_buffer
        right_cone_point = self.right_f + rot_buffer

        # Define bounds for the grid
        self.x_lower = min(left_cone_point[0], right_cone_point[0],
                           endpoint_left_f[0], endpoint_right_f[0], self.target[0]) - self.edge_buffer
        self.x_upper = max(left_cone_point[0], right_cone_point[0],
                           endpoint_left_f[0], endpoint_right_f[0], self.target[0]) + self.edge_buffer
        self.y_lower = min(left_cone_point[1], right_cone_point[1],
                           endpoint_left_f[1], endpoint_right_f[1], self.target[1]) - self.edge_buffer
        self.y_upper = max(left_cone_point[1], right_cone_point[1],
                           endpoint_left_f[1], endpoint_right_f[1], self.target[1]) + self.edge_buffer

        self.walls = [self.left_b, self.left_f, left_cone_point, right_cone_point, self.right_f, self.right_b]

 def _makePreGraspPose(self, boxMat, axis):
     if axis==0: #x axis
         alpha = 0
         gamma = 0
     else: #y axis
         alpha = pi/2
         gamma = -pi/2
     ik_request = PositionIKRequest()
     ik_request.ik_link_name = self.toolLinkName
     with self._joint_state_lock:
         ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states)
     ik_request.pose_stamped = PoseStamped()
     ik_request.pose_stamped.header.stamp = rospy.Time.now()
     ik_request.pose_stamped.header.frame_id = self.frameID
     beta = pi/2
     while beta < 3*pi/2:
         rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz')
         distance = self.preGraspDistance + self.gripperFingerLength
         preGraspMat = transformations.translation_matrix([0,0,-distance])
         fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat)
         p = transformations.translation_from_matrix(fullMat)
         q = transformations.quaternion_from_matrix(fullMat)
         print "trying {} radians".format(beta)
         try:
             self._ik_server(ik_request, Constraints(), rospy.Duration(5.0))
         except rospy.service.ServiceException:
             beta += 0.1
         else:
             if ik_resp.error_code.val > 0:
                 return beta
             else:
                 #print ik_resp.error_code.val
                 beta += 0.01
     rospy.logerr('No way to pick this up. All IK solutions failed.')
     return 7 * pi / 6

    def publish_state(self, t):
        state_msg = TransformStamped()
        t_ned_imu = tft.translation_matrix(self.model.get_position())
        R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
        T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
        #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
        if self.T_imu_vicon is None:
            # grab the static transform from imu to vicon frame from param server:
            frames = rospy.get_param("frames", None)
            ypr = frames['vicon_to_imu']['rotation']
            q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
            R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
            t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
#            rospy.loginfo(str(R_vicon_imu))
#            rospy.loginfo(str(t_vicon_imu))
            self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
            self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
            self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
            rospy.loginfo(str(self.T_enu_ned))
            rospy.loginfo(str(self.T_imu_vicon))
            #rospy.loginfo(str(T_vicon_imu))
        # we have /ned -> /imu, need to output /enu -> /vicon:
        T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
        state_msg.header.stamp  = t
        state_msg.header.frame_id = "/enu"
        state_msg.child_frame_id = "/simflyer1/flyer_vicon"
        stt = state_msg.transform.translation
        stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
        stro = state_msg.transform.rotation
        stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
        
        self.pub_state.publish(state_msg)

    def generate_ep(self):
        cart_cur = self.cart_arm.get_end_effector_pose()
        if self.step_ind < self.num_steps:
            self.cart_cur_goal = self.trajectory[self.step_ind]
            self.step_ind += 1
        else:
            self.cart_cur_goal = self.cart_goal
        self.cart_err = self.cart_arm.ep_error(cart_cur, self.cart_cur_goal)
#self.cart_err[np.where(np.fabs(self.cart_err) < self.err_goal_thresh * 0.6)] = 0.0
        cart_control = np.array([pidc.update_state(cart_err_i) for pidc, cart_err_i in 
                                zip(self.controllers, self.cart_err)])
        cep_pos_cur, cep_rot_cur = self.cart_arm.get_ep()
        cep_pos_new = cep_pos_cur - cart_control[:3,0]
        cart_rot_control = np.mat(tf_trans.euler_matrix(cart_control[3,0], cart_control[4,0], 
                                                 cart_control[5,0]))[:3,:3]
        cep_rot_new = cep_rot_cur * cart_rot_control.T
        ep_new = (cep_pos_new, cep_rot_new)
        if self.debug:
            print "="*50
            print "cart_cur", cart_cur
            print "-"*50
            print "cur goal", self.cart_cur_goal
            print "-"*50
            print "err", self.cart_err
            print "-"*50
            print "cart_control", cart_control
        return EPStopConditions.CONTINUE, ep_new

 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 = tf_trans.quaternion_from_euler(*rot_euler, axes='sxyz')
     homo_mat = np.mat(tf_trans.euler_matrix(*rot_euler))
     homo_mat[:3,3] = np.mat([pos]).T
     return homo_mat, quat, rot_euler

 def test_move_out_of_collision(self):
   env = orpy.Environment()
   env.Load('data/pumablocks.env.xml')
   body = env.GetKinBody('lego0')
   Tinit = body.GetTransform()
   def move_until_collision(step, direction, timeout=1.0):
     start_time = time.time()
     Tcollision = body.GetTransform()
     while not env.CheckCollision(body):
       Tcollision[:3,3] += step*np.array(direction)
       body.SetTransform(Tcollision)
       if time.time()-start_time > timeout:
         return False
     return True
   # Move down into collision with the table
   body.SetTransform(Tinit)
   self.assertTrue( move_until_collision(0.005, [0, -1, 0]) )
   result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.006)
   self.assertTrue(result)
   body.SetTransform(Tinit)
   self.assertTrue( move_until_collision(0.005, [0, -1, 0]) )
   result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.001)
   self.assertFalse(result)
   # Show we can cope with tilted objects
   body.SetTransform(Tinit)
   move_until_collision(0.005, [0, -1, 0])
   Toffset = tr.euler_matrix(np.deg2rad(10), 0, 0)
   Tbody = body.GetTransform()
   Tnew = np.dot(Tbody, Toffset)
   body.SetTransform(Tnew)
   result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.015)
   self.assertTrue(result)

    def calc_transformation(self, name, relative_to=None):
        calc_from_joint = False
        if relative_to:
            if relative_to in self.urdf.link_map:
                parent_link_name = relative_to
            elif relative_to in self.urdf.joint_map:
                parent_link_name = self.urdf.joint_map[name].parent
                calc_from_joint = True
        else:
            parent_link_name = self.urdf.get_root()

        calc_to_joint = False
        if name in self.urdf.link_map:
            child_link_name = name
        elif name in self.urdf.joint_map:
            child_link_name = self.urdf.joint_map[name].child
            calc_to_joint = True

        chains = self.urdf.get_chain(parent_link_name, child_link_name,
                                     joints=True, links=True)
        if calc_from_joint:
            chains = chains[1:]
        if calc_to_joint:
            chains = chains[:-1]

        poses = []
        for name in chains:
            if name in self.urdf.joint_map:
                joint = self.urdf.joint_map[name]
                if joint.origin is not None:
                    poses.append(joint.origin)
            elif name in self.urdf.link_map:
                link = self.urdf.link_map[name]
                if link.visual is not None and link.visual.origin is not None:
                    poses.append(link.visual.origin)
        m = np.dot(T.translation_matrix((0,0,0)),
                   T.euler_matrix(0,0,0))
        for p in poses:
            n = np.dot(T.translation_matrix(tuple(p.xyz)),
                       T.euler_matrix(*tuple(p.rpy)))
            m = np.dot(m, n)
        t = T.translation_from_matrix(m)
        q = T.quaternion_from_matrix(m)
        return tuple(t), (q[3], q[0], q[1], q[2])

 def pose_to_matrix(self, ps):
     trans = np.matrix([-ps.pose.position.x,
                        -ps.pose.position.y,
                        ps.pose.position.z]).T
     r, p, y = euler_from_quaternion([ps.pose.orientation.x,
                                      ps.pose.orientation.y,
                                      ps.pose.orientation.z,
                                      ps.pose.orientation.w])
     rot = np.matrix(euler_matrix(-r, -p, -y)[:3, :3]).T
     return rot, trans

def get_path(buoy_l, buoy_r):
    # Vector between the two buoys
    between_vector = buoy_r.position - buoy_l.position

    # Rotate that vector to point through the buoys
    r = trns.euler_matrix(0, 0, np.radians(90))[:3, :3]
    direction_vector = r.dot(between_vector)
    direction_vector /= np.linalg.norm(direction_vector)

    return between_vector, direction_vector

 def fromROSPose2DModel(self, model):
   self.type = model.type
   self.pose = LocalPose()
   matrix = euler_matrix(0.0, 0.0, model.pose.theta)
   matrix[0][3] = model.pose.x
   matrix[1][3] = model.pose.y
   self.pose.pose = fromMatrix(matrix)
   self.geometry_type = 'POSE2D'
   geometry_string = 'POINT(%f %f %f)' % (model.pose.x, model.pose.y, 0.0)
   self.geometry =  WKTElement(geometry_string)

	def test_to_tf(self):
		tb = tb_angles(45,-5,24)
		self.assertTrue(tb.to_tf().flags.writeable)
		
		for yaw in self.yaw:
			for pitch in self.pitch:
				for roll in self.roll:
					tb = tb_angles(yaw,pitch,roll)
					m = tb.to_tf()
					m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx')
					self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))

    def get_rot_matrix(self, rev=False):
        frm, to = 'ardrone/odom', 'ardrone/ardrone_base_bottomcam'
        if rev:
            frm, to = to, frm

        self.tf.waitForTransform(frm, to, rospy.Time(0), rospy.Duration(3))
        trans, rot = self.tf.lookupTransform(frm, to, rospy.Time(0))

        x, y, z = euler_from_quaternion(rot)
        yaw = euler_from_quaternion(rot, axes='szxy')[0]

        return yaw, np.array(euler_matrix(-x, -y, z))

def MsgToPose(msg):

	#Parse the ROS message to a 4x4 pose format

	#Get translation and rotation (from Euler angles)
	pose = transformations.euler_matrix(msg.roll,msg.pitch,msg.yaw) 

        pose[0,3] = msg.pt.x
        pose[1,3] = msg.pt.y
        pose[2,3] = msg.pt.z
    
	return pose

        def pub_head_registration(ud):
            cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose)
            # find the center of the ellipse given a cheek click
            cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx'))
            cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
            cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
            #b_B_c[0:3,0:3] = np.eye(3)
            norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
            head_rot = np.arctan2(norm_xy[1], norm_xy[0])
            cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3]
            self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
            ell_center = cheek_pose * cheek_transformation
            self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center))

            # create an ellipsoid msg and command it 
            ep = EllipsoidParams()
            ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
            ep.height = 0.924
            ep.E = 0.086
            self.ep_pub.publish(ep)

            return 'succeeded'

    def place_generator(self):

        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.57
        place_pose.pose.position.y = 0.16
        place_pose.pose.position.z = 0.56
        place_pose.pose.orientation.w = 1.0

        P = transformations.quaternion_matrix(
            [
                place_pose.pose.orientation.x,
                place_pose.pose.orientation.y,
                place_pose.pose.orientation.z,
                place_pose.pose.orientation.w,
            ]
        )
        P[0, 3] = place_pose.pose.position.x
        P[1, 3] = place_pose.pose.position.y
        P[2, 3] = place_pose.pose.position.z

        places = []
        yaw_angles = [0, 1, 57, -1, 57, 3, 14]
        x_vals = [0, 0.05, 0.1, 0.15]
        z_vals = [0.05, 0.1, 0.15]
        for y in yaw_angles:
            G = transformations.euler_matrix(0, 0, y)

            G[0, 3] = 0.0  # offset about x
            G[1, 3] = 0.0  # about y
            G[2, 3] = 0.0  # about z

            for z in z_vals:
                for x in x_vals:

                    TM = np.dot(P, G)
                    pl = deepcopy(place_pose)
                    pl.pose.position.x = TM[0, 3] + x
                    pl.pose.position.y = TM[1, 3]
                    pl.pose.position.z = TM[2, 3] + z

                    quat = transformations.quaternion_from_matrix(TM)
                    pl.pose.orientation.x = quat[0]
                    pl.pose.orientation.y = quat[1]
                    pl.pose.orientation.z = quat[2]
                    pl.pose.orientation.w = quat[3]
                    pl.header.frame_id = REFERENCE_FRAME
                    places.append(deepcopy(pl))

        return places

    def _MsgToPose(msg):
        """
        Parse the ROS message to a 4x4 pose format
        @param msg The ros message containing a pose
        @return A 4x4 transformation matrix containing the pose
        as read from the message
        """
        # Get translation and rotation (from Euler angles)
        pose = transformations.euler_matrix(msg.roll * 0.0, msg.pitch * 0.0, msg.yaw * 0.0)

        pose[0, 3] = msg.pt.x
        pose[1, 3] = msg.pt.y
        pose[2, 3] = msg.pt.z

        return pose

    def orient_division(self, grid, point, angle_offset_mult=1):
        """ Returns a position to go to on the other side of the line """
        angle_offset = 1 * angle_offset_mult  # rads

        point = np.array(point)
        r_pos = self.navigator.pose[0][:2] - point[:2]
        angle = np.arctan2(*r_pos[::-1]) - angle_offset
        
        center = np.array(grid.shape) / 2

        line_pt2 = trns.euler_matrix(0, 0, angle)[:3, :3].dot(np.array([1E3, 0, 0]))
        cv2.line(grid, tuple(center), tuple(line_pt2[:2].astype(np.int32)), 255, 2)

        # Let's put ourselves on the other side of that there line
        return self.generate_target(angle, r_pos) + point[:2]

	def __init__(self):
		rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.localCb)
		rospy.Subscriber("/itech_ros/mavros_offboard/enu", PoseStamped, self.enuCb)
		
		self.localEnuPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "local_enu"), PoseStamped, queue_size=10)
		self.offsetPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "offset"), PointStamped, queue_size=10)
		
		self.offset = None
		self.localPose = PoseModel()
		self.localEnuPose = PoseModel()
		
		#tick posee are the poses at the time that we get positioning data.
		self.enuTickPose = PoseModel()
		self.localTickPose = PoseModel()
		
		self.r = euler_matrix(0, 0, math.pi/-2)

def pose_relative_rot(pose, r=0., p=0., y=0., degrees=True):
    """Return a pose rotated relative to a given pose."""
    ps = deepcopy(pose) 
    if degrees:
        r = math.radians(r)
        p = math.radians(p)
        y = math.radians(y)
    des_rot_mat = tft.euler_matrix(r,p,y) 
    q_ps = [ps.pose.orientation.x, 
            ps.pose.orientation.y, 
            ps.pose.orientation.z, 
            ps.pose.orientation.w]
    state_rot_mat = tft.quaternion_matrix(q_ps) 
    final_rot_mat = np.dot(state_rot_mat, des_rot_mat) 
    ps.pose.orientation = Quaternion(
                            *tft.quaternion_from_matrix(final_rot_mat))
    return ps

    def post_grasp(self, new_pose, obj_idx, fl):

        ######### GRASP OBJECT/ REMOVE FROM SCENE ######.

        if fl == "true":
            self.close_gripper()
            self.aro = obj_idx
        else:
            self.open_gripper()
            self.aro = None
        rospy.sleep(2)

        ### POST GRASP RETREAT ###
        M1 = transformations.quaternion_matrix(
            [
                new_pose.pose.orientation.x,
                new_pose.pose.orientation.y,
                new_pose.pose.orientation.z,
                new_pose.pose.orientation.w,
            ]
        )
        M1[0, 3] = new_pose.pose.position.x
        M1[1, 3] = new_pose.pose.position.y
        M1[2, 3] = new_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 0, 0)

        M2[0, 3] = 0.0  # offset about x
        M2[1, 3] = 0.0  # about y
        M2[2, 3] = 0.25  # about z

        T1 = np.dot(M2, M1)
        npo = deepcopy(new_pose)
        npo.pose.position.x = T1[0, 3]
        npo.pose.position.y = T1[1, 3]
        npo.pose.position.z = T1[2, 3]

        quat = transformations.quaternion_from_matrix(T1)
        npo.pose.orientation.x = quat[0]
        npo.pose.orientation.y = quat[1]
        npo.pose.orientation.z = quat[2]
        npo.pose.orientation.w = quat[3]
        npo.header.frame_id = REFERENCE_FRAME
        self.right_arm.plan(npo.pose)
        self.right_arm.go(wait=True)