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 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 Update(self, timeout=10):
        marker_message = rospy.wait_for_message(self.marker_topic,
                                                MarkerArray,
                                                timeout=timeout)
        
        added_kinbodies = []
        updated_kinbodies = []
        
        for marker in marker_message.markers:
            if marker.ns in self.marker_data:
                kinbody_file, kinbody_offset = self.marker_data[marker.ns]
                kinbody_offset = numpy.array(kinbody_offset)
                marker_pose = numpy.array(quaternion_matrix([
                        marker.pose.orientation.x,
                        marker.pose.orientation.y,
                        marker.pose.orientation.z,
                        marker.pose.orientation.w]))
                marker_pose[0,3] = marker.pose.position.x
                marker_pose[1,3] = marker.pose.position.y
                marker_pose[2,3] = marker.pose.position.z
                
                self.listener.waitForTransform(
                        self.detection_frame,
                        self.destination_frame,
                        rospy.Time(),
                        rospy.Duration(timeout))
                frame_trans, frame_rot = self.listener.lookupTransform(
#                        self.detection_frame,
                        self.destination_frame,
                        self.detection_frame,
                        rospy.Time(0))
                frame_offset = numpy.matrix(quaternion_matrix(frame_rot))
                frame_offset[0,3] = frame_trans[0]
                frame_offset[1,3] = frame_trans[1]
                frame_offset[2,3] = frame_trans[2]
                
                kinbody_pose = numpy.array(numpy.dot(numpy.dot(frame_offset,marker_pose),
                                                     kinbody_offset))
                
                kinbody_name = kinbody_file.replace('.kinbody.xml', '')
                kinbody_name = kinbody_name + str(marker.id)
                
                # load the object if it does not exist
                if self.env.GetKinBody(kinbody_name) is None:
                    new_body = prpy.rave.add_object(
                            self.env,
                            kinbody_name,
                            os.path.join(self.kinbody_directory, kinbody_file))
                    added_kinbodies.append(new_body)
                    self.generated_bodies.append(new_body)
                
                body = self.env.GetKinBody(kinbody_name)
                body.SetTransform(kinbody_pose)
                updated_kinbodies.append(body)
        
        return added_kinbodies, updated_kinbodies

	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 update(self, dt, desired, current):
     ((desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot)), ((p, o), (p_dot, o_dot)) = desired, current
     
     world_from_body = transformations.quaternion_matrix(o)[:3, :3]
     x_dot = numpy.concatenate([
         world_from_body.dot(p_dot),
         world_from_body.dot(o_dot),
     ])
     
     world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3]
     desired_x_dot = numpy.concatenate([
         world_from_body.dot(desired_p_dot),
         world_from_body.dot(desired_o_dot),
     ])
     desired_x_dotdot = numpy.concatenate([
         world_from_body.dot(desired_p_dotdot),
         world_from_body.dot(desired_o_dotdot),
     ])
     
     error_position_world = numpy.concatenate([
         desired_p - p,
         quat_to_rotvec(transformations.quaternion_multiply(
             desired_o,
             transformations.quaternion_inverse(o),
         )),
     ])
     
     world_from_body2 = numpy.zeros((6, 6))
     world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
     body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T)
     
     
     error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
     
     pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world)
     
     output = pd_output
     if self.config['use_rise']:
         rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
                         body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))
         
         self._rise_term = self._rise_term + dt/2*(rise_term_int + self._rise_term_int_prev)
         self._rise_term_int_prev = rise_term_int
         
         output = output + self._rise_term
     else:
         # zero rise term so it doesn't wind up over time
         self._rise_term = numpy.zeros(6)
         self._rise_term_int_prev = numpy.zeros(6)
     output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
     output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot)
     
     wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6]))
     return wrench_from_vec(pd_output), wrench_from_vec(output)

    def recalculate_transform(self, currentTime):
        """
        Creates updated transform from /odom to /map given recent odometry and
        laser data.
        
        :Args:
            | currentTime (rospy.Time()): Time stamp for this update
         """
        
        transform = Transform()

        T_est = transformations.quaternion_matrix([self.estimatedpose.pose.pose.orientation.x,
                                                   self.estimatedpose.pose.pose.orientation.y,
                                                   self.estimatedpose.pose.pose.orientation.z,
                                                   self.estimatedpose.pose.pose.orientation.w])
        T_est[0, 3] = self.estimatedpose.pose.pose.position.x
        T_est[1, 3] = self.estimatedpose.pose.pose.position.y
        T_est[2, 3] = self.estimatedpose.pose.pose.position.z
        
        T_odom = transformations.quaternion_matrix([self.last_odom_pose.pose.pose.orientation.x,
                                                   self.last_odom_pose.pose.pose.orientation.y,
                                                   self.last_odom_pose.pose.pose.orientation.z,
                                                   self.last_odom_pose.pose.pose.orientation.w])
        T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
        T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
        T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
        T = np.dot(T_est, np.linalg.inv(T_odom))
        q = transformations.quaternion_from_matrix(T) #[:3, :3])

        transform.translation.x = T[0, 3] 
        transform.translation.y = T[1, 3] 
        transform.translation.z = T[2, 3] 
        transform.rotation.x = q[0]
        transform.rotation.y = q[1]
        transform.rotation.z = q[2]
        transform.rotation.w = q[3]
        

        # Insert new Transform into a TransformStamped object and add to the
        # tf tree
        new_tfstamped = TransformStamped()
        new_tfstamped.child_frame_id = "/odom"
        new_tfstamped.header.frame_id = "/map"
        new_tfstamped.header.stamp = currentTime
        new_tfstamped.transform = transform

        # Add the transform to the list of all transforms
        self.tf_message = tfMessage(transforms=[new_tfstamped])

    def cb(data):
        # rospy.loginfo("{0}".format(data))
        stamp = data.header.stamp
        tf_listener.waitForTransform(world_frame, link_frame, stamp, rospy.Duration(4.0))

        source_frame = world_frame
        target_frame = link_frame
        tf_link = tf_listener.lookupTransform(source_frame, target_frame, stamp)
        T_link = transformations.quaternion_matrix(tf_link[1])
        T_link[:3, 3] = tf_link[0]

        pose_chessboard = data.pose
        T_chessboard = pose_matrix(pose_chessboard)
        # T_chessboard =  numpy.linalg.inv(T_chessboard)

        yaml_data.append({"T_chessboard": sum(T_chessboard.tolist(), []), "T_link": sum(T_link.tolist(), [])})

        yf = open(out_fn, "w")
        yaml.dump(yaml_data, yf)

        print len(yaml_data)
        print stamp
        print T_chessboard
        print T_link
        print "---"
        yf.close()

