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 predictSinglePose(self, armName, curPose, prevPose, dt=1.0):
        if dt <= 0:
            print 'Error: Illegal timestamp'
            return None

        # Convert pose to numpy matrix
        curTrans = tft.translation_matrix([curPose.position.x, curPose.position.y, curPose.position.z])
        curRot = tft.quaternion_matrix([curPose.orientation.x, curPose.orientation.y ,curPose.orientation.z, curPose.orientation.w])
        curPoseMatrix = np.dot(curTrans, curRot)

        prevTrans = tft.translation_matrix([prevPose.position.x, prevPose.position.y, prevPose.position.z])
        prevRot = tft.quaternion_matrix([prevPose.orientation.x, prevPose.orientation.y ,prevPose.orientation.z, prevPose.orientation.w])
        prevPoseMatrix = np.dot(prevTrans, prevRot)
        
        deltaPoseMatrix = np.linalg.inv(prevPoseMatrix).dot(curPoseMatrix)
        deltaAngles = euler_from_matrix(deltaPoseMatrix[:3,:3])
        deltaPos = deltaPoseMatrix[:3,3]

        #deltaAngles = np.array([a / dt for a in deltaAngles])
        deltaPos = deltaPos / dt
        #deltaPoseMatrix = euler_matrix(deltaAngles[0], deltaAngles[1], deltaAngles[2])
        deltaPoseMatrix[:3,3] = deltaPos

        gpList, sysList = self.predict(armName, [curPoseMatrix], [deltaPoseMatrix])
        return tfx.pose(gpList[0], frame=curPose.frame, stamp=curPose.stamp), tfx.pose(sysList[0], frame=curPose.frame, stamp=curPose.stamp)

	def object_pose_callback(self,msg):
		print 'object pose cb'
		(tf_trans,tf_rot) = self.pr2.tf_listener.lookupTransform(msg.header.frame_id,self.base_frame,rospy.Time(0))
		msg_tf = numpy.mat(numpy.dot(tft.translation_matrix(tf_trans),tft.quaternion_matrix(tf_rot)))
		
		q = numpy.array([msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w])
		p = numpy.array([msg.pose.position.x,msg.pose.position.y,msg.pose.position.z])
		rot = numpy.mat(tft.quaternion_matrix(q))
		trans = numpy.mat(tft.translation_matrix(p))
		self.object_pose = msg_tf * trans * rot

	def localCb(self, data):
		self.localPose.setPoseStamped(data)
		
		if(not (self.enuTickPose.isNone() or self.offset is None)):
			t = self.localPose.getTranslation()
			q = self.enuTickPose.getQuaternion()
			
			q = quaternion_matrix(q)
			t = translation_matrix(t)
		
			self.localEnuPose.setMatrix(numpy.dot(q,t))
			
			t = self.localEnuPose.getTranslation()
			
			t[0] -= self.offset[0]
			t[1] -= self.offset[1]
			t[2] -= self.offset[2]
			
			q = self.localEnuPose.getQuaternion()
			
			self.localEnuPose.setTranslationQuaternion(t, q)
			
			p = PointStamped()
			p.point.x = self.offset[0]
			p.point.y = self.offset[1]
			p.point.z = self.offset[2]
			
			p.header = Header(0, rospy.rostime.get_rostime(), "world")
			
			self.offsetPub.publish(p)
			
			self.localEnuPub.publish(self.localEnuPose.getPoseStamped())

	def get_Tmatrix(self,disableinvert=False):
		Tmatrix = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),array(tb_to_tf(self.yaw, self.pitch, self.roll, rad=True)))
		
		if not disableinvert and self.invert:
			Tmatrix = tft.inverse_matrix(Tmatrix)
		
		return Tmatrix

  def __init__(self, side='r', tf_listener = None, tf_broadcaster = None, ctrl_mng=None, moveit_cmdr=None):

    if side == "right" or side == "r":
      self.arm = "right_arm"
      self.side = "r"
    elif side == 'left' or side == 'l':
      self.arm = "left_arm"
      self.side = "l"
    else:
      rospy.logerr("Side " + side + " is not supported")
      raise

    self.tf_listener = tf.TransformListener() if not tf_listener else tf_listener
    #init a TF transform broadcaster for the object frame
    self.tf_broadcaster = tf.TransformBroadcaster() if not tf_broadcaster else tf_broadcaster
    self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng

    #the pretouch sensor frame
    self.pretouch_sensor_frame_id = '/r_gripper_l_finger_tip_pretouch_frame'

    #Gripper client and open the gripper
    rospy.loginfo('open the ' + side + '_gripper')
    self.gripper = Gripper(self.arm)
    self.gripper.open()    

    #controller_magager_client
    self.ctrl_mng = ControllerManagerClient() if not ctrl_mng else ctrl_mng
    self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])
    self.count = 0
    #PoseStamped command publisher for jt controller
    self.pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
    self.eef_frame = self.side + "_wrist_roll_link"
    self.reference_frame = "base_link"

    # MoveIt! Commander
    self.moveit_cmd = MoveGroupCommander(self.arm) if not moveit_cmdr else moveit_cmdr
    self.moveit_cmd.set_pose_reference_frame(self.reference_frame)
    self.move_arm_to_side() # Move the arm to the sidea
    self.step_size = 0.0002
    self.move_step_mat = np.matrix(translation_matrix(np.array([self.step_size, 0.0, 0.0])))

    #pickup action server
    #self.pickup_client = actionlib.SimpleActionClient('/object_manipulator/object_manipulator_pickup', PickupAction)
    #rospy.loginfo("waiting for PickupAction Server")
    #self.pickup_client.wait_for_server()
    #rospy.loginfo("PickupAction Server Connected")

    #service client to /add_point service
    self.add_point_client = rospy.ServiceProxy('add_point', AddPoint)

    #draw_functions object for drawing stuff in rviz
    self.draw_functions = draw_functions.DrawFunctions('pretouch_executor_markers')

    #the transform from wrist_frame to the center of the fingers
    self.gripper_to_r_finger = np.eye(4)
    self.gripper_to_r_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
    self.gripper_to_r_finger[1][3] = -0.0400 #y-translation from wrist_frame (0.0425)    
    self.gripper_to_l_finger = np.eye(4)
    self.gripper_to_l_finger[0][3] = 0.1615 #x-translation from wrist_frame (0.1615)
    self.gripper_to_l_finger[1][3] = 0.0400 #y-translation from wrist_frame (-0.0425)

 def transformation_matrix(rot_angle, rot_dir, trans):
     # print "rot_angle: ", rot_angle
     # print "rot_dir: ", rot_dir
     # print "trans: ", trans
     return tfs.concatenate_matrices(
         tfs.rotation_matrix(rot_angle, rot_dir),
         tfs.translation_matrix(trans))

  def adjustPretouchPose(self, pose):    
    #transform the pose from fingertip_frame to X_wrist_roll_link
    r_gripper_l_fingertip_to_r_wrist_roll_link_mat = np.matrix(translation_matrix(np.array([-0.18, -0.049, 0.0])))
  
    current_pose = pose
    pretouch_mat = pose_to_mat(current_pose)
    pretouch_wrist_roll_pose = mat_to_pose(pretouch_mat * r_gripper_l_fingertip_to_r_wrist_roll_link_mat)

    #TODO Query IK solution for that pretouch pose
    # while no IK solution, rotate w.r.t Z-axis toward the robot (positive rotation)
    
    #print 'ik solution for l_probe: ', self.getConstraintAwareIK(l_probe.pretouch_pose)
    #print 'ik solution for r_probe: ', self.getConstraintAwareIK(r_probe.pretouch_pose)

		#select one here...
    #(joint_position, have_solution) = self.getIK(pretouch_wrist_roll_pose)
    (joint_position, have_solution) = self.getConstraintAwareIK(pretouch_wrist_roll_pose)
    
    '''
    if not have_solution:
      #rotate the pose toward the robot
      #rot_mat = rotation_matrix(l_rot_z, np.array([0,0,1])) #0.175 radians = 10 degree
      print 'No IK solution found'
    '''
    return (pretouch_wrist_roll_pose, joint_position, have_solution)

 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 tag_callback(self, msg_tag):
        # Listen for the transform of the tag in the world
        avg = PoseAverage.PoseAverage()
        for tag in msg_tag.detections:
            try:
                Tt_w = self.tfbuf.lookup_transform(self.world_frame, "tag_{id}".format(id=tag.id), rospy.Time(), rospy.Duration(1))
                Mtbase_w=self.transform_to_matrix(Tt_w.transform)
                Mt_tbase = tr.concatenate_matrices(tr.translation_matrix((0,0,0.17)), tr.euler_matrix(0,0,np.pi))
                Mt_w = tr.concatenate_matrices(Mtbase_w,Mt_tbase)
                Mt_r=self.pose_to_matrix(tag.pose)
                Mr_t=np.linalg.inv(Mt_r)
                Mr_w=np.dot(Mt_w,Mr_t)
                Tr_w = self.matrix_to_transform(Mr_w)
                avg.add_pose(Tr_w)
                self.publish_sign_highlight(tag.id)
            except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
                rospy.logwarn("Error looking up transform for tag_%s", tag.id)
                rospy.logwarn(ex.message)

        Tr_w =  avg.get_average() # Average of the opinions

        # Broadcast the robot transform
        if Tr_w is not None:
            # Set the z translation, and x and y rotations to 0
            Tr_w.translation.z = 0
            rot = Tr_w.rotation
            rotz=tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2]
            (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0, 0, rotz)
            T = TransformStamped()
            T.transform = Tr_w
            T.header.frame_id = self.world_frame
            T.header.stamp = rospy.Time.now()
            T.child_frame_id = self.duckiebot_frame
            self.pub_tf.publish(TFMessage([T]))
            self.lifetimer = rospy.Time.now()

 def getBaseTransform(self):
   now = rospy.Time()
   self.tf_listener.waitForTransform('/base_link','/odom_combined', now,
                                     rospy.Duration(10))
   pos, quat = self.tf_listener.lookupTransform('/odom_combined','/base_link', now)
   trans = transf.translation_matrix(pos)
   rot = transf.quaternion_matrix(quat)
   return np.dot(trans, rot)

