def getTransformations(self):
        pose = self.listener.lookupTransform('torso_base', self.prefix+'_arm_7_link', rospy.Time(0))
        self.T_B_W = pm.fromTf(pose)

        pose_tool = self.listener.lookupTransform(self.prefix+'_arm_7_link', self.prefix+'_arm_tool', rospy.Time(0))
        self.T_W_T = pm.fromTf(pose_tool)
        self.T_T_W = self.T_W_T.Inverse()

    def handle_cart_cmd(self, msg):
        side = self.side
        try:
            # Get the current position of the hydra
            hydra_frame = fromTf(self.listener.lookupTransform(
                '/hydra_base',
                '/hydra_'+self.SIDE_STR[side]+'_grab',
                rospy.Time(0)))

            # Get the current position of the end-effector
            tip_frame = fromTf(self.listener.lookupTransform(
                '/world',
                self.tip_link,
                rospy.Time(0)))

            # Capture the current position if we're starting to move
            if not self.deadman_engaged:
                self.deadman_engaged = True
                self.cmd_origin = hydra_frame
                self.tip_origin = tip_frame
            else:
                self.deadman_max = max(self.deadman_max, msg.axes[self.DEADMAN[side]])
                # Update commanded TF frame
                cmd_twist = kdl.diff(self.cmd_origin, hydra_frame)
                cmd_twist.vel = self.scale*self.deadman_max*cmd_twist.vel
                self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
            rospy.logwarn(str(ex))

    def _collect_static_tfs(self):

        while len(self._missing_frames) > 0 and not rospy.is_shutdown():
            self._lock.acquire()
            for frame in self._missing_frames:

                for trial in range(10):
                    if frame.rfind("gripper") > 0:
                        try:
                            (trans, rot) = self._tf_listener.lookupTransform(frame + "_base", frame, rospy.Time(0))
                            self._tfs[frame] = posemath.fromTf((trans, rot))
                            break

                        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                            rospy.sleep(0.5)

                    else:
                        try:
                            (trans, rot) = self._tf_listener.lookupTransform("/base", frame, rospy.Time(0))
                            self._missing_frames.remove(frame)
                            self._tfs[frame] = posemath.fromTf((trans, rot))
                            break

                        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                            rospy.sleep(0.5)

            self._lock.release()
            self._rate.sleep()

    def handle_cart_cmd(self, scaling):
        """"""

        try:
            # Get the current position of the hydra
            input_frame = fromTf(self.listener.lookupTransform(
                self.input_ref_frame_id,
                self.input_frame_id,
                rospy.Time(0)))

            # Get the current position of the end-effector
            tip_frame = fromTf(self.listener.lookupTransform(
                self.input_ref_frame_id,
                self.tip_link,
                rospy.Time(0)))

            # Capture the current position if we're starting to move
            if not self.deadman_engaged:
                self.deadman_engaged = True
                self.cmd_origin = input_frame
                self.tip_origin = tip_frame
            else:
                self.cart_scaling = scaling
                # Update commanded TF frame
                cmd_twist = kdl.diff(self.cmd_origin, input_frame)
                cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel
                #rospy.logwarn(cmd_twist)
                self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)

        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
            rospy.logwarn(str(ex))

    def getContactPointsInFrame(self, threshold, frame_name):
        self.tactile_lock.acquire()
        latest_index = copy.copy(self.tactile_data_index)
        self.tactile_lock.release()

        tactile_frames_names = [
        '/'+self.prefix+'_HandFingerOneKnuckleThreeLink',
        '/'+self.prefix+'_HandFingerTwoKnuckleThreeLink',
        '/'+self.prefix+'_HandFingerThreeKnuckleThreeLink',
        '/'+self.prefix+'_HandPalmLink']
        contacts = []
        forces = []

        pressure_frames = [self.pressure_frames, self.pressure_frames, self.pressure_frames, self.palm_pressure_frames]
        for tact_idx in range(len(tactile_frames_names)):
            tact_name = tactile_frames_names[tact_idx]
            for buf_prev_idx in range(20, self.tactile_data_len-2):
                buf_idx = latest_index - buf_prev_idx
