def segment_through_pixel(self, pixel, start_dist=None):
     """
     Returns segment from camera origin through pixel
     
     :param pixel: 2d list/np.array
     :return geometry3d.Segment (with endpoints in frame 'base_link')
     """
     # assert (0 <= pixel[0] <= self.height) and (0 <= pixel[1] <= self.width)
     start_dist = start_dist if start_dist is not None else self.min_range
     
     pixel = np.array(pixel)
     pixel_centered = pixel - np.array([self.height/2., self.width/2.])
     
     pixel3d_centered_m_min = start_dist*np.array([pixel_centered[1]/self.fx,
                                                   pixel_centered[0]/self.fy,
                                                   1])
     pixel3d_centered_m_max = self.max_range*np.array([pixel_centered[1]/self.fx,
                                                      pixel_centered[0]/self.fy,
                                                      1])
     
     transform = self.get_pose().as_tf()
     p0 = (transform*tfx.pose(pixel3d_centered_m_min)).position.array 
     p1 = (transform*tfx.pose(pixel3d_centered_m_max)).position.array        
     
     return geometry3d.Segment(p0, p1)

 def __init__(self):
     self.outcomes = None
     self.homePoseLeft = tfx.pose([-0.04907726751924995, 0.027288735500984575, -0.1211606501908539],
         (0.9835887535507493, -0.09932464655036198, -0.14884734393715604, 0.023070472014753367))
     self.homePoseRight = tfx.pose([0.061241857051286236, 0.014307808069346816, -0.10446866837544996],
         (-0.9689889616996428, 0.1939060552483166, -0.1474787309756946, 0.04136251626876463))
     rospy.sleep(5)

