def interpolate_joint_space(self, goal, total_time, nb_points, start=None):
     """
     Interpolate a trajectory from a start state (or current state) to a goal in joint space
     :param goal: A RobotState to be used as the goal of the trajectory
     param total_time: The time to execute the trajectory
     :param nb_points: Number of joint-space points in the final trajectory
     :param start: A RobotState to be used as the start state, joint order must be the same as the goal
     :return: The corresponding RobotTrajectory
     """
     dt = total_time*(1.0/nb_points)
     # create the joint trajectory message
     traj_msg = JointTrajectory()
     rt = RobotTrajectory()
     if start == None:
         start = self.get_current_state()
     joints = []
     start_state = start.joint_state.position
     goal_state = goal.joint_state.position
     for j in range(len(goal_state)):
         pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1)
         joints.append(pose_lin[1:].tolist())
     for i in range(nb_points):
         point = JointTrajectoryPoint()
         for j in range(len(goal_state)):
             point.positions.append(joints[j][i])
         # append the time from start of the position
         point.time_from_start = rospy.Duration.from_sec((i+1)*dt)
         # append the position to the message
         traj_msg.points.append(point)
     # put name of joints to be moved
     traj_msg.joint_names = self.joint_names()
     # send the message
     rt.joint_trajectory = traj_msg
     return rt

 def send_command(self, cmd):
     rospy.logdebug("  Sending cmd to controller [%s]"%self._controller_id)
     cmd_msg = JointTrajectory()
     cmd_msg.header.stamp = rospy.Time().now()
     cmd_msg.joint_names = self._config["joint_names"]
     cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
     self._pub.publish(cmd_msg)

    def check_collision(self):
        if not self.collsion_srv:
            return False

        # Create a new trajectory by combining both the trajectories, assumption is that the timing is same
        # The order of joining should be [left + right]
        self.adjust_traj_length()
        traj = JointTrajectory()
        traj.joint_names = self.left._goal.trajectory.joint_names + self.right._goal.trajectory.joint_names

        no_points = len(self.left._goal.trajectory.points)
        for index in xrange(no_points):
            pt = JointTrajectoryPoint()
            left_pt = self.left._goal.trajectory.points[index]
            right_pt = self.right._goal.trajectory.points[index]

            pt.positions = left_pt.positions + right_pt.positions
            pt.velocities = left_pt.velocities + right_pt.velocities
            pt.time_from_start = left_pt.time_from_start
            traj.points.append(pt)

        msg = CollisionCheckRequest()
        msg.trajectory = traj

        try:
            collision_service = rospy.ServiceProxy("collision_check", CollisionCheck)
            resp = collision_service(msg)
            if resp.collision_found:
                rospy.logwarn("Collision Found")
            else:
                rospy.loginfo("Collision not found, good to go :)")
            return resp.collision_found
        except rospy.ServiceException as e:
            rospy.logwarn("Exception on collision check {}".format(e))
            return True

    def exectute_joint_traj(self, joint_trajectory, timing):
        '''Moves the arm through the joint sequence'''

        # First, do filtering on the trajectory to fix the velocities
        trajectory = JointTrajectory()

        # Initialize the server
        # When to start the trajectory: 0.1 seconds from now
        trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        trajectory.joint_names = self.joint_names

        ## Add all frames of the trajectory as way points
        for i in range(len(timing)):
            positions = joint_trajectory[i].joint_pose
            velocities = [0] * len(positions)
            # Add frames to the trajectory
            trajectory.points.append(JointTrajectoryPoint(positions=positions,
                                     velocities=velocities,
                                     time_from_start=timing[i]))

        output = self.filter_service(trajectory=trajectory,
                                     allowed_time=rospy.Duration.from_sec(20))
        rospy.loginfo('Trajectory for arm ' + str(self.arm_index) +
                      ' has been filtered.')
        traj_goal = JointTrajectoryGoal()

        # TO-DO: check output.error_code
        traj_goal.trajectory = output.trajectory
        traj_goal.trajectory.header.stamp = (rospy.Time.now() +
                                            rospy.Duration(0.1))
        traj_goal.trajectory.joint_names = self.joint_names

        # Sends the goal to the trajectory server
        self.traj_action_client.send_goal(traj_goal)