#                buf_idx_prev = buf_idx - 1
                if buf_idx < 0:
                    buf_idx += self.tactile_data_len
#                if buf_idx_prev < 0:
#                    buf_idx_prev += self.tactile_data_len

                time = self.tactile_data[buf_idx][0]
                tactile_data = self.tactile_data[buf_idx][tact_idx+1]

                if self.listener.canTransform('torso_base', tact_name, time) and self.listener.canTransform('torso_base', frame_name, time):
                    T_B_F = pm.fromTf(self.listener.lookupTransform('torso_base', tact_name, time))
                    T_B_R = pm.fromTf(self.listener.lookupTransform('torso_base', frame_name, time))
                    T_R_B = T_B_R.Inverse()
                    for i in range(0, len(pressure_frames[tact_idx])):
#                        print "i"
                        neighbourhood_ok = True
                        # check the time neighbourhood
                        for buf_neigh in range(-19, 19):
                            buf_neigh_idx = buf_idx+buf_neigh
                            if buf_neigh_idx < 0:
                                buf_neigh_idx += self.tactile_data_len
                            elif buf_neigh_idx >= self.tactile_data_len:
                                buf_neigh_idx -= self.tactile_data_len
#                            print buf_neigh_idx
#                            print self.tactile_data[0][1]
                            if self.tactile_data[buf_neigh_idx][tact_idx+1][i] < threshold:
#                            if self.tactile_data[0][1][0] < threshold:
                                neighbourhood_ok = False
                                break
                        if neighbourhood_ok:#tactile_data[i] > threshold:
#                            contacts.append( T_R_B * T_B_F * pressure_frames[tact_idx][i] * PyKDL.Vector() )
                            h1 = self.pressure_info.sensor[tact_idx].halfside1[i]
                            h2 = self.pressure_info.sensor[tact_idx].halfside2[i]
                            contacts.append( (T_R_B * T_B_F * pressure_frames[tact_idx][i], PyKDL.Vector(h1.x, h1.y, h1.z).Norm(), PyKDL.Vector(h2.x, h2.y, h2.z).Norm()) )
                    break

        return contacts

def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed):

    '''
    this function assumes everything is represented in the quaternions in the /base_link frame
    '''
    if object_name.endswith('_scan'):
        object_name = object_name[:-5]
    dictObj = objDict()
    objSpec = dictObj.getEntry(object_name)

    F_bin_frame = posemath.fromTf(bin_frame)
    F_obj_frame = posemath.fromTf(obj_frame)

    objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0)
    objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0)
    objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0)
    
    binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0)
    binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0)
    binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0)
    
    rRProj = kdl.dot(objRed , binRed)
    gRProj = kdl.dot(objGreen, binRed)
    bRProj = kdl.dot(objBlue, binRed)

    tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)]

    if simtrackUsed:
        for i in range(3):
            if i in objSpec.invalidApproachAxis:
                tmpApproach1[i] = 0

    tmpApproach2 = [rRProj, gRProj, bRProj]
    axisApproach = tmpApproach1.index(max(tmpApproach1))


    objAxes = [objRed, objGreen, objBlue]
    tmpGrasp1 = []

    for i in range(3):
        if simtrackUsed:
            if i == axisApproach or i in objSpec.invalidGraspAxis:
                tmpGrasp1.append(0)
                continue
        tmpGrasp1.append(kdl.dot(objAxes[i], binBlue))

    tmpGrasp2 = [abs(t) for t in tmpGrasp1]

    axisGrasp = tmpGrasp2.index(max(tmpGrasp2))


    return axisApproach, tmpApproach2[axisApproach]/abs(tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp]/abs(tmpGrasp1[axisGrasp])

    def within_tolerance(self, tip_pose, target_pose, scale=1.0):
        # compute cartesian command error
        tip_frame = fromTf(tip_pose)
        target_frame = fromTf(target_pose)

        twist_err = kdl.diff(tip_frame, target_frame)
        linear_err = twist_err.vel.Norm()
        angular_err = twist_err.rot.Norm()

        #print("linear: %g, angular %g" % (linear_err, angular_err))

        # decide if planning is needed
        return linear_err < scale*self.linear_err_threshold and angular_err < scale*self.angular_err_threshold