def test_servo():
    rospy.init_node('gripper_control',anonymous=True)
    rospy.sleep(2)
    listener = tf.TransformListener()
    gripperControl = GripperControlClass(arm, tf.TransformListener())
    rospy.sleep(2)

    gripperControl.start()

    rospy.loginfo('Press enter')
    raw_input()

    currPose = tfx.pose(gripperControl.getGripperPose(MyConstants.Frames.Link0))
    
    # w.r.t. Link0
    desPose = armDot

    
    transBound = .01
    rotBound = 25
    
    rate = rospy.Rate(1)

    while not Util.withinBounds(currPose, desPose, transBound, rotBound) and not rospy.is_shutdown():
        rospy.loginfo('LOOP!!!!!!!!!')
        
        gripperControl.pause()

        currPose = tfx.pose(gripperControl.getGripperPose(MyConstants.Frames.Link0))
    
        deltaPose = Util.deltaPose(currPose, desPose)
    
        gripperControl.goToGripperPoseDelta(gripperControl.getGripperPose(MyConstants.Frames.Link0), deltaPose)
        
        rate.sleep()

    def execute(self, userdata):
        if MasterClass.PAUSE_BETWEEN_STATES:
           pause_func(self)

        rospy.loginfo('Moving to receptacle')
        # move to receptacle with object

        currPose = tfx.pose(self.ravenArm.getGripperPose())
        receptaclePose = tfx.pose(userdata.receptaclePose)    
        #ignore orientation
        receptaclePose.orientation = currPose.orientation

        print 'getting trajectory'
        endPoseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, receptaclePose)
        print 'got receptacle trajectory', endPoseTraj is None

        if endPoseTraj != None:
            self.ravenArm.executePoseTrajectory(endPoseTraj)
        

        rospy.loginfo('Opening the gripper')
        # open gripper (consider not all the way)
        #self.ravenArm.openGripper(duration=self.gripperOpenCloseDuration)
        self.ravenArm.setGripper(0.75)
        return 'success'

    def alignGrippers(self):
        rightArmPose = self.rightArm.get_pose()
        leftArmPose = self.leftArm.get_pose()
        newLeftArmPose = tfx.pose(leftArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0))  # Create new pose

        newRightArmPose = tfx.pose(rightArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0))
        newRightJoints = self.rightArm.ik(newRightArmPose)  # Updates arm pose if valid
        newLeftJoints = self.leftArm.ik(newLeftArmPose)
        if newRightJoints is not None:
            self.rightArm.go_to_joints(newRightJoints)
            print ("new_pose: {0}".format(newRightArmPose))
            print "Aligning right gripper..."
            self.rightArm.open_gripper()
            self.valid = True
        else:
            rospy.loginfo("Invalid Right Arm Pose")
            print "RIGHT ALIGNMENT FAILED"
            self.valid = False
            return self
        rospy.sleep(0.5)
        if newLeftJoints is not None:
            self.leftArm.go_to_joints(newLeftJoints)
            print "Aligning left gripper..."
            self.leftArm.open_gripper()
        else:
            rospy.loginfo("Invalid Left Arm Pose")
            print "LEFT ALIGNMENT FAILED"
        rospy.sleep(0.5)

 def raiseArms(self):
     rightArmPose = self.rightArm.get_pose()
     leftArmPose = self.leftArm.get_pose()
     newRightArmPose = tfx.pose(rightArmPose)  # Create new pose
     newLeftArmPose = tfx.pose(leftArmPose)  # Create new pose
     newRightArmPose.position = (
         RIGHT_UP
     )  # either update by adding to xyz offset array "+ [0, -pos_step, 0]" or assign new position, defualt keeps position the same
     newLeftArmPose.position = LEFT_UP
     newRightJoints = self.rightArm.ik(newRightArmPose)  # Updates arm pose if valid
     newLeftJoints = self.leftArm.ik(newLeftArmPose)
     if newRightJoints is not None:
         self.rightArm.go_to_joints(newRightJoints)
         print ("new_pose: {0}".format(newRightArmPose))
         self.valid = True
     else:
         rospy.loginfo("Invalid Right Arm Pose")
         print newRightArmPose
         self.valid = False
         return self
     rospy.sleep(0.01)
     if newLeftJoints is not None:
         self.leftArm.go_to_joints(newLeftJoints)
     else:
         rospy.loginfo("Invalid Left Arm Pose")
     rospy.sleep(0.01)
     return self

    def goToPose(self, end, start=None, duration=None, speed=None):
        if start == None:
            start = self.currentPose
            if start == None:
                rospy.loginfo('Have not received currentPose yet, aborting goToPose')
                return

        start = tfx.pose(start)
        end = tfx.pose(end)
        
        if duration is None:
            if speed is None:
                speed = self.defaultPoseSpeed
            duration = end.position.distance(start.position) / speed
        '''
        print
        print 'POSE', end
        print
        '''
        def fn(cmd, t, start=start, end=end, arm=self.arm):
            pose = start.interpolate(end, t)
            
            toolPose = pose.msg.Pose()
            '''
            if t == 1:
                print
                print 'END', toolPose
                print
            '''
            # not sure if correct
            cmd.controller = Constants.CONTROLLER_CARTESIAN_SPACE
            RavenController.addArmPoseCmd(cmd, arm, toolPose)

        self.addStage('goToPose', duration, fn)