def dual_arm_test():
    pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
    cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
    rospy.init_node('dual_arm_test')
    rospy.sleep(1.0)
    while not rospy.is_shutdown():
        traj = JointTrajectory()
        traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
        ptn = JointTrajectoryPoint()
        ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
        ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
        ptn.time_from_start = rospy.Duration(2.0)
        traj.header.stamp = rospy.Time.now()
        traj.points.append(ptn)
        pub.publish(traj)
        cob_pub.publish(traj)
        rospy.sleep(3.0)
        ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
        ptn.time_from_start = rospy.Duration(2.0)
        traj.points = []
        traj.header.stamp = rospy.Time.now()
        traj.points.append(ptn)
        pub.publish(traj)
        cob_pub.publish(traj)
        rospy.sleep(3.0)

    def __create_spin_command(self, arm, theta=math.pi):
        if arm == 'l':
            jnts = self.robot_state.left_arm_positions 
        if arm == 'r':
            jnts = self.robot_state.right_arm_positions 
        
        command = JointTrajectory()
        command.joint_names = ['%s_shoulder_pan_joint' % arm[0], 
                               '%s_shoulder_lift_joint' % arm[0],
                               '%s_upper_arm_roll_joint' % arm[0],
                               '%s_elbow_flex_joint' % arm[0],
                               '%s_forearm_roll_joint' % arm[0],
                               '%s_wrist_flex_joint' % arm[0],
                               '%s_wrist_roll_joint' % arm[0]]
        
        jnts[-1] += theta
        command.points.append(JointTrajectoryPoint(
            positions=jnts,
            velocities = [0.0] * (len(command.joint_names)),
            accelerations = [],
            time_from_start =  rospy.Duration(0.1)))

        goal = JointTrajectoryGoal()
        goal.trajectory = command
        return goal

def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
    """
    This function will convert the plan to a joint trajectory compatible with the 
    /arm_N/arm_controller/command topic
    """
    theplan = plan

    # create joint trajectory message
    jt = JointTrajectory()

    # fill the header
    jt.header.seq = seq
    jt.header.stamp.secs = 0  # secs
    jt.header.stamp.nsecs = 0  # nsecs
    jt.header.frame_id = frame_id

    # specify the joint names
    jt.joint_names = theplan.joint_trajectory.joint_names

    # fill the trajectory points and computer constraint times
    njtp = len(theplan.joint_trajectory.points)
    for ii in range(0, njtp):
        jtp = JointTrajectoryPoint()
        jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
        jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
        jt.points.append(jtp)
    return jt

    def send_arm_goal(self, arm_goal):
        arm_joint_names = ['joint_lift',
                           'joint_waist',
                           'joint_big_arm',
                           'joint_forearm',
                           'joint_wrist_pitching',
                           'joint_wrist_rotation']

        rospy.loginfo("Waiting for follow_joint_trajectory server")
        self.arm_client.wait_for_server()
        rospy.loginfo("Connected to follow_joint_trajectory server")
        rospy.sleep(1)
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joint_names
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].time_from_start = rospy.Duration(10)
        rospy.loginfo("Preparing for moving the arm to goal position!")
        rospy.sleep(1)
        arm_goal_pos = FollowJointTrajectoryGoal()
        arm_goal_pos.trajectory = arm_trajectory

        arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
        self.arm_client.send_goal(arm_goal_pos)
        rospy.loginfo("Send goal to the trajectory server successfully!")
        self.arm_client.wait_for_result()

    def joint_trajectory(self, joint_trajectory):
        '''
        Returns just the part of the trajectory corresponding to the arm.

        **Args:**
        
            **joint_trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to convert

        **Returns:**
            A trajectory_msgs.msg.JointTrajectory corresponding to only this arm in joint_trajectory

        **Raises:**
            
            **ValueError:** If some arm joint is not found in joint_trajectory
        '''
        arm_trajectory = JointTrajectory()
        arm_trajectory.header = copy.deepcopy(joint_trajectory.header)
        arm_trajectory.joint_names = copy.copy(self.joint_names)
        indexes = [-1]*len(self.joint_names)
        for (i, an) in enumerate(self.joint_names):
            for (j, n) in enumerate(joint_trajectory.joint_names):
                if an == n:
                    indexes[i] = j
                    break
            if indexes[i] < 0:
                raise ValueError('Joint '+n+' is missing from joint trajectory')
        
        for joint_point in joint_trajectory.points:
            arm_point = JointTrajectoryPoint()
            arm_point.time_from_start = joint_point.time_from_start
            for i in indexes:
                arm_point.positions.append(joint_point.positions[i])
            arm_trajectory.points.append(arm_point)
        return arm_trajectory

