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 _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if self._mode == "velocity":
         velocities = [0.0] * len(joint_names)
         cmd = dict(zip(joint_names, velocities))
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_velocities(cmd)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
     elif self._mode == "position" or self._mode == "position_w_id":
         raw_pos_mode = self._mode == "position_w_id"
         if raw_pos_mode:
             pnt = JointTrajectoryPoint()
             pnt.positions = self._get_current_position(joint_names)
             if dimensions_dict["velocities"]:
                 pnt.velocities = [0.0] * len(joint_names)
             if dimensions_dict["accelerations"]:
                 pnt.accelerations = [0.0] * len(joint_names)
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
             # zero inverse dynamics feedforward command
             if self._mode == "position_w_id":
                 pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
                 self._pub_ff_cmd.publish(ff_pnt)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)

    def test_trajectory(self):
        # our goal variable
        goal = FollowJointTrajectoryGoal()

        # First, the joint names, which apply to all waypoints
        goal.trajectory.joint_names = ('shoulder_pitch_joint',
                                       'shoulder_pan_joint',
                                       'upperarm_roll_joint',
                                       'elbow_flex_joint',
                                       'forearm_roll_joint',
                                       'wrist_pitch_joint',
                                       'wrist_roll_joint')

        # We will have two waypoints in this goal trajectory

        # First trajectory point
        # To be reached 1 second after starting along the trajectory
        point = JointTrajectoryPoint()
        point.positions = [0.0] * 7
        point.velocities = [1.0] * 7
        point.time_from_start = rospy.Duration(3.0)
        goal.trajectory.points.append(point)

        #we are done; return the goal
        return goal

    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 runTrajectory(req):

    global Traj_server;
    
    print "---------------------------------"
    print req.task
    print " "
    print req.bin_num
    print " "
    print req.using_torso
    print "---------------------------------"
    
    if req.using_torso == "y":
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
    else:
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);

    if req.task == "Forward":
        file_name = file_root+"/forward";
    elif req.task == "Drop":
        file_name = file_root+"/drop";
    else :
        return taskResponse(False);

    f = open(file_name,"r");
    plan_file = RobotTrajectory();
    buf = f.read();
    plan_file.deserialize(buf);
    
    plan = copy.deepcopy(plan_file); 
  
        
    arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); 
    arm_left_group = moveit_commander.MoveGroupCommander("arm_left");	
    arm_left_group.set_start_state_to_current_state();    
        
    StartPnt = JointTrajectoryPoint();
    StartPnt.positions = arm_left_group.get_current_joint_values();
    StartPnt.velocities = [0]*len(StartPnt.positions);
    StartPnt. accelerations = [0]*len(StartPnt.positions);
    
    #print StartPnt;
    plan.joint_trajectory.points[0] = StartPnt;
    #print plan;
    
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    display_trajectory = moveit_msgs.msg.DisplayTrajectory();    
    robot = moveit_commander.RobotCommander();
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    display_trajectory_publisher.publish(display_trajectory);  
   
   
    print "============ Waiting while", file_name, " is visualized (again)..." 
    arm_left_group.execute(plan);
    print "Trajectory ", file_name, " finished!"   
    
    f.close();
    
    return taskResponse(True);

 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 test_joint_trajectory_action(self):
        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
        larm.wait_for_server()

        try:
            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
        except tf.Exception:
            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
        goal = JointTrajectoryGoal()
        goal.trajectory.header.stamp = rospy.get_rostime()
        goal.trajectory.joint_names.append("J1")
        goal.trajectory.joint_names.append("J2")
        goal.trajectory.joint_names.append("J3")
        goal.trajectory.joint_names.append("J4")
        goal.trajectory.joint_names.append("J5")
        goal.trajectory.joint_names.append("J6")
        goal.trajectory.joint_names.append("J7")

        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
        point.time_from_start = rospy.Duration(5.0)
        goal.trajectory.points.append(point)
        larm.send_goal(goal)
        larm.wait_for_result()
        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
        self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)

    def trajectory_cb(self, traj):
        rospy.loginfo("Received a new trajectory, beginning the split..")
        newtraj_arm = JointTrajectory()
        newtraj_arm.header.stamp = traj.header.stamp
        newtraj_arm.header.frame_id = traj.header.frame_id
        
        for name in traj.joint_names:
            if name[:5] == "Joint":
                newtraj_arm.joint_names += (name,)
        
        for point in traj.points:
            newpoint = JointTrajectoryPoint()
            for i, name in enumerate(traj.joint_names):
                if name[:5] == "Joint":
                    newpoint.positions += (point.positions[i],)
                    newpoint.velocities += (point.velocities[i],)
                    newpoint.accelerations += (point.accelerations[i],)
                    newpoint.time_from_start = point.time_from_start
                    
                    
            newtraj_arm.points += (newpoint,)
        