def get_tag_obj_transforms(input_markers):

    output_markers = MarkerArray()
    tag_detections = {}

    for marker in input_markers.markers:
        tag_id = marker.id
        position_data = marker.pose.position
        orientation_data = marker.pose.orientation

        tag_pose = numpy.matrix(quaternion_matrix([orientation_data.x,
                                                   orientation_data.y,
                                                   orientation_data.z,
                                                   orientation_data.w]))
        tag_pose[0,3] = position_data.x
        tag_pose[1,3] = position_data.y
        tag_pose[2,3] = position_data.z

        tag_detections[tag_id] = tag_pose
    

    #Setup the data structure to dump into config file
    # config_file_data = {}
    # config_file_data['objects'] = {}
    # config_file_data['objects'][OBJECT_TO_ENTER] = {}
    # config_file_data['objects'][OBJECT_TO_ENTER]['tags'] = {}
    if TAG_AT_CENTRE in tag_detections:
        transform = tag_detections[TAG_AT_CENTRE]
        tag_transforms = {}
        for tag in tag_detections:
            tag_transforms[tag] = numpy.linalg.inv(tag_detections[tag])*transform

        print (tag_transforms)

def box_array_cb(box_array):
    polygon_array = PolygonArray()
    model_coefficients_array = ModelCoefficientsArray()
    for box in box_array.boxes:
        polygon_stamped = PolygonStamped()
        coefficients = ModelCoefficients()
        quaternion = np.array([box.pose.orientation.x, box.pose.orientation.y, box.pose.orientation.z, box.pose.orientation.w])
        rotation_matrix = transformations.quaternion_matrix(quaternion)
        ux = numpy.array([rotation_matrix[0][0], rotation_matrix[1][0], rotation_matrix[2][0]])
        uy = numpy.array([rotation_matrix[0][1], rotation_matrix[1][1], rotation_matrix[2][1]])
        uz = numpy.array([rotation_matrix[0][2], rotation_matrix[1][2], rotation_matrix[2][2]])
        dim_x = box.dimensions.x/2
        dim_y = box.dimensions.y/2
        dim_z = box.dimensions.z/2
        point = box.pose.position
        for x, y in [[-dim_x, -dim_y], [dim_x, -dim_y], [dim_x, dim_y], [-dim_x, dim_y]]:
            polygon_stamped.polygon.points.append(Point32(x*ux[0]+y*uy[0]-dim_z*uz[0]+point.x,x*ux[1]+y*uy[1]-dim_z*uz[1]+point.y,x*ux[2]+y*uy[2]-dim_z*uz[2]+point.z))
        polygon_stamped.header = box.header
        polygon_array.polygons.append(polygon_stamped)
        coefficients.values = [-uz[0], -uz[1], -uz[2], ((point.x-dim_z*uz[1])*uz[0]+(point.y-dim_z*uz[1])*uz[1]+(point.z-dim_z*uz[2])*uz[2])]
        coefficients.header = box.header
        model_coefficients_array.coefficients.append(coefficients)
    polygon_array.header = box_array.header
    PArrayPub.publish(polygon_array)
    model_coefficients_array.header = box_array.header
    MArrayPub.publish(model_coefficients_array)