def move_arm( positions, time_from_start = 3.0, arm = 'r', client = None ):

    if (client == None):
        client = init_jt_client(arm)
    else:
        arm = client.action_client.ns[0:1]; # ignore arm argument

    rospy.loginfo( 'move arm \'%s\' to position %s'%( arm, positions ) )    

    joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )
    
    jt = JointTrajectory()
    jt.joint_names = joint_names
    jt.points = []
    jp = JointTrajectoryPoint()
    jp.time_from_start = rospy.Time.from_seconds( time_from_start )
    jp.positions = positions
    jt.points.append( jp )
    
    # push trajectory goal to ActionServer
    jt_goal = JointTrajectoryGoal()
    jt_goal.trajectory = jt
    jt_goal.trajectory.header.stamp = rospy.Time.now() # + rospy.Duration.from_sec( time_from_start )
    client.send_goal( jt_goal )
    client.wait_for_result()    
    return client.get_state()

    def fill_ik_traj(self, poses, dist=None, duration=None, tot_points=None):
        if dist is None:
            dist = self.calc_dist(poses[0], poses[-1])
        if duration is None:
            duration = dist*30
        if tot_points is None:
           tot_points = 500*dist
            
        ik_fracs = np.linspace(0, 1, len(poses))   #A list of fractional positions along course to evaluate ik

        req = self.form_ik_request(poses[0])
        ik_goal = self.ik_pose_proxy(req)
        seed = ik_goal.solution.joint_state.position

        ik_points = [[0]*7 for i in range(len(poses))]
        for i, p in enumerate(poses):
            request = self.form_ik_request(p)
            request.ik_request.ik_seed_state.joint_state.position = seed
            ik_goal = self.ik_pose_proxy(request)
            ik_points[i] = ik_goal.solution.joint_state.position
            seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
        try:
            ik_points = np.array(ik_points)
        except:
            print "Value error"
            print 'ik_points: ', ik_points
        print 'ik_points_array: ', ik_points
        ang_fracs = np.linspace(0,1, tot_points)  
        
        diff = np.max(np.abs(np.diff(ik_points,1,0)),0)
        print 'diff: ', diff
        for i in xrange(len(ik_points)):
            if ik_points[i,4]<0.:
                ik_points[i,4] += 2*math.pi
        if any(ik_points[:,4]>3.8):
            print "OVER JOINT LIMITS!"
            ik_points[:,4] -= 2*math.pi

        # Linearly interpolate angles between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path.  Used to maintain large number of trajectory points without running IK on every one.    
        angle_points = np.zeros((7, tot_points))
        for i in xrange(7):
            angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])

        #Build Joint Trajectory from angle positions
        traj = JointTrajectory()
        traj.header.frame_id = poses[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        #print 'angle_points', len(angle_points[0]), angle_points
        for i in range(len(angle_points[0])):
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = angle_points[:,i]
            traj.points[i].velocities = [0]*7
            traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration)
        angles_safe = self.check_trajectory(traj)
        if angles_safe:
            print "Check Passed: Angles Safe"
            return traj
        else:
            print "Check Failed: Unsafe Angles"
            return None

    def tuck(self):
        # prepare a joint trajectory
        msg = JointTrajectory()
        msg.joint_names = servos
        msg.points = list()
    
        point = JointTrajectoryPoint()
        point.positions = forward
        point.velocities = [ 0.0 for servo in msg.joint_names ]
        point.time_from_start = rospy.Duration(5.0)
        msg.points.append(point)
       # point = JointTrajectoryPoint()
        #point.positions = to_side
        #point.velocities = [ 0.0 for servo in msg.joint_names ]
        #point.time_from_start = rospy.Duration(8.0)
        #msg.points.append(point)
        #point = JointTrajectoryPoint()
        #point.positions = tucked
        #point.velocities = [ 0.0 for servo in msg.joint_names ]
        #point.time_from_start = rospy.Duration(11.0)
        #msg.points.append(point)

        # call action
        msg.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = msg
        self._client.send_goal(goal)

    def convert_trajectory(self, traj):
        """ Converts a trajectory into a joint trajectory
        Args:
            traj: Trajectory to convert
        Returns:
            joint trajectory
        """
        new_traj = JointTrajectory()

        new_traj.header = traj.header
        # Take each joint in the trajectory and add it to the joint trajectory
        for joint_name in traj.joint_names:
            new_traj.joint_names.append(self.joint_map[joint_name].joint_name)
        # For each poiint in the trajectory
        for point in traj.points:
            new_point = JointTrajectoryPoint()
            for i, pos in enumerate(point.positions):
                new_point.positions.append(self.joint_map[new_traj.joint_names[i]].convert_angle(pos))
            for i, vel in enumerate(point.velocities):
                new_point.velocities.append(self.joint_map[new_traj.joint_names[i]].convert_velocity(vel))

            new_point.time_from_start = point.time_from_start
            new_traj.points.append(new_point)

        return new_traj

 def goal_cb(self, pt_msg):
     rospy.loginfo("[{0}] New LookatIK goal received.".format(rospy.get_name()))
     if (pt_msg.header.frame_id != self.camera_ik.base_frame):
         try:
             now = pt_msg.header.stamp
             self.tfl.waitForTransform(pt_msg.header.frame_id,
                                       self.camera_ik.base_frame,
                                       now, rospy.Duration(1.0))
             pt_msg = self.tfl.transformPoint(self.camera_ik.base_frame, pt_msg)
         except (LookupException, ConnectivityException, ExtrapolationException):
             rospy.logwarn("[{0}] TF Error tranforming point from {1} to {2}".format(rospy.get_name(),
                                                                                     pt_msg.header.frame_id,
                                                                                     self.camera_ik.base_frame))
     target = np.array([pt_msg.point.x, pt_msg.point.y, pt_msg.point.z])
     # Get IK Solution
     current_angles = list(copy.copy(self.joint_angles_act))
     iksol = self.camera_ik.lookat_ik(target, current_angles[:len(self.camera_ik.arm_indices)])
     # Start with current angles, then replace angles in camera IK with IK solution
     # Use desired joint angles to avoid sagging over time
     jtp = JointTrajectoryPoint()
     jtp.positions = list(copy.copy(self.joint_angles_des))
     for i, joint_name in enumerate(self.camera_ik.arm_joint_names):
         jtp.positions[self.joint_names.index(joint_name)] = iksol[i]
     deltas = np.abs(np.subtract(jtp.positions, current_angles))
     duration = np.max(np.divide(deltas, self.max_vel_rot))
     jtp.time_from_start = rospy.Duration(duration)
     jt = JointTrajectory()
     jt.joint_names = self.joint_names
     jt.points.append(jtp)
     rospy.loginfo("[{0}] Sending Joint Angles.".format(rospy.get_name()))
     self.joint_traj_pub.publish(jt)

    def create_placings_from_object_pose(self, posestamped):
        """ Create a list of PlaceLocation of the object rotated every 15deg"""
        place_locs = []
        pre_grasp_posture = JointTrajectory()
        # Actually ignored....
        pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id
        pre_grasp_posture.joint_names = [
            name for name in self._gripper_joint_names.split()]
        jtpoint = JointTrajectoryPoint()
        jtpoint.positions = [
            float(pos) for pos in self._gripper_pre_grasp_positions.split()]
        jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
        pre_grasp_posture.points.append(jtpoint)
        # Generate all the orientations every step_degrees_yaw deg
        for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)):
            pl = PlaceLocation()
            pl.place_pose = posestamped
            newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
            pl.place_pose.pose.orientation = Quaternion(
                newquat[0], newquat[1], newquat[2], newquat[3])
            # TODO: the frame is ignored, this will always be the frame of the gripper
            # so arm_tool_link
            pl.pre_place_approach = self.createGripperTranslation(
                Vector3(1.0, 0.0, 0.0))
            pl.post_place_retreat = self.createGripperTranslation(
                Vector3(-1.0, 0.0, 0.0))

            pl.post_place_posture = pre_grasp_posture
            place_locs.append(pl)

        return place_locs

 def move_head(self, pan , tilt):   
     # Which joints define the head?
     head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
     # Create a single-point head trajectory with the head_goal as the end-point
     head_trajectory = JointTrajectory()
     head_trajectory.joint_names = head_joints
     head_trajectory.points.append(JointTrajectoryPoint())
     head_trajectory.points[0].positions = pan , tilt
     head_trajectory.points[0].velocities = [0.0 for i in head_joints]
     head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
     head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
 
     # Send the trajectory to the head action server
     rospy.loginfo('Moving the head to goal position...')
     
     head_goal = FollowJointTrajectoryGoal()
     head_goal.trajectory = head_trajectory
     head_goal.goal_time_tolerance = rospy.Duration(0.0)
 
     # Send the goal
     self.head_client.send_goal(head_goal)
     
     # Wait for up to 5 seconds for the motion to complete 
     self.head_client.wait_for_result(rospy.Duration(2.0))
     
     rospy.loginfo('...done')

    def __init__(self):
        rospy.init_node('arm_simple_trajectory')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Which joints define the arm?
        arm_joints = ['joint_lift',
                      'joint_waist',
                      'joint_big_arm', 
                      'joint_forearm',
                      'joint_wrist_pitching',
                      'joint_wrist_rotation']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        else:
            # Set a goal configuration for the arm
            arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
    
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for xm arm trajectory controller...')
        
        arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
                                                  FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        rospy.sleep(1)
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        rospy.sleep(1)
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)

        rospy.loginfo('...done')
        rospy.sleep(1)

