Difference between pages "Joint Torque Springs Example" and "Joint Trajectory Playback Example"

(Difference between pages)
Jump to: navigation , search
(Add code walkthrough into one page)
 
(Edit code into one page)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''This example shows joint torque control usage. After moving to neutral, the robot will enter torque control mode, applying torques representing virtual springs holding the joints to their start position. The example calculates and applies torques linearly relative to offset from the start position creating the illusion that springs of configurable stiffness are attached to each joint.'''</span>
+
<span style="font-size:120%;">'''Enable the robot joint trajectory interface, parse a file created using the joint position recorder example, and send the resulting joint trajectory to the action server.'''</span>
  
 
</div>
 
</div>
Line 10: Line 10:
 
== Introduction ==
 
== Introduction ==
  
The Joint Torque Springs Example demonstrates the usage of torque controller to apply torque commands to robot's joints. In the main() function, the dynamic reconfigure from server is initialized and passed to an object of the JointSprings class. On initialization, the object creates an instance of the limb class for the particular side that is passed, and captures the default spring and damping constants from the dynamic reconfigure server. The main() function calls the method, move_to_neutral() to send the limbs to a neutral position. Then, the attach_spring() method captures the initial joint angles of the limb and calls the update_forces() method which calculates the necessary torques based on Hooke's law and publishes them as joint commands.
+
This example demonstrates the usage of the Joint Trajectory Action Server to command raw joint position commands. The <code>main()</code> creates and instance of the Trajectory class and calls the <code>parse_file()</code> method. This method is responsible for parsing the input file and packing them as Request message for the <code>Joint Trajectory Action server</code>. The <code>start()</code> method is then called, which sends the Request messages to the action server. Finally, wait() is then called, which verifies if the trajectory execution was successful. Action Servers - robot/limb/right/follow_joint_trajectory.
 +
<br />
  
'''Interfaces - '''
 
<div style="column-count:3;-moz-column-count:3;-webkit-column-count:3">
 
* Limb.set_joint_torques()
 
* Limb.move_to_neutral()
 
* Limb.joint_angles()
 
* Limb.joint_velocities()
 
* Limb.set_command_timeout()
 
* Limb.exit_control_mode()
 
 
</div>
 
</div>
 +
 +
<div class="content-block">
 +
 +
== Overview ==
 +
 +
A commonly used ROS method for robot arm motion control is the joint trajectory action interface. The trajectory_controller and it's corresponding joint trajectory action server is the intera_interface implementation to support this action interface. This example shows usage for launching the joint trajectory action server, and parsing of a text file describing timestamped joint position goals into a joint trajectory action call to be commanded to the joint trajectory action server.
  
 
</div>
 
</div>
Line 28: Line 27:
 
== Usage ==
 
== Usage ==
  
Start the joint torque springs example from an RSDK terminal session using:
+
Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], ex:
 +
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_torque_springs.py
+
$ rosrun intera_interface robot_enable.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Some messages will appear after start:
+
Record a joint position file using the recorder.py example, ex:
 +
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
Initializing node...
+
$ rosrun intera_examples recorder.py -f <position_file_name>
Getting robot state...
 
Enabling robot...
 
Running. Ctrl-c to quit
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The robot will move to neutral pose first, when done moving to neutral pose, you can interact with the robot by grabbing, pushing, and rotating each joint to feel the torques applied that represent the virtual springs attached.
+
The recorder is now recording joint positions with corresponding timestamps for robot arm. Move the arms while holding the cuffs. If the position file name exists, the function will overwrite existing file.
  
Pressing <code> Control-C </code> at any time will stop torque mode and exit the example.
+
NOTE: You can open and close the gripper while recording by using the robot's cuff buttons: Oval(lower) = Close, Circle(Upper) = Open
 +
 
 +
Start the joint trajectory controller, ex:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_interface joint_trajectory_action_server.py --mode velocity
 +
</syntaxhighlight>
 +
 
 +
In another RSDK terminal session, Run the joint trajectory playback example program, ex:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_examples joint_trajectory_file_playback.py -f <position_file_name>
 +
</syntaxhighlight>
 +
 
 +
The robot arm will then be commanded to repeat the trajectory recorded during the joint position recording. The difference between this example and the joint_position playback example is that the trajectory controller has the ability to honor the velocities (due to the timestamps) to more accurately repeating the recorded trajectory.
  
 
</div>
 
</div>
Line 50: Line 62:
  
 
== Arguments ==
 
== Arguments ==
 +
'''Important Arguments:'''
 +
Arguments for joint_trajectory_action_server.py
  
'''Important Arguments:''' 
+
See the trajectory controller's usage on the command line by passing trajectory_controller.py the -h, help argument:
  
<code> -l </code> or <code> --limb </code>: The robot arm name (default="right")
+
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_interface joint_trajectory_action_server.py -h
 +
</syntaxhighlight>
  
See the joint torque spring's available arguments on the command line by passing joint_torque_springs.py the <code>-h</code>, help argument:
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_torque_springs.py -h
+
Intera SDK Joint Trajectory Controller
 +
 
 +
    Unlike other robots running ROS, this is not a Motor Controller plugin,
 +
    but a regular node using the SDK interface.   
 +
 
 +
optional arguments:
 +
  -h, --help            show this help message and exit
 +
  -l {right}, --limb {right}
 +
                        joint trajectory action server limb (default: right)
 +
  -r RATE, --rate RATE  trajectory control rate (Hz) (default: 100.0)
 +
  -m {position_w_id,position,velocity}, --mode {position_w_id,position,velocity}
 +
                        control mode for trajectory execution (default:
 +
                        position_w_id)
 +
 
 +
</syntaxhighlight>
 +
 
 +
'''Important Arguments:'''
 +
Arguments for joint_trajectory_file_playback.py
 +
 
 +
See the joint_trajectory_file_playback's usage on the command line by passing joint_trajectory_file_playback.py the -h, help argument:
 +
 
 +
<syntaxhighlight lang="bash" enclose="div">
 +
$ rosrun intera_interface joint_trajectory_file_playback.py -h
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
usage: joint_torque_springs.py [-h] [-l LIMB]
+
RSDK Joint Trajectory Example: File Playback
  
RSDK Joint Torque Example: Joint Springs
+
    Plays back joint positions honoring timestamps recorded
 +
    via the joint_recorder example.
  
     Moves the default limb to a neutral location and enters
+
     Run the joint_recorder.py example first to create a recording
     torque control mode, attaching virtual springs (Hooke.s Law)
+
     file for use with this example. Then make sure to start the
     to each joint maintaining the start position.
+
     joint_trajectory_action_server before running this example.
  
     Run this example and interact by grabbing, pushing, and rotating
+
     This example will use the joint trajectory action server
     each joint to feel the torques applied that represent the
+
     with velocity control to follow the positions and times of
     virtual springs attached. You can adjust the spring
+
     the recorded motion, accurately replicating movement speed
     constant and damping coefficient for each joint using
+
     necessary to hit each trajectory point on time.
    dynamic_reconfigure.
 
   
 
  
 +
required arguments:
 +
  -f FILE, --file FILE  path to input file
 
optional arguments:
 
optional arguments:
 
   -h, --help            show this help message and exit
 
   -h, --help            show this help message and exit
   -l LIMB, --limb LIMB
+
   -l LOOPS, --loops LOOPS
                        limb on which to attach joint springs
+
                        number of playback loops. 0=infinite. (default: 1)
 +
</syntaxhighlight>
 +
 
 +
</div>
 +
 
 +
<div class="content-block">
 +
 
 +
== Interfaces ==
 +
 
 +
'''ROS APIs:'''
 +
 
 +
See the API Reference page for details.
 +
<syntaxhighlight lang="bash" enclose="div">
 +
Joint Trajectory Action Server - /robot/limb/right/follow_joint_trajectory [control_msgs/FollowJointTrajectoryAction]
 +
</syntaxhighlight>
 +
 
 +
'''Intera_interface APIs:'''
 +
<syntaxhighlight lang="bash" enclose="div">
 +
JointTrajectoryActionServer class: joint_trajectory_action_server.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Line 92: Line 148:
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 
import argparse
 
import argparse
import importlib
+
import operator
 +
import sys
 +
import threading
 +
 
 +
from bisect import bisect
 +
from copy import copy
 +
from os import path
  
 
import rospy
 
import rospy
from dynamic_reconfigure.server import (
+
 
     Server,
+
import actionlib
 +
 
 +
from control_msgs.msg import (
 +
     FollowJointTrajectoryAction,
 +
    FollowJointTrajectoryGoal,
 
)
 
)
from std_msgs.msg import (
+
from trajectory_msgs.msg import (
     Empty,
+
     JointTrajectoryPoint,
 
)
 
)
  
 
import intera_interface
 
import intera_interface
 +
 
from intera_interface import CHECK_VERSION
 
from intera_interface import CHECK_VERSION
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the limb class. The <code>CHECK_VERSION</code> is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.
+
This imports the intera_interface for accessing the limb and the gripper class. The <code>actionlib</code> is imported to use its <code>Action Server</code> class.The <code>CHECK_VERSION</code> is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.
  
<syntaxhighlight lang="python" line start="43" enclose="div">
+
<syntaxhighlight lang="python" line start="59" enclose="div">
class JointSprings(object):
+
class Trajectory(object):
     """
+
     def __init__(self, limb="right"):
    Virtual Joint Springs class for torque example.
+
        #create our action server clients
 +
        self._limb_client = actionlib.SimpleActionClient(
 +
            'robot/limb/right/follow_joint_trajectory',
 +
            FollowJointTrajectoryAction,
 +
        )
  
    @param limb: limb on which to run joint springs example
+
        #verify joint trajectory action servers are available
    @param reconfig_server: dynamic reconfigure server
+
        is_server_up = self._limb_client.wait_for_server(rospy.Duration(10.0))
 +
        if not is_server_up:
 +
            msg = ("Action server not available."
 +
                  " Verify action server availability.")
 +
            rospy.logerr(msg)
 +
            rospy.signal_shutdown(msg)
 +
            sys.exit(1)
 +
</syntaxhighlight>
  
    JointSprings class contains methods for the joint torque example allowing
+
Action server clients for the right limb is created. The existence of action server for right limb is checked with a timeout of 10 seconds. If it is not available, a shutdown signal is sent and the program exits.
    moving the limb to a neutral location, entering torque mode, and attaching
 
    virtual springs.
 
    """
 
    def __init__(self, reconfig_server, limb = "right"):
 
        self._dyn = reconfig_server
 
  
        # control parameters
+
<syntaxhighlight lang="python" line start="76" enclose="div">
         self._rate = 1000.0  # Hz
+
         #create our goal request
         self._missed_cmds = 20.0  # Missed cycles before triggering timeout
+
         self.goal = FollowJointTrajectoryGoal()
  
         # create our limb instance
+
         #limb interface - current angles needed for start move
         self._limb = intera_interface.Limb(limb)
+
         self.arm = intera_interface.Limb(limb)
  
         # initialize parameters
+
        self.limb = limb
         self._springs = dict()
+
        self.gripper_name = '_'.join([limb, 'gripper'])
         self._damping = dict()
+
         #gripper interface - for gripper command playback
         self._start_angles = dict()
+
         try:
 +
            self.gripper = intera_interface.Gripper(limb)
 +
            self.has_gripper = True
 +
         except:
 +
            self.has_gripper = False
 +
            rospy.loginfo("Did not detect a connected electric gripper.")
 +
           
 +
        #flag to signify the arm trajectories have begun executing
 +
         self._arm_trajectory_started = False
 +
        #reentrant lock to prevent same-thread lockout
 +
        self._lock = threading.RLock()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>JointSprings</code> class object is initialized by passing the side of the limb under interest, an object of the reconfigure server that holds the default spring and damping constants for the limb. These default values can be modified at runtime through the dynamic reconfigure server.
+
The request message that will hold the goal position for right limbs' action server is created. The current joint is also captured. If there is a electric gripper connected, the gripper positions will be captured as well.
  
<syntaxhighlight lang="python" line start="69" enclose="div">
+
<syntaxhighlight lang="python" line start="97" enclose="div">
         # create cuff disable publisher
+
         # Verify Grippers Have No Errors and are Calibrated
         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
+
         if self.has_gripper:
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
+
            if self.gripper.has_error():
 +
                self.gripper.reboot()
 +
            if not self.gripper.is_calibrated():
 +
                self.gripper.calibrate()
  
        # verify robot is enabled
+
            #gripper goal trajectories
        print("Getting robot state... ")
+
            self.grip = FollowJointTrajectoryGoal()
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
+
 
         self._init_state = self._rs.state().enabled
+
            #gripper control rate
         print("Enabling robot... ")
+
            self._gripper_rate = 20.0  # Hz
         self._rs.enable()
+
 
         print("Running. Ctrl-c to quit")
+
        # Timing offset to prevent gripper playback before trajectory has started
 +
         self._slow_move_offset = 0.0
 +
         self._trajectory_start_offset = rospy.Duration(0.0)
 +
         self._trajectory_actual_offset = rospy.Duration(0.0)
 +
 
 +
        #param namespace
 +
         self._param_ns = '/rsdk_joint_trajectory_action_server/'
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The zero gravity mode can be enabled by pressing the cuff and so it is important to disable the cuff. A publisher for the cuff interaction suppress topic is created. It is also important to note that the robot should be enabled before trying to manipulate its joints. So, the robot is checked if it is enabled and if not, it is enabled. The initial state of the variable is recorded in <code>_init_state</code> variable. This is to ensure that the robot is sent back to that state once the example program ends.
+
The <code>error()</code> method returns whether or not the gripper is in an error state. The possible cause of errors might be over/under-voltage, over/under-current, motor faults, etc. The <code>calibrated()</code> method returns a boolean, true if the gripper is already calibrated. The request message that will hold the goal position for the gripper is created. The namespace is modified and the gripper's control rates is modified.
  
<syntaxhighlight lang="python" line start="81" enclose="div">
+
<syntaxhighlight lang="python" line start="118" enclose="div">
     def _update_parameters(self):
+
     def _execute_gripper_commands(self):
         for joint in self._limb.joint_names():
+
         start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
            self._springs[joint] = self._dyn.config[joint[-2:] + '_spring_stiffness']
+
        grip_cmd = self.grip.trajectory.points
             self._damping[joint] = self._dyn.config[joint[-2:] + '_damping_coefficient']
+
        pnt_times = [pnt.time_from_start.to_sec() for pnt in grip_cmd]
 +
        end_time = pnt_times[-1]
 +
        rate = rospy.Rate(self._gripper_rate)
 +
        now_from_start = rospy.get_time() - start_time
 +
        while(now_from_start < end_time + (1.0 / self._gripper_rate) and
 +
              not rospy.is_shutdown()):
 +
             idx = bisect(pnt_times, now_from_start) - 1
 +
            if self.has_gripper:
 +
                self.gripper.set_position(grip_cmd[idx].positions[0])
 +
            rate.sleep()
 +
            now_from_start = rospy.get_time() - start_time
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>_update_parameters()</code> method captures the dynamically entered user's input for the spring and damping constants through the dynamic reconfigure server.
+
The grip_cmd variable holds the gripper' position that was parsed from the file. The corresponding time stamps are read into pnt_times variable. idx holds the index of the timestamp from the file, relative to the current one. It is important to note that the velocity at which the Joint Positions was traversed is preserved during the playback. The corresponding gripper's position is then commanded to the action server.
  
<syntaxhighlight lang="python" line start="86" enclose="div">
+
<syntaxhighlight lang="python" line start="133" enclose="div">
     def _update_forces(self):
+
     def _clean_line(self, line, joint_names):
 
         """
 
         """
         Calculates the current angular difference between the start position
+
         Cleans a single line of recorded joint positions
         and the current joint positions applying the joint torque spring forces
+
 
         as defined on the dynamic reconfigure server.
+
        @param line: the line described in a list to process
 +
         @param joint_names: joint name keys
 +
 
 +
        @return command: returns dictionary {joint: value} of valid commands
 +
         @return line: returns list of current line values stripped of commas
 
         """
 
         """
         # get latest spring constants
+
        def try_float(x):
         self._update_parameters()
+
            try:
 
+
                return float(x)
         # disable cuff interaction
+
            except ValueError:
         self._pub_cuff_disable.publish()
+
                return None
 +
         #convert the line of strings to a float or None
 +
         line = [try_float(x) for x in line.rstrip().split(',')]
 +
        #zip the values with the joint names
 +
        combined = zip(joint_names[1:], line[1:])
 +
        #take out any tuples that have a none value
 +
        cleaned = [x for x in combined if x[1] is not None]
 +
         #convert it to a dictionary with only valid commands
 +
        command = dict(cleaned)
 +
         return (command, line,)
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The cuff interaction is disabled as long as the example runs and each time the forces are updated, the <code>_pub_cuff_disable</code> publisher publishes an empty message.
+
This <code>try_float()</code> function replaces the contents of the variable passed with float and none values. Then, a tuple is constructed by zipping the names passed with the values from the variable line. This may contain valid values as well as none values . After removing the none values, a dictionary is constructed. This dictionary is then parsed to create valid Joint commands.
  
<syntaxhighlight lang="python" line start="98" enclose="div">
+
<syntaxhighlight lang="python" line start="158" enclose="div">
         # create our command dict
+
    def _add_point(self, positions, side, time):
         cmd = dict()
+
        """
         # record current angles/velocities
+
         Appends trajectory with new point
         cur_pos = self._limb.joint_angles()
+
 
         cur_vel = self._limb.joint_velocities()
+
        @param positions: joint positions
         # calculate current forces
+
        @param side: limb to command point
        for joint in self._start_angles.keys():
+
         @param time: time from start for point in seconds
            # spring portion
+
        """
            cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
+
         #creates a point in trajectory with time_from_start and positions
                                                  cur_pos[joint])
+
         point = JointTrajectoryPoint()
             # damping portion
+
        point.positions = copy(positions)
            cmd[joint] -= self._damping[joint] * cur_vel[joint]
+
         point.time_from_start = rospy.Duration(time)
        # command new joint torques
+
         if side == self.limb:
        self._limb.set_joint_torques(cmd)
+
            self.goal.trajectory.points.append(point)
 +
        elif self.has_gripper and side == self.gripper_name:
 +
             self.grip.trajectory.points.append(point)
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Hooke's law is applied to calculate the necessary torques to be applied on each joints. The updated spring and damping constants are used in the calculation. The <code>set_joint_torques()</code> method of the limb interface publishes the set of joint torques and their corresponding names with the mode as 3 to the topic <code>robot/limb/right/joint_command </code>. The mode indicates that the joint commands are to be passed to the torque controller.
+
This method creates a request message of the <code>JointTrajectoryPoint</code> type and appends the goal position based on the side of the limb/gripper that is requesting. It compiles a list of Joint Positions as the trajectory.
  
<syntaxhighlight lang="python" line start="113" enclose="div">
+
<syntaxhighlight lang="python" line start="175" enclose="div">
     def move_to_neutral(self):
+
     def parse_file(self, filename):
 
         """
 
         """
         Moves the limb to neutral location.
+
         Parses input file into FollowJointTrajectoryGoal format
 +
 
 +
        @param filename: input filename
 
         """
 
         """
         self._limb.move_to_neutral()
+
         #open recorded file
 +
        with open(filename, 'r') as f:
 +
            lines = f.readlines()
 +
        #read joint names specified in file
 +
        joint_names = lines[0].rstrip().split(',')
 +
        #parse joint names for right limb
 +
        for name in joint_names:
 +
            if self.limb == name[:-3]:
 +
                self.goal.trajectory.joint_names.append(name)
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>move_to_neutral()</code> method of the limb interface, moves all the joints to their neutral pose. This method employs the position controller to send the joints to a pre-defined neutral position.
+
The lines are split into a list with ',' as the delimiter to extract the joint names. The arm of the current joint, found by stripping the "_<joint>" (last three characters) from the joint_name, is checked and stored.
 +
 
 +
<syntaxhighlight lang="python" line start="191" enclose="div">
 +
        def find_start_offset(pos):
 +
            #create empty lists
 +
            cur = []
 +
            cmd = []
 +
            dflt_vel = []
 +
            vel_param = self._param_ns + "%s_default_velocity"
 +
            #for all joints find our current and first commanded position
 +
            #reading default velocities from the parameter server if specified
 +
            for name in joint_names:
 +
                if self.limb == name[:-3]:
 +
                    cmd.append(pos[name])
 +
                    cur.append(self.arm.joint_angle(name))
 +
                    prm = rospy.get_param(vel_param % name, 0.25)
 +
                    dflt_vel.append(prm)
 +
            diffs = map(operator.sub, cmd, cur)
 +
            diffs = map(operator.abs, diffs)
 +
            #determine the largest time offset necessary across all joints
 +
            offset = max(map(operator.div, diffs, dflt_vel))
 +
            return offset
 +
</syntaxhighlight>
  
<syntaxhighlight lang="python" line start="119" enclose="div">
+
The first commanded position and the current position for all the joints are captured. The default values that were loaded into the param server are read. The largest time offset necessary across all the joints is calculated.
     def attach_springs(self):
+
 
 +
<syntaxhighlight lang="python" line start="211" enclose="div">
 +
        for idx, values in enumerate(lines[1:]):
 +
            #clean each line of file
 +
            cmd, values = self._clean_line(values, joint_names)
 +
            #find allowable time offset for move to start position
 +
            if idx == 0:
 +
                # Set the initial position to be the current pose.
 +
                # This ensures we move slowly to the starting point of the
 +
                # trajectory from the current pose - The user may have moved
 +
                # arm since recording
 +
                cur_cmd = [self.arm.joint_angle(jnt) for jnt in self.goal.trajectory.joint_names]
 +
                self._add_point(cur_cmd, self.limb, 0.0)
 +
                start_offset = find_start_offset(cmd)
 +
                # Gripper playback won't start until the starting movement's
 +
                # duration has passed, and the actual trajectory playback begins
 +
                self._slow_move_offset = start_offset
 +
                self._trajectory_start_offset = rospy.Duration(start_offset + values[0])
 +
            #add a point for this set of commands with recorded time
 +
            cur_cmd = [cmd[jnt] for jnt in self.goal.trajectory.joint_names]
 +
            self._add_point(cur_cmd, self.limb, values[0] + start_offset)
 +
            if self.has_gripper:
 +
                cur_cmd = [cmd[self.gripper_name]]
 +
                self._add_point(cur_cmd, self.gripper_name, values[0] + start_offset)
 +
</syntaxhighlight>
 +
 
 +
The non-float values are cleaned and a dictionary of valid Joint Commands is returned in the _clean_line() function as explained above. The time offset for move to start position is also calculated. The parsed set of recorded commands along with their recorded time is added to the goal list as well as gripper's command and time.
 +
 
 +
<syntaxhighlight lang="python" line start="234" enclose="div">
 +
     def _feedback(self, data):
 +
        # Test to see if the actual playback time has exceeded
 +
        # the move-to-start-pose timing offset
 +
        if (not self._get_trajectory_flag() and
 +
              data.actual.time_from_start >= self._trajectory_start_offset):
 +
            self._set_trajectory_flag(value=True)
 +
            self._trajectory_actual_offset = data.actual.time_from_start
 +
</syntaxhighlight>
 +
 
 +
Called in start function, test to see if the actual playback time has exceeded the move-to-start-pose timing offset.
 +
 
 +
<syntaxhighlight lang="python" line start="242" enclose="div">
 +
    def _set_trajectory_flag(self, value=False):
 +
        with self._lock:
 +
            # Assign a value to the flag
 +
            self._arm_trajectory_started = value
 +
 
 +
    def _get_trajectory_flag(self):
 +
        temp_flag = False
 +
        with self._lock:
 +
            # Copy to external variable
 +
            temp_flag = self._arm_trajectory_started
 +
        return temp_flag
 +
</syntaxhighlight>
 +
 
 +
Assign the value to the trajectory flag function and get the trajectory flag value function.
 +
 
 +
<syntaxhighlight lang="python" line start="254" enclose="div">
 +
    def start(self):
 
         """
 
         """
         Switches to joint torque mode and attached joint springs to current
+
         Sends FollowJointTrajectoryAction request
        joint positions.
 
 
         """
 
         """
         # record initial joint angles
+
        self._limb_client.send_goal(self.goal, feedback_cb=self._feedback)
         self._start_angles = self._limb.joint_angles()
+
         # Syncronize playback by waiting for the trajectories to start
 +
         while not rospy.is_shutdown() and not self._get_trajectory_flag():
 +
            rospy.sleep(0.05)
 +
        if self.has_gripper:
 +
            self._execute_gripper_commands()
 +
</syntaxhighlight>
  
        # set control rate
+
This function sends the FollowJointTrajectoryAction request message which includes the recorded positions and their corresponding time, to the Joint Trajectory Action Server.
        control_rate = rospy.Rate(self._rate)
 
  
         # for safety purposes, set the control rate command timeout.
+
<syntaxhighlight lang="python" line start="265" enclose="div">
         # if the specified number of command cycles are missed, the robot
+
    def stop(self):
         # will timeout and disable
+
         """
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
+
        Preempts trajectory execution by sending cancel goals
 +
         """
 +
         if (self._limb_client.gh is not None and
 +
            self._limb_client.get_state() == actionlib.GoalStatus.ACTIVE):
 +
            self._limb_client.cancel_goal()
  
         # loop at specified rate commanding new joint torques
+
         #delay to allow for terminating handshake
         while not rospy.is_shutdown():
+
         rospy.sleep(0.1)
            if not self._rs.state().enabled:
 
                rospy.logerr("Joint torque example failed to meet "
 
                            "specified control rate timeout.")
 
                break
 
            self._update_forces()
 
            control_rate.sleep()
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The start angles recorded here are used by the <code>_update_forces</code> method to determine the dx, which is the displacement caused from the starting position of the joints. The <code>_update_forces</code> method is invoked at 1000Hz, which performs the spring motion as explained above.
+
The Trajectory execution is stopped by sending cancel goals to the Joint trajectory action server.
  
<syntaxhighlight lang="python" line start="144" enclose="div">
+
<syntaxhighlight lang="python" line start="276" enclose="div">
     def clean_shutdown(self):
+
     def wait(self):
 
         """
 
         """
         Switches out of joint torque mode to exit cleanly
+
         Waits for and verifies trajectory execution result
 
         """
 
         """
         print("\nExiting example...")
+
         #create a timeout for our trajectory execution
         self._limb.exit_control_mode()
+
        #total time trajectory expected for trajectory execution plus a buffer
         if not self._init_state and self._rs.state().enabled:
+
        last_time = self.goal.trajectory.points[-1].time_from_start.to_sec()
             print("Disabling robot...")
+
         time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
             self._rs.disable()
+
        timeout = rospy.Duration(self._slow_move_offset +
 +
                                last_time +
 +
                                time_buffer)
 +
 
 +
         finish = self._limb_client.wait_for_result(timeout)
 +
        result = (self._limb_client.get_result().error_code == 0)
 +
 
 +
        #verify result
 +
        if all([finish, result]):
 +
             return True
 +
        else:
 +
            msg = ("Trajectory action failed or did not finish before "
 +
                  "timeout/interrupt.")
 +
             rospy.logwarn(msg)
 +
            return False
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The method <code>exit_control_mode()</code> of the limb interface switches to position controller mode from torque/velocity controller and saves the current joint angles as the current joint position. Finally, it checks if the robot was initially disabled and if so disables it.
+
This method waits for the completion of the trajectory execution by Joint trajectory action server<code>.
  
<syntaxhighlight lang="python" line start="155" enclose="div">
+
<syntaxhighlight lang="python" line start="301" enclose="div">
 
def main():
 
def main():
     """RSDK Joint Torque Example: Joint Springs
+
     """RSDK Joint Trajectory Example: File Playback
  
     Moves the default limb to a neutral location and enters
+
     Plays back joint positions honoring timestamps recorded
     torque control mode, attaching virtual springs (Hooke's Law)
+
     via the joint_recorder example.
    to each joint maintaining the start position.
 
  
     Run this example and interact by grabbing, pushing, and rotating
+
     Run the joint_recorder.py example first to create a recording
     each joint to feel the torques applied that represent the
+
    file for use with this example. Then make sure to start the
     virtual springs attached. You can adjust the spring
+
    joint_trajectory_action_server before running this example.
     constant and damping coefficient for each joint using
+
 
     dynamic_reconfigure.
+
     This example will use the joint trajectory action server
 +
    with velocity control to follow the positions and times of
 +
    the recorded motion, accurately replicating movement speed
 +
     necessary to hit each trajectory point on time.
 +
     """
 +
     epilog = """
 +
Related examples:
 +
  joint_recorder.py; joint_position_file_playback.py.
 
     """
 
     """
     # Querying the parameter server to determine Robot model and limb name(s)
+
     arg_fmt = argparse.RawDescriptionHelpFormatter
     rp = intera_interface.RobotParams()
+
     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
    valid_limbs = rp.get_limb_names()
+
                                    description=main.__doc__,
     if not valid_limbs:
+
                                    epilog=epilog)
        rp.log_message(("Cannot detect any limb parameters on this robot. "
+
     parser.add_argument(
                        "Exiting."), "ERROR")
+
        '-f', '--file', metavar='PATH', required=True,
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
+
        help='path to input file'
    # Parsing Input Arguments
+
     )
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
 
     parser = argparse.ArgumentParser(formatter_class=arg_fmt)
 
 
     parser.add_argument(
 
     parser.add_argument(
         "-l", "--limb", dest="limb", default=valid_limbs[0],
+
         '-l', '--loops', type=int, default=1,
        choices=valid_limbs,
+
         help='number of playback loops. 0=infinite.'
         help='limb on which to attach joint springs'
+
    )
        )
+
    # remove ROS args and filename (sys.arv[0]) for argparse
 
     args = parser.parse_args(rospy.myargv()[1:])
 
     args = parser.parse_args(rospy.myargv()[1:])
    # Grabbing Robot-specific parameters for Dynamic Reconfigure
+
</syntaxhighlight>
    config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
+
 
    config_module = "intera_examples.cfg"
+
The name source file to read the Joint Position is captured along with the number of times the trajectory has to be looped from the command line.
    cfg = importlib.import_module('.'.join([config_module,config_name]))
+
 
    # Starting node connection to ROS
+
<syntaxhighlight lang="python" line start="335" enclose="div">
 
     print("Initializing node... ")
 
     print("Initializing node... ")
     rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
+
     rospy.init_node("sdk_joint_trajectory_file_playback")
     dynamic_cfg_srv = Server(cfg, lambda config, level: config)
+
    print("Getting robot state... ")
     js = JointSprings(dynamic_cfg_srv, limb=args.limb)
+
     rs = intera_interface.RobotEnable(CHECK_VERSION)
    # register shutdown callback
+
     print("Enabling robot... ")
    rospy.on_shutdown(js.clean_shutdown)
+
     rs.enable()
     js.move_to_neutral()
+
     print("Running. Ctrl-c to quit")
     js.attach_springs()
 
  
 +
    traj = Trajectory()
 +
    traj.parse_file(path.expanduser(args.file))
 +
    #for safe interrupt handling
 +
    rospy.on_shutdown(traj.stop)
 +
    result = True
 +
    loop_cnt = 1
 +
    loopstr = str(args.loops)
 +
    if args.loops == 0:
 +
        args.loops = float('inf')
 +
        loopstr = "forever"
 +
    while (result == True and loop_cnt <= args.loops
 +
          and not rospy.is_shutdown()):
 +
        print("Playback loop %d of %s" % (loop_cnt, loopstr,))
 +
        traj.start()
 +
        result = traj.wait()
 +
        loop_cnt = loop_cnt + 1
 +
    print("Exiting - File Playback Complete")
  
 
if __name__ == "__main__":
 
if __name__ == "__main__":
Line 300: Line 541:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The rospy node is initialized and then an instance of the dynamic reconfigure is created. As indicated above, this supplies the default and runtime, spring and dynamic constants. In custom programs this can be avoided by assigning spring and damping constants directly in the code.
+
The node is initialized. An instance of the <code>Trajectory</code> class is created. <code>The parse_file()</code> method is called to extract the recorded Joint Positions and their corresponding timestamps as explained above. The <code>start()</code> method is called to send the <code>FollowJointTrajectory</code> request message to the <code>Joint Trajectory Action server</code>. This loops for the user specified number of times.
  
 
</div>
 
</div>

Revision as of 10:43, 25 November 2016

Enable the robot joint trajectory interface, parse a file created using the joint position recorder example, and send the resulting joint trajectory to the action server.

Introduction

This example demonstrates the usage of the Joint Trajectory Action Server to command raw joint position commands. The main() creates and instance of the Trajectory class and calls the parse_file() method. This method is responsible for parsing the input file and packing them as Request message for the Joint Trajectory Action server. The start() method is then called, which sends the Request messages to the action server. Finally, wait() is then called, which verifies if the trajectory execution was successful. Action Servers - robot/limb/right/follow_joint_trajectory.

Overview

A commonly used ROS method for robot arm motion control is the joint trajectory action interface. The trajectory_controller and it's corresponding joint trajectory action server is the intera_interface implementation to support this action interface. This example shows usage for launching the joint trajectory action server, and parsing of a text file describing timestamped joint position goals into a joint trajectory action call to be commanded to the joint trajectory action server.

Usage

Verify that the robot is enabled from an SDK terminal session, ex:

$ rosrun intera_interface robot_enable.py

Record a joint position file using the recorder.py example, ex:

$ rosrun intera_examples recorder.py -f <position_file_name>

The recorder is now recording joint positions with corresponding timestamps for robot arm. Move the arms while holding the cuffs. If the position file name exists, the function will overwrite existing file.

NOTE: You can open and close the gripper while recording by using the robot's cuff buttons: Oval(lower) = Close, Circle(Upper) = Open

Start the joint trajectory controller, ex:

$ rosrun intera_interface joint_trajectory_action_server.py --mode velocity

In another RSDK terminal session, Run the joint trajectory playback example program, ex:

$ rosrun intera_examples joint_trajectory_file_playback.py -f <position_file_name>

The robot arm will then be commanded to repeat the trajectory recorded during the joint position recording. The difference between this example and the joint_position playback example is that the trajectory controller has the ability to honor the velocities (due to the timestamps) to more accurately repeating the recorded trajectory.

Arguments

Important Arguments: Arguments for joint_trajectory_action_server.py

See the trajectory controller's usage on the command line by passing trajectory_controller.py the -h, help argument:

$ rosrun intera_interface joint_trajectory_action_server.py -h
Intera SDK Joint Trajectory Controller

    Unlike other robots running ROS, this is not a Motor Controller plugin,
    but a regular node using the SDK interface.    

optional arguments:
  -h, --help            show this help message and exit
  -l {right}, --limb {right}
                        joint trajectory action server limb (default: right)
  -r RATE, --rate RATE  trajectory control rate (Hz) (default: 100.0)
  -m {position_w_id,position,velocity}, --mode {position_w_id,position,velocity}
                        control mode for trajectory execution (default:
                        position_w_id)

Important Arguments: Arguments for joint_trajectory_file_playback.py

See the joint_trajectory_file_playback's usage on the command line by passing joint_trajectory_file_playback.py the -h, help argument:

$ rosrun intera_interface joint_trajectory_file_playback.py -h
RSDK Joint Trajectory Example: File Playback

    Plays back joint positions honoring timestamps recorded
    via the joint_recorder example.

    Run the joint_recorder.py example first to create a recording
    file for use with this example. Then make sure to start the
    joint_trajectory_action_server before running this example.

    This example will use the joint trajectory action server
    with velocity control to follow the positions and times of
    the recorded motion, accurately replicating movement speed
    necessary to hit each trajectory point on time.

required arguments:
  -f FILE, --file FILE   path to input file
optional arguments:
  -h, --help            show this help message and exit
  -l LOOPS, --loops LOOPS
                         number of playback loops. 0=infinite. (default: 1)

Interfaces

ROS APIs:

See the API Reference page for details.

Joint Trajectory Action Server - /robot/limb/right/follow_joint_trajectory [control_msgs/FollowJointTrajectoryAction]

Intera_interface APIs:

JointTrajectoryActionServer class: joint_trajectory_action_server.py

Code Walkthrough

Now, let's break down the code.

33 import argparse
34 import operator
35 import sys
36 import threading
37 
38 from bisect import bisect
39 from copy import copy
40 from os import path
41 
42 import rospy
43 
44 import actionlib
45 
46 from control_msgs.msg import (
47     FollowJointTrajectoryAction,
48     FollowJointTrajectoryGoal,
49 )
50 from trajectory_msgs.msg import (
51     JointTrajectoryPoint,
52 )
53 
54 import intera_interface
55 
56 from intera_interface import CHECK_VERSION

This imports the intera_interface for accessing the limb and the gripper class. The actionlib is imported to use its Action Server class.The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

59 class Trajectory(object):
60     def __init__(self, limb="right"):
61         #create our action server clients
62         self._limb_client = actionlib.SimpleActionClient(
63             'robot/limb/right/follow_joint_trajectory',
64             FollowJointTrajectoryAction,
65         )
66 
67         #verify joint trajectory action servers are available
68         is_server_up = self._limb_client.wait_for_server(rospy.Duration(10.0))
69         if not is_server_up:
70             msg = ("Action server not available."
71                    " Verify action server availability.")
72             rospy.logerr(msg)
73             rospy.signal_shutdown(msg)
74             sys.exit(1)

Action server clients for the right limb is created. The existence of action server for right limb is checked with a timeout of 10 seconds. If it is not available, a shutdown signal is sent and the program exits.

76         #create our goal request
77         self.goal = FollowJointTrajectoryGoal()
78 
79         #limb interface - current angles needed for start move
80         self.arm = intera_interface.Limb(limb)
81 
82         self.limb = limb
83         self.gripper_name = '_'.join([limb, 'gripper'])
84         #gripper interface - for gripper command playback
85         try:
86             self.gripper = intera_interface.Gripper(limb)
87             self.has_gripper = True
88         except:
89             self.has_gripper = False
90             rospy.loginfo("Did not detect a connected electric gripper.")
91             
92         #flag to signify the arm trajectories have begun executing
93         self._arm_trajectory_started = False
94         #reentrant lock to prevent same-thread lockout
95         self._lock = threading.RLock()

The request message that will hold the goal position for right limbs' action server is created. The current joint is also captured. If there is a electric gripper connected, the gripper positions will be captured as well.

 97         # Verify Grippers Have No Errors and are Calibrated
 98         if self.has_gripper:
 99             if self.gripper.has_error():
100                 self.gripper.reboot()
101             if not self.gripper.is_calibrated():
102                 self.gripper.calibrate()
103 
104             #gripper goal trajectories
105             self.grip = FollowJointTrajectoryGoal()
106 
107             #gripper control rate
108             self._gripper_rate = 20.0  # Hz
109 
110         # Timing offset to prevent gripper playback before trajectory has started
111         self._slow_move_offset = 0.0
112         self._trajectory_start_offset = rospy.Duration(0.0)
113         self._trajectory_actual_offset = rospy.Duration(0.0)
114 
115         #param namespace
116         self._param_ns = '/rsdk_joint_trajectory_action_server/'

The error() method returns whether or not the gripper is in an error state. The possible cause of errors might be over/under-voltage, over/under-current, motor faults, etc. The calibrated() method returns a boolean, true if the gripper is already calibrated. The request message that will hold the goal position for the gripper is created. The namespace is modified and the gripper's control rates is modified.

118     def _execute_gripper_commands(self):
119         start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
120         grip_cmd = self.grip.trajectory.points
121         pnt_times = [pnt.time_from_start.to_sec() for pnt in grip_cmd]
122         end_time = pnt_times[-1]
123         rate = rospy.Rate(self._gripper_rate)
124         now_from_start = rospy.get_time() - start_time
125         while(now_from_start < end_time + (1.0 / self._gripper_rate) and
126               not rospy.is_shutdown()):
127             idx = bisect(pnt_times, now_from_start) - 1
128             if self.has_gripper:
129                 self.gripper.set_position(grip_cmd[idx].positions[0])
130             rate.sleep()
131             now_from_start = rospy.get_time() - start_time

The grip_cmd variable holds the gripper' position that was parsed from the file. The corresponding time stamps are read into pnt_times variable. idx holds the index of the timestamp from the file, relative to the current one. It is important to note that the velocity at which the Joint Positions was traversed is preserved during the playback. The corresponding gripper's position is then commanded to the action server.

133     def _clean_line(self, line, joint_names):
134         """
135         Cleans a single line of recorded joint positions
136 
137         @param line: the line described in a list to process
138         @param joint_names: joint name keys
139 
140         @return command: returns dictionary {joint: value} of valid commands
141         @return line: returns list of current line values stripped of commas
142         """
143         def try_float(x):
144             try:
145                 return float(x)
146             except ValueError:
147                 return None
148         #convert the line of strings to a float or None
149         line = [try_float(x) for x in line.rstrip().split(',')]
150         #zip the values with the joint names
151         combined = zip(joint_names[1:], line[1:])
152         #take out any tuples that have a none value
153         cleaned = [x for x in combined if x[1] is not None]
154         #convert it to a dictionary with only valid commands
155         command = dict(cleaned)
156         return (command, line,)

This try_float() function replaces the contents of the variable passed with float and none values. Then, a tuple is constructed by zipping the names passed with the values from the variable line. This may contain valid values as well as none values . After removing the none values, a dictionary is constructed. This dictionary is then parsed to create valid Joint commands.

158     def _add_point(self, positions, side, time):
159         """
160         Appends trajectory with new point
161 
162         @param positions: joint positions
163         @param side: limb to command point
164         @param time: time from start for point in seconds
165         """
166         #creates a point in trajectory with time_from_start and positions
167         point = JointTrajectoryPoint()
168         point.positions = copy(positions)
169         point.time_from_start = rospy.Duration(time)
170         if side == self.limb:
171             self.goal.trajectory.points.append(point)
172         elif self.has_gripper and side == self.gripper_name:
173             self.grip.trajectory.points.append(point)

This method creates a request message of the JointTrajectoryPoint type and appends the goal position based on the side of the limb/gripper that is requesting. It compiles a list of Joint Positions as the trajectory.

175     def parse_file(self, filename):
176         """
177         Parses input file into FollowJointTrajectoryGoal format
178 
179         @param filename: input filename
180         """
181         #open recorded file
182         with open(filename, 'r') as f:
183             lines = f.readlines()
184         #read joint names specified in file
185         joint_names = lines[0].rstrip().split(',')
186         #parse joint names for right limb
187         for name in joint_names:
188             if self.limb == name[:-3]:
189                 self.goal.trajectory.joint_names.append(name)

The lines are split into a list with ',' as the delimiter to extract the joint names. The arm of the current joint, found by stripping the "_<joint>" (last three characters) from the joint_name, is checked and stored.

191         def find_start_offset(pos):
192             #create empty lists
193             cur = []
194             cmd = []
195             dflt_vel = []
196             vel_param = self._param_ns + "%s_default_velocity"
197             #for all joints find our current and first commanded position
198             #reading default velocities from the parameter server if specified
199             for name in joint_names:
200                 if self.limb == name[:-3]:
201                     cmd.append(pos[name])
202                     cur.append(self.arm.joint_angle(name))
203                     prm = rospy.get_param(vel_param % name, 0.25)
204                     dflt_vel.append(prm)
205             diffs = map(operator.sub, cmd, cur)
206             diffs = map(operator.abs, diffs)
207             #determine the largest time offset necessary across all joints
208             offset = max(map(operator.div, diffs, dflt_vel))
209             return offset

The first commanded position and the current position for all the joints are captured. The default values that were loaded into the param server are read. The largest time offset necessary across all the joints is calculated.

211         for idx, values in enumerate(lines[1:]):
212             #clean each line of file
213             cmd, values = self._clean_line(values, joint_names)
214             #find allowable time offset for move to start position
215             if idx == 0:
216                 # Set the initial position to be the current pose.
217                 # This ensures we move slowly to the starting point of the
218                 # trajectory from the current pose - The user may have moved
219                 # arm since recording
220                 cur_cmd = [self.arm.joint_angle(jnt) for jnt in self.goal.trajectory.joint_names]
221                 self._add_point(cur_cmd, self.limb, 0.0)
222                 start_offset = find_start_offset(cmd)
223                 # Gripper playback won't start until the starting movement's
224                 # duration has passed, and the actual trajectory playback begins
225                 self._slow_move_offset = start_offset
226                 self._trajectory_start_offset = rospy.Duration(start_offset + values[0])
227             #add a point for this set of commands with recorded time
228             cur_cmd = [cmd[jnt] for jnt in self.goal.trajectory.joint_names]
229             self._add_point(cur_cmd, self.limb, values[0] + start_offset)
230             if self.has_gripper:
231                 cur_cmd = [cmd[self.gripper_name]]
232                 self._add_point(cur_cmd, self.gripper_name, values[0] + start_offset)

The non-float values are cleaned and a dictionary of valid Joint Commands is returned in the _clean_line() function as explained above. The time offset for move to start position is also calculated. The parsed set of recorded commands along with their recorded time is added to the goal list as well as gripper's command and time.

234     def _feedback(self, data):
235         # Test to see if the actual playback time has exceeded
236         # the move-to-start-pose timing offset
237         if (not self._get_trajectory_flag() and
238               data.actual.time_from_start >= self._trajectory_start_offset):
239             self._set_trajectory_flag(value=True)
240             self._trajectory_actual_offset = data.actual.time_from_start

Called in start function, test to see if the actual playback time has exceeded the move-to-start-pose timing offset.

242     def _set_trajectory_flag(self, value=False):
243         with self._lock:
244             # Assign a value to the flag
245             self._arm_trajectory_started = value
246 
247     def _get_trajectory_flag(self):
248         temp_flag = False
249         with self._lock:
250             # Copy to external variable
251             temp_flag = self._arm_trajectory_started
252         return temp_flag

Assign the value to the trajectory flag function and get the trajectory flag value function.

254     def start(self):
255         """
256         Sends FollowJointTrajectoryAction request
257         """
258         self._limb_client.send_goal(self.goal, feedback_cb=self._feedback)
259         # Syncronize playback by waiting for the trajectories to start
260         while not rospy.is_shutdown() and not self._get_trajectory_flag():
261             rospy.sleep(0.05)
262         if self.has_gripper:
263             self._execute_gripper_commands()

This function sends the FollowJointTrajectoryAction request message which includes the recorded positions and their corresponding time, to the Joint Trajectory Action Server.

265     def stop(self):
266         """
267         Preempts trajectory execution by sending cancel goals
268         """
269         if (self._limb_client.gh is not None and
270             self._limb_client.get_state() == actionlib.GoalStatus.ACTIVE):
271             self._limb_client.cancel_goal()
272 
273         #delay to allow for terminating handshake
274         rospy.sleep(0.1)

The Trajectory execution is stopped by sending cancel goals to the Joint trajectory action server.

276     def wait(self):
277         """
278         Waits for and verifies trajectory execution result
279         """
280         #create a timeout for our trajectory execution
281         #total time trajectory expected for trajectory execution plus a buffer
282         last_time = self.goal.trajectory.points[-1].time_from_start.to_sec()
283         time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
284         timeout = rospy.Duration(self._slow_move_offset +
285                                  last_time +
286                                  time_buffer)
287 
288         finish = self._limb_client.wait_for_result(timeout)
289         result = (self._limb_client.get_result().error_code == 0)
290 
291         #verify result
292         if all([finish, result]):
293             return True
294         else:
295             msg = ("Trajectory action failed or did not finish before "
296                    "timeout/interrupt.")
297             rospy.logwarn(msg)
298             return False

This method waits for the completion of the trajectory execution by Joint trajectory action server.

301 def main():
302     """RSDK Joint Trajectory Example: File Playback
303 
304     Plays back joint positions honoring timestamps recorded
305     via the joint_recorder example.
306 
307     Run the joint_recorder.py example first to create a recording
308     file for use with this example. Then make sure to start the
309     joint_trajectory_action_server before running this example.
310 
311     This example will use the joint trajectory action server
312     with velocity control to follow the positions and times of
313     the recorded motion, accurately replicating movement speed
314     necessary to hit each trajectory point on time.
315     """
316     epilog = """
317 Related examples:
318   joint_recorder.py; joint_position_file_playback.py.
319     """
320     arg_fmt = argparse.RawDescriptionHelpFormatter
321     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
322                                      description=main.__doc__,
323                                      epilog=epilog)
324     parser.add_argument(
325         '-f', '--file', metavar='PATH', required=True,
326         help='path to input file'
327     )
328     parser.add_argument(
329         '-l', '--loops', type=int, default=1,
330         help='number of playback loops. 0=infinite.'
331     )
332     # remove ROS args and filename (sys.arv[0]) for argparse
333     args = parser.parse_args(rospy.myargv()[1:])

The name source file to read the Joint Position is captured along with the number of times the trajectory has to be looped from the command line.

335     print("Initializing node... ")
336     rospy.init_node("sdk_joint_trajectory_file_playback")
337     print("Getting robot state... ")
338     rs = intera_interface.RobotEnable(CHECK_VERSION)
339     print("Enabling robot... ")
340     rs.enable()
341     print("Running. Ctrl-c to quit")
342 
343     traj = Trajectory()
344     traj.parse_file(path.expanduser(args.file))
345     #for safe interrupt handling
346     rospy.on_shutdown(traj.stop)
347     result = True
348     loop_cnt = 1
349     loopstr = str(args.loops)
350     if args.loops == 0:
351         args.loops = float('inf')
352         loopstr = "forever"
353     while (result == True and loop_cnt <= args.loops
354            and not rospy.is_shutdown()):
355         print("Playback loop %d of %s" % (loop_cnt, loopstr,))
356         traj.start()
357         result = traj.wait()
358         loop_cnt = loop_cnt + 1
359     print("Exiting - File Playback Complete")
360 
361 if __name__ == "__main__":
362     main()

The node is initialized. An instance of the Trajectory class is created. The parse_file() method is called to extract the recorded Joint Positions and their corresponding timestamps as explained above. The start() method is called to send the FollowJointTrajectory request message to the Joint Trajectory Action server. This loops for the user specified number of times.