def transformFt2Global(ftlist):
    global listener
    # transform ft data to global frame
    (pos_trasform, ori_trasform) = lookupTransform('base_link', 'link_ft', listener)
    rotmat = tfm.quaternion_matrix(ori_trasform)
    ft_global = np.dot(rotmat, ftlist + [1.0])
    return ft_global[0:3].tolist()

    def detect(self, timeout=10):
        marker_message = rospy.wait_for_message(self.marker_topic, MarkerArray, timeout=timeout)
        camera_message = rospy.wait_for_message(self.camera_topic, CameraInfo, timeout=timeout)
        camera_matrix = np.array([0])
        for marker in marker_message.markers:
            if marker.id == self.marker_number:
                marker_pose = np.array(
                    quaternion_matrix(
                        [
                            marker.pose.orientation.x,
                            marker.pose.orientation.y,
                            marker.pose.orientation.z,
                            marker.pose.orientation.w,
                        ]
                    )
                )
                marker_pose[0, 3] = marker.pose.position.x
                marker_pose[1, 3] = marker.pose.position.y
                marker_pose[2, 3] = marker.pose.position.z
                marker_pose = np.delete(marker_pose, 3, 0)

                camera_intrinsics = np.array(camera_message.K).reshape(3, 3)

                print camera_intrinsics
                print marker_pose
                camera_matrix = np.dot(camera_intrinsics, marker_pose)
                print camera_matrix
        return camera_matrix

    def execute(self, userdata):


        userdata.marker_pose.header.stamp = rospy.Time.now()
        pose = self.tf.transformPose('/base_link', userdata.marker_pose)
        p = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
        q = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w]
        rm = tfs.quaternion_matrix(q)
        # assemble a new coordinate frame that has z-axis aligned with
        # the vertical direction and x-axis facing the z-axis of the
        # original frame
        z = rm[:3, 2]
        z[2] = 0
        axis_x = tfs.unit_vector(z)
        axis_z = tfs.unit_vector([0, 0, 1])
        axis_y = np.cross(axis_x, axis_z)
        rm = np.vstack((axis_x, axis_y, axis_z)).transpose()
        # shift the pose along the x-axis
        p += np.dot(rm, [self.DISTANCE, 0, 0])[:3]
        userdata.goal_pose = pose
        userdata.goal_pose.pose.position.x = p[0]
        userdata.goal_pose.pose.position.y = p[1]
        userdata.goal_pose.pose.position.z = p[2]
        yaw = tfs.euler_from_matrix(rm)[2]
        q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
        userdata.goal_pose.pose.orientation.x = q[0]
        userdata.goal_pose.pose.orientation.y = q[1]
        userdata.goal_pose.pose.orientation.z = q[2]
        userdata.goal_pose.pose.orientation.w = q[3]
        return 'succeeded'

 def computePlaceToBaseMatrix(self, place):
     place_quaternion = place[3:]
     rotation_matrix = quaternion_matrix(place_quaternion)
     translation = place[0:3]
     rotation_matrix[[0, 1, 2], 3] = translation
     place_to_base_matrix = rotation_matrix
     return place_to_base_matrix

    def process_wrench(self, msg):
        cur_wrench = np.mat([msg.wrench.force.x, 
                             msg.wrench.force.y, 
                             msg.wrench.force.z, 
                             msg.wrench.torque.x, 
                             msg.wrench.torque.y, 
                             msg.wrench.torque.z]).T
        try:
            (ft_pos, ft_quat) = self.tf_list.lookupTransform(self.gravity_frame, 
                                                             self.netft_frame, rospy.Time(0))
        except:
            return
        rot_mat = np.mat(tf_trans.quaternion_matrix(ft_quat))[:3,:3]
        z_grav = self.react_mult * rot_mat.T * np.mat([0, 0, -1.]).T
        force_grav = np.mat(np.zeros((6, 1)))
        force_grav[:3, 0] = self.mass * g * z_grav
        torque_grav = np.mat(np.zeros((6, 1)))
        torque_grav[3:, 0] = np.mat(np.cross(self.com_pos.T.A[0], force_grav[:3, 0].T.A[0])).T
        zeroing_wrench = force_grav + torque_grav + self.wrench_zero
        zeroed_wrench = self.react_mult * (cur_wrench - zeroing_wrench)
        
        if not self.got_zero:
            self.wrench_zero = (cur_wrench - (force_grav + torque_grav))
            self.got_zero = True

        tf_zeroed_wrench = self.transform_wrench(zeroed_wrench)
        if tf_zeroed_wrench is None:
            return
        zero_msg = WrenchStamped(msg.header, 
                                 Wrench(Vector3(*tf_zeroed_wrench[:3,0]), Vector3(*tf_zeroed_wrench[3:,0])))
        self.zero_pub.publish(zero_msg)
        self.visualize_wrench(tf_zeroed_wrench)

	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 getSkeletonTransformation(user_id, tf, tf_name, world_name, last_updated):
    """
    Return the rototranslation matrix between tf_name and world_name 
    and the time stamp
    
    @param user_id id of the human
    @param tf 
    @param tf_name name of the element in the tf tree
    @param world_name name of the world in the tf tree
    @param last_update time stamp of the last time the tf tree was updated
    """
    
    tf_name_full = getFullTfName(user_id, tf_name)
    if tf.frameExists(tf_name_full) and tf.frameExists(world_name):   
        try:                
            time = tf.getLatestCommonTime(tf_name_full, world_name)
            pos, quat = tf.lookupTransform(world_name, tf_name_full, time)
        except:
            return last_updated, None
        last_updated = time.secs if last_updated <= time.secs else last_updated
        r = transformations.quaternion_matrix(quat)
        skel_trans = transformations.identity_matrix()
        skel_trans[0:4,0:4] = r
        skel_trans[0:3, 3] = pos    
        return last_updated, skel_trans
    else:
        return last_updated, None

    def planCallback(self, msg):
        # get the goal
        pose_stamped = msg.poses[-1]
        pose = pose_stamped.pose

        # look ahead one meter
        R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
        point = [1, 0, 0, 1]
        M = R*point

        p = PointStamped()
        p.header.frame_id = pose_stamped.header.frame_id
        p.header.stamp = rospy.Time(0)
        p.point.x += pose_stamped.pose.position.x + M[0,0]
        p.point.y += pose_stamped.pose.position.y + M[1,0]
        p.point.z += pose_stamped.pose.position.z + M[2,0]

        # transform to base_link
        p = self.listener.transformPoint("base_link", p)

        # update
        with self.mutex:
            if p.point.x < 0.65:
                self.x = 0.65
            else:
                self.x = p.point.x
            if p.point.y > 0.5:
                self.y = 0.5
            elif p.point.y < -0.5:
                self.y = -0.5
            else:
                self.y = p.point.y

    def test_LArmIK(self):
        #   /WAIST /LARM_JOINT5_Link
        # - Translation: [0.325, 0.182, 0.074]
        # - Rotation: in Quaternion [-0.000, -0.707, 0.000, 0.707]
        #             in RPY [-1.694, -1.571, 1.693]
        for torso_angle in ([0, -10, 10]):
            torso_goal = self.goal_Torso()
            torso_goal = self.setup_Positions(torso_goal, [[torso_angle]])
            self.torso.send_goal_and_wait(torso_goal)
            for p in [[ 0.325, 0.182, 0.074], # initial
                      [ 0.3, -0.2, 0.074], [ 0.3, -0.1, 0.074], [ 0.3,  0.0, 0.074],
                      [ 0.3,  0.1, 0.074], [ 0.3,  0.2, 0.074], [ 0.3,  0.3, 0.074],
                      [ 0.4, -0.1, 0.074], [ 0.4, -0.0, 0.074], [ 0.4,  0.1, 0.074],
                      [ 0.4,  0.2, 0.074], [ 0.4,  0.3, 0.074], [ 0.4,  0.4, 0.074],
                      ] :

                print "solve ik at p = ", p
                ret = self.set_target_pose('LARM', p, [-1.694,-1.571, 1.693], 1.0)
                self.assertTrue(ret.operation_return, "ik failed")
                ret = self.wait_interpolation_of_group('LARM')
                self.assertTrue(ret.operation_return, "wait interpolation failed")

                rospy.sleep(1)
                now = rospy.Time.now()
                self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
                (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
                numpy.testing.assert_array_almost_equal(trans, p, decimal=1)
                print "current position   p = ", trans
                print "                 rot = ", rot
                print "                     = ", quaternion_matrix(rot)[0:3,0:3]

    def odometry_callback(self, msg):
        """Handle updated measured velocity callback."""
        if not bool(self.config):
            return

        p = msg.pose.pose.position
        q = msg.pose.pose.orientation
        p = numpy.array([p.x, p.y, p.z])
        q = numpy.array([q.x, q.y, q.z, q.w])

        if not self.initialized:
            # If this is the first callback: Store and hold latest pose.
            self.pos_des  = p
            self.quat_des = q
            self.initialized = True

        # Compute control output:
        t = msg.header.stamp.to_sec()

        # Position error wrt. body frame
        e_pos = trans.quaternion_matrix(q).transpose()[0:3,0:3].dot(self.pos_des - p)

        vz = self.pid_depth.regulate(e_pos[2], t)
        vx = self.pid_forward.regulate(numpy.linalg.norm(e_pos[0:2]), t)
        wx = self.pid_heading.regulate(numpy.arctan2(), t)

        v_linear = numpy.array([vx, 0, vz])
        v_angular = numpy.array([0, 0, wz])

        # Convert and publish vel. command:
        cmd_vel = geometry_msgs.Twist()
        cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
        cmd_vel.angular = geometry_msgs.Vector3(*v_angular)

        self.pub_cmd_vel.publish(cmd_vel)

def getpose():
    pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names=['right_s0', 'right_s1', 'right_e0', 'right_e1',  'right_w0', 'right_w1', 'right_w2']
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
    qc = (joint_positions.position[9:16])
    angles = [qc[2],qc[3],qc[0],qc[1],qc[4],qc[5],qc[6]]
    pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names=['right_e1']
    command_msg.command=[0]
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()
    exit = 0
    listener2=tf.TransformListener()

    #the transformations
    now=rospy.Time()
    listener2.waitForTransform("/torso","/right_hand",now,rospy.Duration(1.0))
    (trans08,rot08)=listener2.lookupTransform("/torso","/right_hand",now)

    # Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1])
    R08 = transformations.quaternion_matrix(rot08)
    T08 = numpy.vstack((numpy.column_stack((R08[0:3,0:3], numpy.transpose(numpy.array(trans08)))),[0,0,0,1]))
    return (angles, T08)