def matrix_from_StampedTransform(msg):
    T = msg.transform
    trans = [T.translation.x, T.translation.y, T.translation.z]
    quat = [T.rotation.x, T.rotation.y, T.rotation.z, T.rotation.w]

    return tfs.concatenate_matrices(
        tfs.translation_matrix(trans),
        tfs.quaternion_matrix(quat))

    def predictSinglePose(self, pose, arm_side):
        # Convert pose to numpy matrix
        p = tft.translation_matrix([pose.position.x,pose.position.y,pose.position.z])
        rot = tft.quaternion_matrix([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w])
        input_pose = np.dot(p,rot)

        gpList, sysList = self.predict([input_pose], arm_side)
        return tfx.pose(gpList[0], frame=pose.frame, stamp=pose.stamp), tfx.pose(sysList[0], frame=pose.frame, stamp=pose.stamp)

    def _makeGraspPath(self, preGraspGoal):
        '''Given a pregrasp MoveArmGoal message, generate a MoveArmGoal message to perform the final
        approach to the object to grasp it.'''
        
        graspGoal = MoveArmGoal()
        graspGoal.planner_service_name = self.plannerServiceName
        motion_plan_request = MotionPlanRequest()
        motion_plan_request.group_name = self.armGroupName
        motion_plan_request.num_planning_attempts = 25
        motion_plan_request.planner_id = ""
        motion_plan_request.allowed_planning_time = rospy.Duration(5,0)
        
        #Orientation constraint is the same as for pregrasp
        motion_plan_request.goal_constraints.orientation_constraints = copy.deepcopy(preGraspGoal.motion_plan_request.goal_constraints.orientation_constraints)
        #motion_plan_request.goal_constraints.orientation_constraints[0].orientation = Quaternion(0.656778, 0.261999, 0.648401, -0.282093) #stow orientation for debug
        graspGoal.motion_plan_request = motion_plan_request
        
        #Translate from pregrasp position to final position in a roughly straight line
        o = motion_plan_request.goal_constraints.orientation_constraints[0].orientation
        p = preGraspGoal.motion_plan_request.goal_constraints.position_constraints[0].position
        preGraspMat = transformations.quaternion_matrix([o.x,o.y,o.z,o.w])
        preGraspMat[:3, 3] = [p.x,p.y,p.z]
        distance = self.preGraspDistance * .5 + self.gripperFingerLength * .5
        graspTransMat = transformations.translation_matrix([0,0,distance])
        graspMat = transformations.concatenate_matrices(preGraspMat, graspTransMat)
        #print preGraspMat
        #print graspTransMat
        #print graspMat
        p = transformations.translation_from_matrix(graspMat)
       
        #Publish grasp transform for visualization
        self._tf_broadcaster.sendTransform(
                (p[0],p[1],p[2]),
                (o.x, o.y, o.x, o.w),
                motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp,
                "grasp",
                motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id)
 
        pos_constraint = PositionConstraint()
        pos_constraint.header = motion_plan_request.goal_constraints.orientation_constraints[0].header
        pos_constraint.link_name = self.toolLinkName
        pos_constraint.position = Point(p[0],p[1],p[2])
        #pos_constraint.position = Point(-0.0644721, 0.609922, 0) #Stow position for debug
        pos_constraint.constraint_region_shape.type = Shape.SPHERE
        pos_constraint.constraint_region_shape.dimensions = [0.01]
        pos_constraint.weight = 1
        motion_plan_request.goal_constraints.position_constraints.append(pos_constraint)
        #TODO: Add path constraint to require a (roughly) cartesian move
        
        #Turn off collision operations between the gripper and all objects
        for collisionName in self.gripperCollisionNames:
            collisionOperation = CollisionOperation(collisionName, 
                                    CollisionOperation.COLLISION_SET_ALL,
                                    0.0,
                                    CollisionOperation.DISABLE)
            graspGoal.operations.collision_operations.append(collisionOperation)
        return graspGoal

 def approach(self, approach_dist, step_size):
   self.ctrl_mng.switch_controllers([self.side+'_cart'], [self.side+'_arm_controller'])
   current_mat = self.current_mat
   move_step_mat = np.matrix(translation_matrix(np.array([step_size, 0.0, 0.0])))
   for i in range(int(approach_dist/step_size)):
     current_mat = current_mat * move_step_mat
     self.pose_pub.publish(stamp_pose(mat_to_pose(current_mat), self.reference_frame))
     rospy.sleep(0.1)    
   self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])