#        rospy.loginfo("Sending trajectory:\n" + str(newtraj_arm))
        rospy.loginfo("Publishing the arm trajectory")
        self.traj_publisher.publish(newtraj_arm)

    def control_loop(self):
        if self.commands is None:
            raise Exception('Command list is empty.  Was the control plan loaded?')
        
        # loop through the command list
        for cmd in self.commands:

            # wait for proxy to be in active state
            self.wait_for_state(self._proxy_state_running)
            
            # process commands
            cmd_spec_str = None
            spec = None
            t = cmd[ProxyCommand.key_command_type]
            if not (t == "noop"):
                cmd_spec_str = cmd[ProxyCommand.key_command_spec]
                if not isinstance(cmd_spec_str, basestring):
                    spec = float(cmd_spec_str)
                else:
                    spec = self.positions[cmd_spec_str]
                rospy.loginfo("Command type: " + t + ", spec: " + str(cmd_spec_str) + ", value: " + str(spec))
                       
            # check for any wait depends
            self.wait_for_depend(cmd)

            # execute command
            # could do this with a dictionary-based function lookup, but who cares
            if t == 'noop':
                rospy.loginfo("Command type: noop")
                self.wait_for_depend(cmd)
            elif t == 'sleep':
                rospy.loginfo("sleep command")
                v = float(spec)
                rospy.sleep(v)
            elif t == 'move_gripper':
                rospy.loginfo("gripper command")
                self.move_gripper(spec)
            elif t == 'move_arm':
                rospy.loginfo("move_arm command")
                rospy.logdebug(spec)                
                goal = FollowJointTrajectoryGoal()
                goal.trajectory.joint_names = self.arm_joint_names
                jtp = JointTrajectoryPoint()
                jtp.time_from_start = rospy.Duration(0.5)  # fudge factor for gazebo controller
                jtp.positions = spec
                jtp.velocities = [0]*len(spec)
                goal.trajectory.points.append(jtp)
                self._arm_goal = copy.deepcopy(goal)
                self.move_arm()
            elif t == 'plan_exec_arm':
                rospy.loginfo("plan and execute command not implemented")
                raise NotImplementedError()
            else:
                raise Exception("Invalid command type: " + str(cmd.type))

            # check for any set dependencies action
            self.set_depend(cmd)

            # check for any clear dependencies action
            self.clear_depend(cmd)

    def createHeadGoal(self):
        (roll, pitch, yaw) = euler_from_quaternion([self.last_oculus_msg.pose.orientation.x,
                                                    self.last_oculus_msg.pose.orientation.y,
                                                    self.last_oculus_msg.pose.orientation.z,
                                                    self.last_oculus_msg.pose.orientation.w])
        rospy.loginfo("roll, pitch, yaw: ")
        rospy.loginfo(str(roll) + " " + str(pitch) + " " + str(yaw))
        head_goal = FollowJointTrajectoryGoal()
        head_goal.trajectory.joint_names.append('head_1_joint') # 1 positivo izquierda, -1 derecha
        head_goal.trajectory.joint_names.append('head_2_joint') # -1 arriba, 1 abajo
        # Current position trajectory point
        jtp_current = JointTrajectoryPoint()
        jtp_current.positions.append(self.last_head_state.actual.positions[0])
        jtp_current.positions.append(self.last_head_state.actual.positions[1])
        jtp_current.velocities.append(self.last_head_state.actual.velocities[0])
        jtp_current.velocities.append(self.last_head_state.actual.velocities[1])
        jtp_current.time_from_start = rospy.Duration(0.0)
        
        # Next goal position
        jtp = JointTrajectoryPoint()
        jtp.positions.append(yaw *-1)
        jtp.positions.append(pitch *-1)