def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
    """
    Returns pose0 - pose1
    """

    currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
    currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
    
    currPos, desPos = currPose.position, desPose.position
    currRot, desRot = currPose.orientation, desPose.orientation

    if posFrame is not None and currPoseFrame is not None:
        tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
        currPos = tf_currPos_to_posFrame * currPos

        tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
        desPos = tf_desPos_to_posFrame * desPos

    if rotFrame is not None and currPoseFrame is not None:
        tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
        currRot = tf_currRot_to_rotFrame * currRot

        tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
        desRot = tf_desRot_to_rotFrame * desRot

    deltaPosition = desPos.array - currPos.array
    
    currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
    deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)

    deltaPose = tfx.pose(deltaPosition, deltaQuat)

    return deltaPose

    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 test_image_rays():
    env = rave.Environment()
    env.Load('../envs/pr2-test.env.xml')
    env.SetViewer('qtcoin')
    env.GetViewer().SendCommand('SetFiguresInCamera 1') # also shows the figures in the image
    time.sleep(1)
    robot = env.GetRobots()[0]
    
    cam = robot.GetAttachedSensor('head_cam').GetSensor()
    type = rave.Sensor.Type.Camera
    cam_geom = cam.GetSensorGeometry(type)
    
    depth = robot.GetAttachedSensor('head_depth').GetSensor()
    type = rave.Sensor.Type.Laser
    depth_geom = depth.GetSensorGeometry(type)
    
    #cam.Configure(rave.Sensor.ConfigureCommand.PowerOn)
    #cam.Configure(rave.Sensor.ConfigureCommand.RenderDataOn)
    
    #cam_pose = tfx.pose(cam.GetTransform())
    #cam_pose.position.z += .32
    
    cam_pose = tfx.pose([0,0,0.05], frame='wide_stereo_optical_frame')
    cam_pose_world = tfx.pose(utils.openraveTransformFromTo(robot, cam_pose.matrix, cam_pose.frame, 'world'))
    img_plane_center = cam_pose + [0, 0, .01]
    
    global handles
    img_plane_world = tfx.pose(utils.openraveTransformFromTo(robot, img_plane_center.matrix, cam_pose.frame, 'world'))
    #handles.append(utils.plot_point(env, img_plane_world.position.array, size=.0005))
    
    height, width, _ = cam_geom.imagedata.shape
    f = cam_geom.KK[0,0]
    F = .01 # real focal length in meters
    
    W = F*(width/f)
    H = F*(height/f)
    
    width_offsets = np.linspace(-W/2.0, W/2.0, 64)
    height_offsets = np.linspace(-H/2.0, H/2.0, 48)
    
    directions = np.zeros((len(width_offsets)*len(height_offsets), 3))
    
    index = 0
    for w_offset in width_offsets:
        for h_offset in height_offsets:
            p = img_plane_center + [w_offset, h_offset, 0]
            p_world = tfx.pose(utils.openraveTransformFromTo(robot, p.matrix, p.frame, 'world'))
            direction = (p_world.position.array - cam_pose_world.position.array)
            direction = 5 * direction/np.linalg.norm(direction)
            directions[index,:] = direction
            index += 1
            
            #closest_collision(env, cam_pose_world.position.array, direction, plot=False)
            #handles.append(utils.plot_point(env, p_world.position.array, size=.0001))
    start_time = time.time()
    closest_collisions(env, cam_pose_world.position.array, directions, plot=False)
    print('Total time: {0}'.format(time.time() - start_time))
    
    IPython.embed()
    rave.RaveDestroy()

  def succesorState(self):
    ''' Updates current state to the next succesor state
      by default it just returns current state
    '''
    rightArmPose = self.rightArm.get_pose()
    leftArmPose = self.leftArm.get_pose()
    newRightArmPose = tfx.pose(rightArmPose) # Create new pose
    newLeftArmPose = tfx.pose(leftArmPose) # Create new pose

    ''' ------ CHANGE CODE BELOW THIS POINT ------ '''

    newRightArmPose.position = [0.632,  -0.740,  0.772] # either update by adding to xyz offset array "+ [0, -pos_step, 0]" or assign new position, defualt keeps position the same
    newLeftArmPose.position = [0.632,  0.177,  0.772]

    ''' ------ CHANGE CODE ABOVE THIS POINT ------ '''


    newRightJoints = self.rightArm.ik(newRightArmPose) # Updates arm pose if valid
    newLeftJoints = self.leftArm.ik(newLeftArmPose)
    if newRightJoints is not None:
      self.rightArm.go_to_joints(newRightJoints)
      print('new_pose: {0}'.format(newRightArmPose))
    else:
      rospy.loginfo('Invalid Right Arm Pose')
    rospy.sleep(.01)
    if newLeftJoints is not None:
      self.leftArm.go_to_joints(newLeftJoints)
    else:
      rospy.loginfo('Invalid Left Arm Pose')
    rospy.sleep(.01)
    return self # Modify to generate a valid succesor states, does not update state if pose is invalid and instead reverts to last valid state

    def __init__(self, arm=Constants.Arm.Left):
        self.arm = arm

        if self.arm == Constants.Arm.Left:
            self.toolframe = Constants.Frames.LeftTool
        else:
            self.toolframe = Constants.Frames.RightTool

        self.transFrame = Constants.Frames.Link0
        self.rotFrame = self.toolframe
            
        self.gripperControl = GripperControlClass(arm, tf.TransformListener())
        self.imageDetector = ARImageDetectionClass()
                
        self.homePose = self.imageDetector.getHomePose()

        # ar_frame is the target
        self.ar_frame = '/stereo_33'
        self.objectPose = tfx.pose([0,0,0], frame=self.ar_frame)
        tf_ar_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, self.ar_frame, wait=5)
        self.objectPose = tf_ar_to_link0 * self.objectPose
        self.objectPose = tfx.pose(self.objectPose.position, tfx.tb_angles(-90,90,0))
        self.objectPose.position.y -= .002

        rospy.sleep(3)

 def teleop(self):
     print('{0} arm teleop'.format(self.lrlong))
     
     pos_step = .01
     delta_position = {'a' : [0, pos_step, 0],
                       'd' : [0, -pos_step, 0],
                       'w' : [pos_step, 0, 0],
                       'x' : [-pos_step, 0, 0],
                       '+' : [0, 0, pos_step],
                       '-' : [0, 0, -pos_step]}
     
     angle_step = 2.0
     delta_angle = {'o' : [angle_step, 0, 0],
                    'p' : [-angle_step, 0, 0],
                    'k' : [0, angle_step, 0],
                    'l' : [0, -angle_step, 0],
                    'n' : [0, 0, angle_step],
                    'm' : [0, 0, -angle_step]}
     
     char = ''
     while char != 'q':
         char = utils.Getch.getch()
         pose = self.get_pose()
         new_pose = tfx.pose(pose)
         if delta_position.has_key(char):
             new_pose.position = pose.position.array + delta_position[char]
         ypr = np.array([pose.tb_angles.yaw_deg, pose.tb_angles.pitch_deg, pose.tb_angles.roll_deg])
         if delta_angle.has_key(char):
             ypr += np.array(delta_angle[char])
             new_pose = tfx.pose(pose.position, tfx.tb_angles(ypr[0], ypr[1], ypr[2]))
         self.set_pose(new_pose)    
         time.sleep(.01)
         
     print('{0} arm end teleop'.format(self.lrlong))