def omni_callback(joint_state):
    global update_pub, last_button_state
    sendTf(marker_tf, '/marker', fixed_frame)
    sendTf(zero_tf, '/base', fixed_frame)
    sendTf(marker_ref, '/marker_ref', fixed_frame)
    sendTf(stylus_ref, '/stylus_ref', fixed_frame)
        
    try:
        update = InteractionCursorUpdate()
        update.pose.header = std_msgs.msg.Header()
        update.pose.header.stamp = rospy.Time.now()
        update.pose.header.frame_id = 'marker_ref'
        if button_clicked and last_button_state == update.GRAB:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked and last_button_state == update.KEEP_ALIVE:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked:
            update.button_state = update.GRAB
        elif last_button_state == update.KEEP_ALIVE:
            update.button_state = update.RELEASE
        else:
            update.button_state = update.NONE
            updateRefs()
        update.key_event = 0

        control_tf = ((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, control_offset))
        if button_clicked:
            # Get pose corresponding to transform between stylus reference and current position.
            stylus_tf = listener.lookupTransform('/stylus_ref', '/stylus', rospy.Time(0))
            stylus_matrix = pm.toMatrix(pm.fromTf(stylus_tf))
            control_matrix = pm.toMatrix(pm.fromTf(control_tf))
            p = pm.toMsg(pm.fromMatrix(numpy.dot(control_matrix, stylus_matrix)))
        else:
            p = pm.toMsg(pm.fromTf(control_tf))
        
        # Simply scale this up a bit to increase the workspace.
        workspace = 4
        p.position.x = p.position.x * workspace
        p.position.y = p.position.y * workspace
        p.position.z = p.position.z * workspace

        update.pose.pose = p

        last_button_state = update.button_state

        # Publish feedback.
        update_pub.publish(update)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        rospy.logerr("Couldn't look up transform. These things happen...")

    def calib(self):

        self._left_gripper_pose = kdl.Frame()
        self._right_gripper_pose = kdl.Frame()
        self._tote_pose = kdl.Frame()

        while not rospy.is_shutdown():
            try:
                (trans, rot) = self._tf_listener.lookupTransform('/base', 'left_gripper', rospy.Time(0))
                self._left_gripper_pose = posemath.fromTf((trans, rot))

                (trans, rot) = self._tf_listener.lookupTransform('/base', 'right_gripper', rospy.Time(0))
                self._right_gripper_pose = posemath.fromTf((trans, rot))
                break
            except:
                rospy.logerr(rospy.get_name() + ': could not get left/right gripper poses for calibration of the tote')
                rospy.sleep(1.0)

        vl = copy.deepcopy(self._left_gripper_pose.p)
        vl[2] = 0.0

        vr = copy.deepcopy(self._right_gripper_pose.p)
        vr[2] = 0.0

        v = vr - vl

        # center on the lower edge of tote closest to the Baxter
        c = vl + 0.5*v

        # calculate the perpendicular vector
        v.Normalize()
        v_perp = kdl.Rotation.RotZ(0.5*np.pi)*v

        # calculate the center of the tote
        center = c + self._tote_width*0.5*v_perp
        self._tote_pose.p[0] = center[0]
        self._tote_pose.p[1] = center[1]
        self._tote_pose.p[2] = (self._left_gripper_pose.p[2] + self._right_gripper_pose.p[2])*0.5-self._tote_height


        # calculate the angle of rotation along the Z axis
        rotz_angle = np.arctan(v[0]/-v[1])
        rotz_angle = rotz_angle -np.pi*0.5
        R = kdl.Rotation.RPY(0, 0, rotz_angle)

        self._tote_pose.M = R

        return True

 def get_new_waypoint(self,obj):
     try:
         (trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
         return pm.toMsg(pm.fromTf((trans,rot)))
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
     return None

 def getJarMarkerPose(self):
     try:
         self.listener.waitForTransform('torso_base', 'ar_marker_0', rospy.Time.now(), rospy.Duration(4.0))
         jar_marker = self.listener.lookupTransform('torso_base', 'ar_marker_0', rospy.Time(0))
     except:
         return None
     return pm.fromTf(jar_marker)

    def get_waypoints(self,frame_type,predicates,transforms,names):
        self.and_srv.wait_for_service()

        type_predicate = PredicateStatement()
        type_predicate.predicate = frame_type
        type_predicate.params = ['*','','']

        res = self.and_srv([type_predicate]+predicates)

        print "Found matches: " + str(res.matching)
        #print res

        if (not res.found) or len(res.matching) < 1:
          return None

        poses = []
        for tform in transforms:
            poses.append(pm.fromMsg(tform))

        #print poses
        new_poses = []
        new_names = []

        for match in res.matching:
            try:
                (trans,rot) = self.listener.lookupTransform(self.world,match,rospy.Time(0))
                for (pose, name) in zip(poses,names):
                    #resp.waypoints.poses.append(pm.toMsg(pose * pm.fromTf((trans,rot))))
                    new_poses.append(pm.toMsg(pm.fromTf((trans,rot)) * pose))
                    new_names.append(match + "/" + name)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.logwarn('Could not find transform from %s to %s!'%(self.world,match))
        
        return (new_poses, new_names)

 def getCameraPose(self):
     try:
         self.listener.waitForTransform('torso_base', "camera", rospy.Time.now(), rospy.Duration(4.0))
         T_B_C_tf = self.listener.lookupTransform('torso_base', "camera", rospy.Time(0))
     except:
         return None
     return pm.fromTf(T_B_C_tf)

    def get_new_waypoint(self,obj):
        try:
            # TODO: make the snap configurable
            #(trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
            (eg_trans,eg_rot) = self.listener.lookupTransform(self.gripper_center,self.endpoint,rospy.Time(0))
            (og_trans,og_rot) = self.listener.lookupTransform(obj,self.gripper_center,rospy.Time(0))

            rospy.logwarn("gripper obj:" + str((og_trans, og_rot)))
            rospy.logwarn("endpoint gripper:" + str((eg_trans, eg_rot)))

            xyz, rpy = [], []
            for dim in og_trans:
                # TODO: test to see if we should re-enable this
                if abs(dim) < 0.0001:
                    xyz.append(0.)
                else:
                    xyz.append(dim)
            Rog = kdl.Rotation.Quaternion(*og_rot)
            for dim in Rog.GetRPY():
                rpy.append(np.round(dim / np.pi * 8.) * np.pi/8.)
            Rog_corrected = kdl.Rotation.RPY(*rpy)
            Vog_corrected = kdl.Vector(*xyz)
            Tog_corrected = kdl.Frame(Rog_corrected, Vog_corrected)

            rospy.logwarn(str(Tog_corrected) + ", " + str(Rog_corrected.GetRPY()))

            Teg = pm.fromTf((eg_trans, eg_rot))
            return pm.toMsg(Tog_corrected * Teg)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
        return None

def transform_wrist_frame(T_tool, ft, tool_x_offset=0.0):
    '''
    :param T_tool: desired  *_gripper_tool_frame pose w.r.t. some reference frame. Can be of type kdl.Frame or geometry_msgs.Pose.
    :param ft: bool. True if the arm has a force-torque sensor
    :param tool_x_offset: (double) offset in tool length
    :return: T_torso_wrist. geometry_msgs.Pose type. Wrist pose w.r.t. same ref frame as T_tool
    '''

    if ft:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.216 + tool_x_offset, 0.0, 0.0))

    else:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.180 + tool_x_offset, 0.0, 0.0))

    if type(T_tool) is Pose:

        T_tool_ = posemath.fromMsg(T_tool)

    elif type(T_tool) is tuple and len(T_tool) is 2:
        T_tool_ = posemath.fromTf(T_tool)

    else:
        T_tool_ = T_tool


    T_wrist_ = T_tool_*T_wrist_tool.Inverse()

    T_wrist = posemath.toMsg(T_wrist_)

    return T_wrist

    def getKDLtf(self, base_frame, frame):
