def __init__(self, whicharm):
        

        self.compliant = False
        if (whicharm == 0):
          self.arm = Arm('r')
          gripper_traj_serv_name = '/r_gripper_traj_action'
          effort_topic_name = '/r_arm_controller/effort_exceeded'
        else:
          self.arm = Arm('l')
          gripper_traj_serv_name = '/l_gripper_traj_action'
          effort_topic_name = '/l_arm_controller/effort_exceeded'

        self.kinematics_utils = kinematicsUtils.KinematicsUtils()
        self.whicharm = whicharm

        #Joint velocity limits in rad/sec
        self.vel_limits = [0.8, 0.8, 2.0, 2.0, 3.0, 3.0, 10.0]
        #self.vel_limits = [1.0]*7

        self.traj_goal = JointTrajectoryGoal()
        self.traj_goal.trajectory.points = []
        
        #Connect to the gripper trajectory action server
        self.gripper_traj_client = al.SimpleActionClient(gripper_traj_serv_name, Pr2GripperTrajAction)
        while not self.gripper_traj_client.wait_for_server(rospy.Duration(5.0)):
            print "Waiting for the gripper traj action server..."
        print "Connected to gripper traj action server"
        self.grip_traj_goal = Pr2GripperTrajGoal()
        self.grip_traj_goal.gripper_traj = []

        self.joint_names = self.kinematics_utils.getJointNames(self.whicharm)
        self.traj_goal.trajectory.joint_names = self.joint_names
      
        #Set up empty logging file
        logfile = open('logIK.pickle', 'w')
        logfile.close()
        
        #Keep track of the correspondence between old points passed in and new respaced points
        self.adjusted_ind = []    
        
        #Keep track if there is a previous traj we were following
        self.is_first_traj = True


        rospy.Subscriber(effort_topic_name, Bool, self.effortCB)