def main():
    bag = rosbag.Bag(sys.argv[1])

    output_bag_name = sys.argv[2]

    # pose_topic = '/dvrk_psm1/joint_position_cartesian'
    # gripper_angle_topic = '/dvrk_psm1/gripper_position'

    # pose_and_angle_tuples = {}
    # i = 0

    pose_topic_R = '/dvrk_psm1/joint_position_cartesian'
    gripper_angle_topic_R = '/dvrk_psm1/gripper_position'

    pose_topic_L = '/dvrk_psm2/joint_position_cartesian'
    gripper_angle_topic_L = '/dvrk_psm2/gripper_position'

    pose_and_angle_tuples_R = {}
    pose_and_angle_tuples_L = {}
    i = 0
    j = 0
    curr_angle_R = None
    curr_pose_R = None
    curr_angle_L = None
    curr_pose_L = None

    for topic, msg, t in bag.read_messages(topics=[pose_topic_R, gripper_angle_topic_R, 
        pose_topic_L, gripper_angle_topic_L]):
        if topic == gripper_angle_topic_R:
            curr_angle_R = msg.data
        elif topic == pose_topic_R:
            curr_pose_R = tfx.pose(msg)
        elif topic == gripper_angle_topic_L:
            curr_angle_L = msg.data
        else:
            curr_pose_L = tfx.pose(msg)

        if curr_pose_R != None and curr_angle_R != None:

            data = [curr_pose_R.position.x, curr_pose_R.position.y, curr_pose_R.position.z, curr_pose_R.rotation.x,
                curr_pose_R.rotation.y, curr_pose_R.rotation.z, curr_pose_R.rotation.w, curr_pose_R.stamp.seconds, curr_angle_R]
            pose_and_angle_tuples_R[i] = data
            i += 1
            curr_angle_R = None
            curr_pose_R = None

        if curr_pose_L != None and curr_angle_L != None:
            data = [curr_pose_L.position.x, curr_pose_L.position.y, curr_pose_L.position.z, curr_pose_L.rotation.x,
                curr_pose_L.rotation.y, curr_pose_L.rotation.z, curr_pose_L.rotation.w, curr_pose_L.stamp.seconds, curr_angle_L]
            # pair = (curr_pose_L, curr_angle_L)
            pose_and_angle_tuples_L[j] = data
            j += 1
            curr_angle_L = None
            curr_pose_L = None
    IPython.embed()
    traj = (pose_and_angle_tuples_L.values(), pose_and_angle_tuples_R.values())
    # IPython.embed()
    pickle.dump(traj, open(output_bag_name, 'wb' ))

    bag.close()

    def run(self):
        while(self.running):
            if True:
                self.broadcaster.sendTransform(CHESSBOARD_TRANSLATION, tfx.tb_to_quat(CHESSBOARD_ROLL_0LINK, CHESSBOARD_PITCH_0LINK, CHESSBOARD_YAW_0LINK),
                    rospy.Time.now(), CHESSBOARD_FRAME, '0_link')
                
                self.broadcaster.sendTransform(SCREW_TRANSLATION, tfx.tb_to_quat(SCREW_ROLL_0LINK, SCREW_PITCH_0LINK, SCREW_YAW_0LINK),
                    rospy.Time.now(), SCREW_FRAME, '0_link')
                

                # get markers and accompanying metadata
                frame = owlGetIntegerv(OWL_FRAME_NUMBER)[0]
                timestamp = owlGetIntegerv(OWL_TIMESTAMP)
                if len(timestamp) > 0:
                    timeInSeconds = timestamp[1]
                    timeInSeconds += timestamp[2] * 1e-6
                    self.messageTime = rospy.Time.from_sec(timeInSeconds)
                
                markers = owlGetMarkers()
                
                if owlGetError() != OWL_NO_ERROR:
                    rospy.loginfo('Getting markers failed...')
                    sys.exit(0)
                    pass
                
                gripperMarkers = []
                registrationMarkers = []
                
                # find the correct markers
                for i in range(markers.size()):
                    m = markers[i]
                    
                    if m.cond > 0:
                        if m.id in GRIPPER_MARKER_IDS:
                            gripperMarkers.append(m)
                        elif m.id in REGISTRATION_MARKER_IDS:
                            registrationMarkers.append(m)
    
                        if self.verbose:
                            print("marker %06d %d: %f %f %f %f" % (frame, m.id, m.cond, m.x, m.y, m.z))
                            
                # get the gripper pose (in phasespace basis)
                if len(gripperMarkers) == 3:
                    gTrans, gRot = self.poseFromMarkers2(gripperMarkers, GRIPPER_MARKER_IDS, rotate=True)
                    
                    gripperPosePhasespace = tfx.pose(gTrans, gRot, frame=PHASESPACE_FRAME, stamp=self.messageTime)
                    self.gripper_pose_pub.publish(gripperPosePhasespace.msg.PoseStamped())
                    
                    self.broadcaster.sendTransform(gTrans, gRot, self.messageTime, PHASESPACE_GRIPPER_FRAME, PHASESPACE_FRAME)
                
                # get the registration marker pose (in phasespace basis)
                if self.register and len(registrationMarkers) == 3:
                    rTrans, rRot = self.poseFromMarkers(registrationMarkers, REGISTRATION_MARKER_IDS, rotate=False)
                    screwPosePhasespaceFrame = tfx.pose(rTrans, rRot)
                    phasespacePoseScrewFrame = screwPosePhasespaceFrame.inverse()

                    self.broadcaster.sendTransform(phasespacePoseScrewFrame.translation, phasespacePoseScrewFrame.rotation, self.messageTime, PHASESPACE_FRAME, CHESSBOARD_FRAME)
                
            self.finished = True