def pose_to_matrix(pose_msg):
    ''' Convert a ros Transform message to a 4x4 homogeneous transform matrix '''
    rot = pose_msg.orientation
    q = np.array([rot.x, rot.y, rot.z, rot.w])
    pos = pose_msg.position
    t = np.array([pos.x, pos.y, pos.z])
    H_t = transformations.translation_matrix(t)
    H_r = transformations.quaternion_matrix(q)
    return np.matrix(np.dot(H_t, H_r))

def transform_to_matrix(transform_msg):
    ''' Convert a ros Transform message to a 4x4 homogeneous transform matrix '''
    rot = transform_msg.rotation
    q = np.array([rot.x, rot.y, rot.z, rot.w])
    trans = transform_msg.translation
    t = np.array([trans.x, trans.y, trans.z])
    H_t = transformations.translation_matrix(t)
    H_r = transformations.quaternion_matrix(q)
    return np.matrix(np.dot(H_t, H_r))

	def pose_callback(self,msg):
		#print 'got msg'
		frame_id = msg.header
		pose = msg.pose
		p = tft.translation_matrix([pose.position.x,pose.position.y,pose.position.z])
		rot = tft.quaternion_matrix([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w])
		
		Tcam_to_cb = dot(p,rot)
		#print 'Tcam_to_cb',Tcam_to_cb
		
		#Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),tft.euler_matrix(0,0,pi/2))
		Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),array(tb_to_mat(self.yaw, self.pitch, self.roll)))
		#print 'Tworld_to_cb',Tworld_to_cb
		
		if self.invert_tf:
			Tworld_to_cb = tft.inverse_matrix(Tworld_to_cb)
		
		Tworld_to_cam = dot(Tworld_to_cb,tft.inverse_matrix(Tcam_to_cb))
		#print 'Tworld_to_cam',Tworld_to_cam
		
		Tworld_to_cam_p = Tworld_to_cam[0:3,3]
		Tworld_to_cam_q = tft.quaternion_from_matrix(Tworld_to_cam)
		
		pub_msg = PoseStamped()
		pub_msg.header.stamp = msg.header.stamp
		pub_msg.header.frame_id = '/world'
		pub_msg.pose.position = Point(*(Tworld_to_cam_p.tolist()))
		pub_msg.pose.orientation = Quaternion(*(Tworld_to_cam_q.tolist()))
		
		#print pub_msg
		self.pub.publish(pub_msg)
		
		pub_cb_msg = PoseStamped()
		pub_cb_msg.header.stamp = msg.header.stamp
		pub_cb_msg.header.frame_id = '/world'
		pub_cb_msg.pose.position = Point(*(Tworld_to_cb[0:3,3].tolist()))
		pub_cb_msg.pose.orientation = Quaternion(*(tft.quaternion_from_matrix(Tworld_to_cb).tolist()))
		
		self.pub_cb.publish(pub_cb_msg)
		
		self.transform = Tworld_to_cam
		self.transform_frame = msg.header.frame_id
		
		self.br.sendTransform(Tworld_to_cam_p,Tworld_to_cam_q,msg.header.stamp,self.transform_frame,'/world')

    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 lift(self, grasp, lift_dist, step_size):
   self.ctrl_mng.switch_controllers([self.side+'_cart'], [self.side+'_arm_controller'])
   current_mat = pose_to_mat(grasp.grasp_pose.pose)
   move_step_mat = np.matrix(translation_matrix(np.array([0.0, 0.0, -step_size])))
   for i in range(int(lift_dist/step_size)):
     #current_mat = current_mat * move_step_mat
     current_mat[2,3] += step_size
     self.pose_pub.publish(stamp_pose(mat_to_pose(current_mat), self.reference_frame))
     rospy.sleep(0.05)    
   self.ctrl_mng.switch_controllers([self.side+'_arm_controller'], [self.side+'_cart'])