#        try:
        pose = self.listener.lookupTransform(base_frame, frame, rospy.Time(0))
#        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
#            print "could not transform: ", base_frame, frame
#            return None
        return pm.fromTf(pose)

 def transform(self):
   for f in self.tool_features.values() + self.world_features.values():
     try:
       frame = self.listener.lookupTransform(self.base_frame_id,
                                             f.frame_id, rospy.Time(0))
     except:
       continue
     f.transform(pm.fromTf(frame))

 def _get_transform(self, from_, to_):
      try:
          #rospy.loginfo("Waiting for the transform %s -> %s" % (from_, to_))
          self.tf_listener.waitForTransform(from_, to_, rospy.Time(0), rospy.Duration(5))
          return posemath.fromTf( self.tf_listener.lookupTransform(from_, to_, rospy.Time(0)) )
      except (tf.Exception):
          rospy.logdebug("Transform lookup from %s to %s failed." % (from_, to_))
      
      return None

 def test_fromTf(self):
     transformer = Transformer(True, rospy.Duration(10.0))
     m = TransformStamped()
     m.header.frame_id = 'wim'
     m.child_frame_id = 'james'
     m.transform.translation.x = 2.71828183
     m.transform.rotation.w = 1.0
     transformer.setTransform(m)
     b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0)))