def testSwitchPlaces(show=True):
    #trajoptpy.SetInteractive(True)
    
    rospy.init_node('testSwitchPlaces',anonymous=True)
    rp = RavenPlanner([MyConstants.Arm.Left,MyConstants.Arm.Right], thread=True)
    
    #rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0)
    #leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0)
    
    leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
    rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)

    #rp.getTrajectoryPoseToPose(MyConstants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50)
    #rp.getTrajectoryPoseToPose(MyConstants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50)
    
    rospy.loginfo('Press enter to get poses')
    raw_input()
    
    rp.getPoseTrajectory(MyConstants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50)
    rp.getPoseTrajectory(MyConstants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50)
    
    #IPython.embed()
    
    print 'waiting'
    rp.waitForTrajReady()
    print 'woooooooo'
    
    if rospy.is_shutdown():
        return
    
    #IPython.embed()
    
    if not show:
        return
    rp.env.SetViewer('qtcoin')
    
    leftPoseTraj = rp.poseTraj[MyConstants.Arm.Left]
    rightPoseTraj = rp.poseTraj[MyConstants.Arm.Right]
    
    for left, right in zip(leftPoseTraj,rightPoseTraj):
        if rospy.is_shutdown():
            break
        rp.updateOpenraveJoints('L', rp.getJointsFromPose('L', left, rp.getCurrentGrasp('L')), grasp=rp.getCurrentGrasp('L'))
        rp.updateOpenraveJoints('R', rp.getJointsFromPose('R', right, rp.getCurrentGrasp('R')), grasp=rp.getCurrentGrasp('R'))
        rospy.loginfo('Press enter to go to next step')
        raw_input()
        
    return
    
    leftTraj = rp.jointTraj[MyConstants.Arm.Left]
    rightTraj = rp.jointTraj[MyConstants.Arm.Right]
    
    for left, right in zip(leftTraj,rightTraj):
        if rospy.is_shutdown():
            break
        rp.updateOpenraveJoints('L', left)
        rp.updateOpenraveJoints('R', right)
        rospy.loginfo('Press enter to go to next step')
        raw_input()

 def _avg_handle_poses_callback(self, msg):
     min_dist, handle_pose = np.inf, None
     for pose in msg.poses:
         dist = tfx.pose(pose).position.norm
         if dist < min_dist:
             min_dist = dist
             handle_pose = tfx.pose(pose, header=msg.header)
     self.handle_pose = handle_pose

    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 _estimatePoseCallback(msg):
     rospy.loginfo('Received estimated gripper pose in MoveTowardsReceptacle')
     print 'pose'
     print tfx.pose(msg)
     print 'time'
     print tfx.pose(msg).stamp.seconds
     print 'now - msg time'
     print rospy.Time.now().to_sec() - tfx.pose(msg).stamp.seconds

    def __init__(self):

        self.outcomes = None

        self.homePoseLeft = tfx.pose([-0.05009142015689537, 0.03540081898889297, -0.14583058800170023],
            (-0.01797605558439532, -0.9838454461982115, 0.03728902700169569, 0.17416810237794897))

        self.homePoseRight = tfx.pose([0.061241857051286236, 0.014307808069346816, -0.10446866837544996],
            (-0.9689889616996428, 0.1939060552483166, -0.1474787309756946, 0.04136251626876463))