class CartesianTrajExecIK():
    
    #0=right, 1=left
    def __init__(self, whicharm):
        

        self.compliant = False
        if (whicharm == 0):
          self.arm = Arm('r')
          gripper_traj_serv_name = '/r_gripper_traj_action'
          effort_topic_name = '/r_arm_controller/effort_exceeded'
        else:
          self.arm = Arm('l')
          gripper_traj_serv_name = '/l_gripper_traj_action'
          effort_topic_name = '/l_arm_controller/effort_exceeded'

        self.kinematics_utils = kinematicsUtils.KinematicsUtils()
        self.whicharm = whicharm

        #Joint velocity limits in rad/sec
        self.vel_limits = [0.8, 0.8, 2.0, 2.0, 3.0, 3.0, 10.0]
        #self.vel_limits = [1.0]*7

        self.traj_goal = JointTrajectoryGoal()
        self.traj_goal.trajectory.points = []
        
        #Connect to the gripper trajectory action server
        self.gripper_traj_client = al.SimpleActionClient(gripper_traj_serv_name, Pr2GripperTrajAction)
        while not self.gripper_traj_client.wait_for_server(rospy.Duration(5.0)):
            print "Waiting for the gripper traj action server..."
        print "Connected to gripper traj action server"
        self.grip_traj_goal = Pr2GripperTrajGoal()
        self.grip_traj_goal.gripper_traj = []

        self.joint_names = self.kinematics_utils.getJointNames(self.whicharm)
        self.traj_goal.trajectory.joint_names = self.joint_names
      
        #Set up empty logging file
        logfile = open('logIK.pickle', 'w')
        logfile.close()
        
        #Keep track of the correspondence between old points passed in and new respaced points
        self.adjusted_ind = []    
        
        #Keep track if there is a previous traj we were following
        self.is_first_traj = True


        rospy.Subscriber(effort_topic_name, Bool, self.effortCB)
        
    
    
    def effortCB(self,data):
        if (self.compliant):
          if (data.data): 
            self.arm.cancelTraj()


    #Use matplotlib to plot the positions in the plan
    def plotPositions(self,plan):
        n_pts = len(plan.points)
        dims = len(plan.points[0].positions)
         
        t_vec = [0]*n_pts
        x_vec = [0]*n_pts
        v_vec = [0]*n_pts
        
        plt.figure()
        for i in range(0,7):
            for j in range(n_pts):
                t_vec[j] = plan.points[j].time_from_start.to_sec()
                x_vec[j] = plan.points[j].positions[i]
            plt.plot(t_vec, x_vec) 
              
  
  
    #Takes a trjectory point and modifies the joint positions so that rotation happens in closest direction to the prev point
    def fixJointRollDirection(self, new_p, last_p):
        for i in [4,6]:
            while(new_p[i] - last_p[i] >= 3.14159):
                new_p[i] -= 6.28318
            while(new_p[i] - last_p[i] < -3.14159):
                new_p[i] += 6.28318
          
          
          
    #If transition over dt is within joint velocity limits, then just return the point to be added to the arm and gripper trajs
    #Otherwise, return multiple points with dt spacing that prevent velocity limit from being exceeded    
    def modifyForJointLimits(self, arm1, arm2, grip1, grip2, dt):

        t_diff = arm2.time_from_start.to_sec() - arm1.time_from_start.to_sec()     
        p1 = arm1.positions
        p2 = arm2.positions   

        new_arm_points = []
        new_grip_points = [] 

        #First, find the joint that has the worst diff to limit ratio, since it will take the most spacing
        diffs = [p2[d]-p1[d] for d in range(0,7)]    
        ratios = [math.fabs(diffs[d] / t_diff) / self.vel_limits[d] for d in range(0,7)]
        max_r = max(ratios)
        
        #Respace points if any joint violates its velocity limit
        if(max_r > 1.0):
            #print "MAX RATIO: ", max_r
            #print arm1.time_from_start.to_sec(), t_diff, diffs
            n_pts = int(math.ceil(max_r))
            increments = [(diffs[d]/n_pts)*dt for d in range(0,7)]
            grip_increment = (grip2-grip1)/n_pts

            #Create a point for each increment that keeps within vel limts
            for i in range(n_pts):
                jp = JointTrajectoryPoint()
                jp.time_from_start = rospy.Duration(arm1.time_from_start.to_sec() + (dt*(i+1)))
                jp.positions = [p1[d] + ((i+1)*increments[d]) for d in range(0,7)]
                new_arm_points.append(jp)
                new_grip_points.append(grip1 + (grip_increment*(i+1)))

        #Otherwise, just return the original points
        else:
            new_arm_points = [arm2]
            new_grip_points = [grip2]

        return [new_arm_points, new_grip_points]
                
    
    
    #Returns the index of the point in the current respaced trajectory that will be active (i.e. robot heading towards) at rostime time 
    def getAdjustedTrajPointAtTime(self,time):
        base_time = self.traj_goal.trajectory.header.stamp
        points = self.traj_goal.trajectory.points
        ind = len(points)-1
        
        for i in range(len(points)):
            if (base_time + points[i].time_from_start) > time:
                ind = i
                break
                
        return ind
        
    
    
    #Returns the index of the point from the original passed-in trajectory that corresponds to the active point in the respaced traj
    def getOrigTrajPointAtTime(self,time):
        ind = self.getAdjustedTrajPointAtTime(time)
        return self.adjusted_ind[ind]
    
    
    
    #Returns the predicted joint pose plus gripper at rostime time
    def getPredictedJointPoseAtTime(self,time):
        ind = self.getAdjustedTrajPointAtTime(time)
        if ind >= 0: 
            if ind >= len(self.grip_traj_goal.gripper_traj):
                ind = len(self.grip_traj_goal.gripper_traj)-1
            if ind >= len(self.traj_goal.trajectory.points) and len(self.traj_goal.trajectory.points) < len(self.grip_traj_goal.gripper_traj):
                ind = len(self.traj_goal.trajectory.points)-1
            return [self.traj_goal.trajectory.points[ind].positions, self.grip_traj_goal.gripper_traj[ind]]
        else:
            return []
    
    
    #Returns the predicted cartesian plus gripper velocity at rostime time
    def getPredictedCartVelAtTime(self,time):
        ind = self.getAdjustedTrajPointAtTime(time)
        next_ind = ind+1
        if next_ind >= len(self.traj_goal.trajectory.points):
            next_ind = ind
        
        p1 = self.traj_goal.trajectory.points[ind].positions
        p2 = self.traj_goal.trajectory.points[next_ind].positions
        c1 = self.kinematics_utils.getFK(p1, self.whicharm, True)
        c2 = self.kinematics_utils.getFK(p2, self.whicharm, True)
        
        g1 = self.grip_traj_goal.gripper_traj[ind]
        g2 = self.grip_traj_goal.gripper_traj[next_ind]
        
        t1 = self.traj_goal.trajectory.points[ind].time_from_start
        t2 = self.traj_goal.trajectory.points[next_ind].time_from_start
        t_diff = (t2-t1).to_sec()
        
        if t_diff == 0:
            a_vel = [0.0]*7
            g_vel = 0.0
        else:
            a_vel = [(c2[d]-c1[d])/t_diff for d in range(7)]
            g_vel = (g2 - g1) / t_diff
            
        return [a_vel, g_vel]
    
    
  
    #Converts a cart traj into a smooth joint traj, dealing with IK misses
    #Assumes that we are starting from start_angles for IK seeding and interpolation in case of IK misses at beginning of traj
    def cartTrajToJointTraj(self, start_angles, x_vec, dt):
        points = []
        t = dt  #Never start first point at time zero!
        seed_angles = start_angles[:]
        
        for i in range(len(x_vec)): 
        
            #Make sure quaternion is normalized
            x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7])

            #Get the inverse kinematic solution for joint angles
            resp = self.kinematics_utils.getIK(x_vec[i], seed_angles, self.whicharm, True)


            if(resp.error_code.val == 1):

                angles = []
                for name in self.joint_names:
                    idx = resp.solution.joint_state.name.index(name)
                    angles.append(resp.solution.joint_state.position[idx])

                #angles = resp.solution.joint_state.position
                jp = JointTrajectoryPoint()
                jp.positions = list(angles)
                jp.time_from_start = rospy.Duration(t)
               
                #Fix the arm and wrist roll so that it keeps going in the correct direction
                if(len(points) > 0):
                    self.fixJointRollDirection(jp.positions, points[-1].positions)
                else:
                    self.fixJointRollDirection(jp.positions, start_angles)    
                
                points.append(jp)
                seed_angles = angles
                
            else:
                print "IK error", resp.error_code.val, "on point", i
            
            #Always increment t, even for IK misses
            t += dt
            
        return points
    
    
        
    #Interpolate to fill in the IK miss gaps
    #We do this because it makes it easier to know where we are going to be at any given time for re-planning
    #If we gave 2 widely spaced (in time) points to the JTAC, we don't know exactly where it would go between the 2 points    
    def fillInIKMissGaps(self, start_angles, points, dt):
        interp_points = []
        last_time = rospy.Duration(0)
        
        for i in range(len(points)):        
            p = points[i]
            diff = p.time_from_start - last_time
            t_diff = diff.to_sec()
            
            #Fill in if the points are not dt spaced as they should be
            if t_diff > dt:
                n_pts = int(t_diff/dt)
                
                if(len(interp_points)==0):
                    start = start_angles
                else:
                    start = interp_points[-1].positions
                
                for j in range(n_pts):
                    increments = [(p.positions[d] - start[d]) / n_pts for d in range(7)]
                    jp = JointTrajectoryPoint()
                    jp.positions = [start[d] + (increments[d]*(j+1)) for d in range(7)]
                    jp.time_from_start = last_time + rospy.Duration(dt*(j+1))
                    interp_points.append(jp)
            
            #Otherwise, just add the point
            else:
                interp_points.append(p)
                
            last_time = p.time_from_start
                
        return interp_points
                   
        
    # it is not working yet. Question posted on the moveit mailing list
    def followCartTrajMoveit(self, x_vec, dt):

        moveit_commander.roscpp_initialize(sys.argv)
        group = moveit_commander.MoveGroupCommander("left_arm")

        waypoints = []
        waypoints.append(group.get_current_pose().pose)
        

        for i in range(len(x_vec)): 
        
            #Make sure quaternion is normalized
            x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7])

            wpose = geometry_msgs.msg.Pose()
            wpose.position.x = x_vec[i][0]
            wpose.position.y = x_vec[i][1]
            wpose.position.z = x_vec[i][2]
            wpose.orientation.x = x_vec[i][3]
            wpose.orientation.y = x_vec[i][4]
            wpose.orientation.z = x_vec[i][5]
            wpose.orientation.w = x_vec[i][6]
            waypoints.append(copy.deepcopy(wpose))

        (plan, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             dt,          # eef_step
                             0.0)         # jump_threshold
        print "fraction: ", fraction
        print "plan: ", plan
        group.execute(plan)        

  
    #Follows a cartesian trajectory and tries to synchronize the gripper trajectory with it       
    def followCartTraj(self, x_vec, grip_traj, dt, start_angles, splice_time, blocking, compliant = False):   
        
        total_extra_time = 0
        new_arm_traj = []
        new_grip_traj = []
        new_adj_ind = []    #A mapping from the number of the points sent to original points

        #Figure out where the splice is going to happen and what the starting pose will be
        if self.is_first_traj:
            self.is_first_traj = False
            seg_start_joints = start_angles
            seg_start_grip = grip_traj[0]
        else:
            pred = self.getPredictedJointPoseAtTime(splice_time)
            if len(pred) > 0:
                [seg_start_joints, seg_start_grip] = pred
            else:
                seg_start_joints = start_angles
                seg_start_grip = grip_traj[0]
                

        #Convert to a joint trajectory and then interpolate between IK misses
        #After fillInIKMissGaps, the resulting traj should be the same length as the one input to followCartTraj
        #Thus, our old/new point index correspondence is still one-to-one
        j_traj = self.cartTrajToJointTraj(seg_start_joints, x_vec, dt)
        
        
        j_traj_interp = self.fillInIKMissGaps(seg_start_joints, j_traj, dt)
        

        #Set up starting conditions for segment
        start_p = JointTrajectoryPoint()
        start_p.time_from_start = rospy.Duration(0.0)
        start_p.positions = seg_start_joints
        start_g = seg_start_grip
        
        #Respace points based on velocity constraints
        for i in range(len(j_traj_interp)):
            
            #Adjust the time of the current traj point based on how many points we've added in along the way
            j_traj_interp[i].time_from_start += rospy.Duration(total_extra_time)
            
            #Respace for veolcity constraints
            [arm_points, grip_points] = self.modifyForJointLimits(start_p, j_traj_interp[i], start_g, grip_traj[i], dt)
            
            #Add the newly spaced points to our new trajectory
            new_arm_traj += arm_points
            new_grip_traj += grip_points
            
            #Record times and correspondences of points to account for repsacing
            #The adjusted ind is the point from the original traj corresponding to the respaced point that is active.
            n_pts = len(arm_points)
            total_extra_time += (n_pts-1) * dt
            new_adj_ind += [i for x in range(n_pts)]
            
            start_p.time_from_start = arm_points[-1].time_from_start
            start_p.positions = arm_points[-1].positions
            start_g = grip_points[-1]
        
        #Make sure we don't splice in the past, or else it will try to go very quickly to a point far into our traj, breaking velocity bounds
        #Also, allow a bit of time for traj processing in the realtime controller
        if splice_time < rospy.Time.now():
            if len(new_arm_traj) > 0:
                print "******** Splice time earlier than current time: ", (splice_time - rospy.Time.now()).to_sec()
                splice_time = rospy.Time.now() 
                
                #Respace between the current pose and the first point so we don't go too fast, since we've passed where we thought we'd be
                #This might cause jerkiness, since the first point might be behind us now, but there's no other solution unless we re-planning
                #This case seems to come up when there are lots of IK misses and not enough actualy traj time for the delay
                start_p = JointTrajectoryPoint()
                start_p.time_from_start = rospy.Duration(0.0)
                start_p.positions = start_angles
                [arm_points, grip_points] = self.modifyForJointLimits(start_p, new_arm_traj[0], new_grip_traj[0], new_grip_traj[0], dt)
                
                #Add on the new points to the front of the traj and update the times for the rest of the points
                extra = len(arm_points) - 1
                extra_t = rospy.Duration(extra*dt)
                for el in new_arm_traj:
                    el.time_from_start += extra_t
                new_arm_traj = arm_points + new_arm_traj[1:]
            
        #Send trajs to joint action controller and gripper traj action controller
        self.traj_goal.trajectory.points = list(new_arm_traj)
        self.traj_goal.trajectory.header.stamp = splice_time #+ rospy.Duration(dt)    #Add in dt so that point we were heading towards isn't deleted, since it isn't in new plan
        self.grip_traj_goal.gripper_traj = list(new_grip_traj)
        self.grip_traj_goal.dt = dt
        
        #print "printing self.grip_traj_goal: ", self.grip_traj_goal
        self.compliant = compliant
        self.gripper_traj_client.send_goal(self.grip_traj_goal)
        self.arm.sendTraj(self.traj_goal)