def axis_marker(tw, id = 0, ns = 'twist'):
  """ make a marker message showing the instantaneous
      rotation axis of a twist message"""

  t = kdl.Twist(kdl.Vector(tw.linear.x,  tw.linear.y,  tw.linear.z),
                kdl.Vector(tw.angular.x, tw.angular.y, tw.angular.z))

  try:
    (x,     rot)  = listener.lookupTransform(target_frame, ref_frame, rospy.Time(0))
    (trans, rotp) = listener.lookupTransform(target_frame, ref_point, rospy.Time(0))
  except (tf.LookupException, tf.ConnectivityException):
    print 'tf exception!'
    return marker.create(id=id, ns=ns, action=Marker.DELETE)

  # put the twist in the right frame
  f = posemath.fromTf( (trans, rot) )
  t = f*t

  direction = t.rot
  location = t.rot * t.vel / kdl.dot(t.rot, t.rot)

  lr = t.rot.Norm()
  lt = t.vel.Norm()

  m = marker.create(id=id, ns=ns, type=Marker.CYLINDER)

  if lr < 0.0001 and lt < 0.0001:
    return marker.create(id=id, ns=ns, action=Marker.DELETE)

  if lr < 0.001:
    # very short axis, assume linear movement
    location = kdl.Vector(0,0,0)
    direction = t.vel
    if lt < min_length:
      direction *= min_length / lt
    m.type = Marker.CUBE
    m = marker.align(m, location, location + direction, 0.02)
  elif lr < min_length:
    direction = direction / lr * min_length
    m = marker.align(m, location - direction, location + direction, 0.02)
  elif lr > max_length:
    direction = direction / lr * max_length
    m = marker.align(m, location - direction, location + direction, 0.02) 
  else:
    #BAH! How do I make this better?
    m = marker.align(m, location - direction, location + direction, 0.02)

  m.header.frame_id = target_frame
  m.frame_locked = True

  if(use_colors):
    m.color = colors[id % len(colors)]
  else:
    m.color = ColorRGBA(0.3, 0.3, 0.3, 1)

  return m