#         jtp.positions.append((yaw *-1) + 1.5707963267948966) # + 180 deg
#         jtp.positions.append((roll + 1.5707963267948966) *-1) # + 180 deg because of torso2
        jtp.velocities.append(0.0)
        jtp.velocities.append(0.0)
        rospy.loginfo("Sending: " + str(jtp.positions))
        #jtp.time_from_start.secs = 1
        jtp.time_from_start.nsecs = 100
        head_goal.trajectory.points.append(jtp_current)
        head_goal.trajectory.points.append(jtp)
        head_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
        return head_goal

    def __init__(self, arm = 'l', record_data = False):
        self.arm = arm
        if arm == 'r':
            print "using right arm on Darci"
            self.RIGHT_ARM = 0  # i don't think this was used anywhere
        else:
            self.LEFT_ARM = 1
        #     print "Left and both arms not implemented in this client yet ... \n"
        #     print "will require writing different function calls since joint data for left and right arm come in together from the meka server"
        #     sys.exit()

        self.kinematics = dak.DarciArmKinematics(arm)
        self.joint_pub = rospy.Publisher('/'+self.arm+'_arm_controller/command', JointTrajectory)
        self.joint_names = None
        self.lock = RLock()
        self.joint_angles = None
        self.joint_velocities = None
        self.J_h = None
        self.time = None
        self.desired_joint_angles = None
        self.stiffness_percent = 0.75
        self.ee_force = None
        self.ee_torque = None
        
        self.skins = None #will need code for all of these skin interfaces
        self.Jc_l = []
        self.n_l = []
        self.values_l = []

        #values from m3gtt repository in robot_config folder
        # These could be read in from a yaml file like this (import yaml; stream = open("FILE_NAME_HERE", 'r'); data = yaml.load(stream))
        # However, not clear if absolute path to yaml file (possibly on another computer) is better then just defining it here
        # The downside is obviously if someone tunes gains differently on the robot.
        self.joint_stiffness = (np.array([1, 1, 1, 1, 0.06, 0.08, 0.08])*180/np.pi*self.stiffness_percent).tolist()
        self.joint_damping = (np.array([0.06, 0.1, 0.015, 0.015, 0.0015, 0.002, 0.002])*180/np.pi*self.stiffness_percent).tolist()
        
        self.record_data = record_data

        if self.record_data:
            from collections import deque
            self.q_record = deque()
            self.qd_record = deque()
            self.times = deque()

        self.state_sub = rospy.Subscriber('/'+self.arm+'_arm_controller/state', JointTrajectoryControllerState, self.robotStateCallback)
        rospy.sleep(1.0)

        while self.joint_angles is None:
            rospy.sleep(0.05)
        self.desired_joint_angles = copy.copy(self.joint_angles)

        self.joint_cmd = JointTrajectory()
        self.joint_cmd.header.stamp = rospy.Time.now()
        self.joint_cmd.header.frame_id = '/torso_lift_link'
        self.joint_cmd.joint_names = self.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = self.desired_joint_angles
        jtp.velocities = [1.]*len(self.joint_names)
        self.joint_cmd.points = [jtp]
        self.joint_pub.publish(self.joint_cmd)

    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 add_point(self, positions, velocities, accelerations, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = copy(velocities)
     point.accelerations = copy(accelerations)
     point.time_from_start = rospy.Duration(time)
     self._goal.trajectory.points.append(point)

    def _execute_trajectory(self, trajectory):
        if self._running:
            return
        self._running = True

        joint_ids = [self._current_joint_states.name.index(joint_name) for joint_name in trajectory.joint_names]
        position_previous = JointTrajectoryPoint()
        position_previous.positions = [self._current_joint_states.position[joint_id] for joint_id in joint_ids]
        # print 'starting from: ' + ('%6.2f ' * len(joint_ids)) % tuple(position_previous.positions)
        position_previous.time_from_start = rospy.Duration(0.0)
        position_index = 0
        start_time = rospy.get_rostime()
        while position_index < len(trajectory.points):
            now = rospy.get_rostime() - start_time
            # print '           to: ' + ('%6.2f ' * len(joint_ids)) % tuple(trajectory.points[position_index].positions)
            while now <= trajectory.points[position_index].time_from_start:
                delta_time = now - position_previous.time_from_start
                position_duration = trajectory.points[position_index].time_from_start - position_previous.time_from_start
                if position_duration.to_sec() == 0.0:
                    progress = 1.0
                else:
                    progress = delta_time.to_sec() / position_duration.to_sec()
                for joint_index, goal_position in enumerate(trajectory.points[position_index].positions):
                    joint_delta = progress * (goal_position - position_previous.positions[joint_index])
                    joint_position = position_previous.positions[joint_index] + joint_delta
                    self._joint_states.position[joint_index] = joint_position
                self._state_publisher.publish(self._joint_states)
                now = rospy.get_rostime() - start_time
                rospy.sleep(0.01)
            position_previous = trajectory.points[position_index]
            position_index += 1

        self._running = False

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 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 get_place_locations(self):
        locations = []
        for xx in np.arange(0.4, 0.7, 0.1):
            for yy in np.arange(0.5, 0.9, 0.1):
                pl = PlaceLocation()
                
                pt = JointTrajectoryPoint()
                pt.positions = [0.5]
                pl.post_place_posture.points.append(pt)
                pl.post_place_posture.joint_names = ['l_gripper_motor_screw_joint']
                
                pl.place_pose.header.frame_id = "odom_combined"
                pl.place_pose.header.stamp = rospy.Time.now()
                pl.place_pose.pose.position.x = xx
                pl.place_pose.pose.position.y = yy
                pl.place_pose.pose.position.z = 1.05
                # Quaternion obtained by pointing wrist down
                #pl.place_pose.pose.orientation.w = 1.0
                pl.place_pose.pose.orientation.x = 0.761
                pl.place_pose.pose.orientation.y = -0.003
                pl.place_pose.pose.orientation.z = -0.648
                pl.place_pose.pose.orientation.w = 0.015

                pl.pre_place_approach.direction.header.frame_id = "odom_combined"
                pl.pre_place_approach.direction.vector.z = -1.0
                pl.pre_place_approach.desired_distance = 0.2
                pl.pre_place_approach.min_distance = 0.1
                
                pl.post_place_retreat.direction.header.frame_id = "odom_combined"
                pl.post_place_retreat.direction.vector.z = 1.0
                pl.post_place_retreat.desired_distance = 0.2
                pl.post_place_retreat.min_distance = 0.1
   
                locations.append(pl)
        return locations

def createHeadGoal(j1, j2):
    """Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
    @arg j1 float value for head_1_joint
    @arg j2 float value for head_2_joint
    @returns FollowJointTrajectoryGoal with the specified goal"""
    fjtg = FollowJointTrajectoryGoal()
    fjtg.trajectory.joint_names.append('head_1_joint')
    fjtg.trajectory.joint_names.append('head_2_joint')
    point = JointTrajectoryPoint()
    point.positions.append(j1)
    point.positions.append(j2)
    point.velocities.append(0.0)
    point.velocities.append(0.0)
    point.time_from_start = rospy.Duration(4.0)
    for joint in fjtg.trajectory.joint_names:  # Specifying high tolerances for the hand as they are slow compared to other hardware
        goal_tol = JointTolerance()
        goal_tol.name = joint
        goal_tol.position = 5.0
        goal_tol.velocity = 5.0
        goal_tol.acceleration = 5.0
        fjtg.goal_tolerance.append(goal_tol)
    fjtg.goal_time_tolerance = rospy.Duration(3)

    fjtg.trajectory.points.append(point)
    fjtg.trajectory.header.stamp = rospy.Time.now()
    return fjtg

 def execute(self, userdata):
     rospy.loginfo('In create_move_group_joints_goal')
     
     _move_joint_goal = FollowJointTrajectoryGoal()
     _move_joint_goal.trajectory.joint_names = userdata.move_joint_list
     
     jointTrajectoryPoint = JointTrajectoryPoint()
     jointTrajectoryPoint.positions = userdata.move_joint_poses
     
     for x in range(0, len(userdata.move_joint_poses)):
         jointTrajectoryPoint.velocities.append(0.0)
      
     jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
     
     _move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
     
     for joint in _move_joint_goal.trajectory.joint_names:
         goal_tol = JointTolerance()
         goal_tol.name = joint
         goal_tol.position = 5.0
         goal_tol.velocity = 5.0
         goal_tol.acceleration = 5.0
         _move_joint_goal.goal_tolerance.append(goal_tol)
     _move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
     _move_joint_goal.trajectory.header.stamp = rospy.Time.now()
     
     rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
     userdata.move_joint_goal = _move_joint_goal
     return 'succeeded'

    def test_joint_angles(self):
        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
        larm.wait_for_server()

        try:
            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
        except tf.Exception:
            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
        goal = JointTrajectoryGoal()
        goal.trajectory.header.stamp = rospy.get_rostime()
        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
        goal.trajectory.joint_names.append("LARM_ELBOW")
        goal.trajectory.joint_names.append("LARM_WRIST_Y")
        goal.trajectory.joint_names.append("LARM_WRIST_P")

        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
        point.time_from_start = rospy.Duration(5.0)
        goal.trajectory.points.append(point)
        larm.send_goal(goal)
        larm.wait_for_result()
        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
        self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)