def main():

    pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
    rospy.init_node('traj_test')

    p1 = JointTrajectoryPoint()
    p1.positions = [0,0,0,0,0,0,0]
    p1.velocities = [0,0,0,0,0,0,0]
    p1.accelerations = [0,0,0,0,0,0,0]

    p2 = JointTrajectoryPoint()
    p2.positions = [0,0,1.0,0,-1.0,0,0]
    p2.velocities = [0,0,0,0,0,0,0]
    p2.accelerations = [0,0,0,0,0,0,0]
    p2.time_from_start = rospy.Time(4.0)

    p3 = JointTrajectoryPoint()
    p3.positions = [0,0,-1.0,0,1.0,0,0]
    p3.velocities = [0,0,0,0,0,0,0]
    p3.accelerations = [0,0,0,0,0,0,0]
    p3.time_from_start = rospy.Time(20.0)


    traj = JointTrajectory()
    traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
    traj.points = [p1,p2,p3]


    rospy.loginfo(traj)

    r = rospy.Rate(1) # 10hz
    pub.publish(traj)
    r.sleep()
    pub.publish(traj)

 def make_gripper_posture(self, pose):
     t = JointTrajectory()
     t.joint_names = GRIPPER_JOINT_NAMES
     tp = JointTrajectoryPoint()
     tp.positions = [pose for j in t.joint_names]
     tp.effort = GRIPPER_EFFORT
     t.points.append(tp)
     return t

 def head_up_msg(self):
     jtm = JointTrajectory()
     jtm.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     jtp.positions = [0]*len(self.joint_names)
     jtp.time_from_start = rospy.Duration(1.0)
     jtm.points = [jtp]
     return jtm