Difference between pages "Joint Trajectory Playback Example" and "Running Examples Overview"

(Difference between pages)
Jump to: navigation , search
(Introduction format)
 
(SDK Examples fix)
 
Line 2: Line 2:
 
<div class="title-block">
 
<div class="title-block">
  
<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>
+
<span style="font-size:120%;">'''This section describes the SDK example programs. These example programs are provided as a starting point, demonstrating usage of the Python SDK packages for control of your robot.'''</span>
  
 
</div>
 
</div>
Line 8: Line 8:
 
<div class="content-block">
 
<div class="content-block">
  
== Overview ==
+
== Prerequisites ==
  
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.
+
* You have completed all of the [[Installation|Installation]] instructions for setting up your robot and development machine.
 +
* The development workstation has been connected to the robot and [[Hello Robot | connectivity has been validated]].
  
 
</div>
 
</div>
Line 16: Line 17:
 
<div class="content-block">
 
<div class="content-block">
  
== Introduction ==
+
== Running Examples with the SDK ==
  
This example demonstrates the usage of the <code>Joint Trajectory Action server</code> 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 - <code>robot/limb/right/follow_joint_trajectory</code>.
+
=== Initialize ===
<br />
 
  
</div>
+
[[SDK Shell | Initialize your environment]]:
 
 
<div class="content-block">
 
 
 
== Usage ==
 
 
 
Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], if the robot is not enabled run:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_interface robot_enable.py
 
</syntaxhighlight>
 
 
 
Record a joint position file using the recorder.py example, ex:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_examples recorder.py -f <position_file_name>
 
</syntaxhighlight>
 
 
 
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:
 
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_interface joint_trajectory_action_server.py --mode velocity
+
$ cd ~/ros_ws
 +
$ ./intera.sh
 
</syntaxhighlight>
 
</syntaxhighlight>
  
In another RSDK terminal session, Run the joint trajectory playback example program, ex:
+
=== Run a program! ===
 
 
<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 class="content-block">
 
 
 
== 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:
 
 
 
<syntaxhighlight lang="bash" enclose="div">
 
$ rosrun intera_interface joint_trajectory_action_server.py -h
 
</syntaxhighlight>
 
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
Intera SDK Joint Trajectory Controller
+
$ rosrun intera_examples <example_program> [arguments]
 
 
    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>
 
</syntaxhighlight>
  
'''Important Arguments:'''
+
Refer to the individual examples below for a complete list of arguments or run it as
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">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_interface joint_trajectory_file_playback.py -h
+
$ rosrun intera_examples <example_program> -h
 
</syntaxhighlight>
 
</syntaxhighlight>
  
<syntaxhighlight lang="bash" enclose="div">
+
to find the list of arguments.
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)
 
</syntaxhighlight>
 
  
 
</div>
 
</div>
Line 124: Line 46:
 
<div class="content-block">
 
<div class="content-block">
  
== Interfaces ==
+
== Clone the SDK Examples ==
 
 
'''ROS APIs:'''
 
  
See the API Reference page for details.
+
The <code>'intera_examples'</code> locate under intera_sdk repo, put <code>'intera_sdk'</code> into your ROS development workspace (<code> ~/ros_ws/src </code>). This will allow you to dig into the examples, follow along with the code walkthroughs and base your custom code off of these examples.
<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">
 
<syntaxhighlight lang="bash" enclose="div">
JointTrajectoryActionServer class: joint_trajectory_action_server.py
+
$ cd ~/ros_ws/src
 +
$ git clone https://github.com/RethinkRobotics/intera_sdk.git
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Line 142: Line 59:
 
<div class="content-block">
 
<div class="content-block">
  
== Code Walkthrough ==
+
== SDK Examples ==
  
Now, let's break down the code.
+
=== Movement ===
  
<syntaxhighlight lang="python" line start="33" enclose="div">
+
[[Joint Position Example]] - Joystick, keyboard and file record/playback examples using joint position control of Sawyer's arm.
import argparse
 
import operator
 
import sys
 
import threading
 
  
from bisect import bisect
+
[[Joint Torque Springs Example]] - Joint torque control example applying virtual spring torques.
from copy import copy
 
from os import path
 
  
import rospy
+
[[IK Service Example|Inverse Kinematics Service Example]] - Basic use of Inverse Kinematics solver service.
  
import actionlib
+
[[Simple Joint trajectory example|Simple Joint Trajectory Example]] - Simple demo using the joint trajectory interface.
  
from control_msgs.msg import (
+
[[Joint Trajectory Playback Example]] - Trajectory playback using the joint trajectory interface.
    FollowJointTrajectoryAction,
 
    FollowJointTrajectoryGoal,
 
)
 
from trajectory_msgs.msg import (
 
    JointTrajectoryPoint,
 
)
 
  
import intera_interface
+
[[Head Movement Example]] - Simple demo moving and nodding the head.
  
from intera_interface import CHECK_VERSION
+
[[Head Action Client Example|Head Action Client Example]] - A demo to showcase the functionality of the head trajectory action server.
</syntaxhighlight>
 
  
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.
+
[[Gripper Example]] - Joystick and Keyboard control for the grippers.
  
<syntaxhighlight lang="python" line start="59" enclose="div">
+
[[Gripper Cuff Control Example]] - Simple cuff-interaction control with Zero-G mode.
class Trajectory(object):
 
    def __init__(self, limb="right"):
 
        #create our action server clients
 
        self._limb_client = actionlib.SimpleActionClient(
 
            'robot/limb/right/follow_joint_trajectory',
 
            FollowJointTrajectoryAction,
 
        )
 
  
        #verify joint trajectory action servers are available
+
=== Robot Configuration ===
        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>
 
  
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.
+
[[URDF Configuration Example]] - A simple ROS node that shows how to add segment and joint subtrees to the robot's model.
  
<syntaxhighlight lang="python" line start="76" enclose="div">
+
=== Input and Output ===
        #create our goal request
 
        self.goal = FollowJointTrajectoryGoal()
 
  
        #limb interface - current angles needed for start move
+
[[Display Image Example]] - Example tool for displaying image files (png, jpeg) on the Head Screen.
        self.arm = intera_interface.Limb(limb)
 
  
        self.limb = limb
+
[[image_view|View Cameras Example]] - Simple tool for viewing camera feed on development machine.
        self.gripper_name = '_'.join([limb, 'gripper'])
 
        #gripper interface - for gripper command playback
 
        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>
 
 
 
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="97" enclose="div">
 
        # Verify Grippers Have No Errors and are Calibrated
 
        if self.has_gripper:
 
            if self.gripper.has_error():
 
                self.gripper.reboot()
 
            if not self.gripper.is_calibrated():
 
                self.gripper.calibrate()
 
 
 
            #gripper goal trajectories
 
            self.grip = FollowJointTrajectoryGoal()
 
 
 
            #gripper control rate
 
            self._gripper_rate = 20.0  # Hz
 
 
 
        # 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>
 
 
 
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="118" enclose="div">
 
    def _execute_gripper_commands(self):
 
        start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
 
        grip_cmd = self.grip.trajectory.points
 
        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>
 
 
 
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="133" enclose="div">
 
    def _clean_line(self, line, joint_names):
 
        """
 
        Cleans a single line of recorded joint positions
 
 
 
        @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
 
        """
 
        def try_float(x):
 
            try:
 
                return float(x)
 
            except ValueError:
 
                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>
 
 
 
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="158" enclose="div">
 
    def _add_point(self, positions, side, time):
 
        """
 
        Appends trajectory with new point
 
 
 
        @param positions: joint positions
 
        @param side: limb to command point
 
        @param time: time from start for point in seconds
 
        """
 
        #creates a point in trajectory with time_from_start and positions
 
        point = JointTrajectoryPoint()
 
        point.positions = copy(positions)
 
        point.time_from_start = rospy.Duration(time)
 
        if side == self.limb:
 
            self.goal.trajectory.points.append(point)
 
        elif self.has_gripper and side == self.gripper_name:
 
            self.grip.trajectory.points.append(point)
 
</syntaxhighlight>
 
 
 
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="175" enclose="div">
 
    def parse_file(self, filename):
 
        """
 
        Parses input file into FollowJointTrajectoryGoal format
 
 
 
        @param filename: input filename
 
        """
 
        #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>
 
 
 
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>
 
 
 
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.
 
 
 
<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):
 
        """
 
        Sends FollowJointTrajectoryAction request
 
        """
 
        self._limb_client.send_goal(self.goal, feedback_cb=self._feedback)
 
        # 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>
 
 
 
This function sends the FollowJointTrajectoryAction request message which includes the recorded positions and their corresponding time, to the Joint Trajectory Action Server.
 
 
 
<syntaxhighlight lang="python" line start="265" enclose="div">
 
    def stop(self):
 
        """
 
        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()
 
 
 
        #delay to allow for terminating handshake
 
        rospy.sleep(0.1)
 
</syntaxhighlight>
 
 
 
The Trajectory execution is stopped by sending cancel goals to the Joint trajectory action server.
 
 
 
<syntaxhighlight lang="python" line start="276" enclose="div">
 
    def wait(self):
 
        """
 
        Waits for and verifies trajectory execution result
 
        """
 
        #create a timeout for our trajectory execution
 
        #total time trajectory expected for trajectory execution plus a buffer
 
        last_time = self.goal.trajectory.points[-1].time_from_start.to_sec()
 
        time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5
 
        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>
 
 
 
This method waits for the completion of the trajectory execution by Joint trajectory action server.
 
 
 
<syntaxhighlight lang="python" line start="301" enclose="div">
 
def main():
 
    """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.
 
    """
 
    epilog = """
 
Related examples:
 
  joint_recorder.py; joint_position_file_playback.py.
 
    """
 
    arg_fmt = argparse.RawDescriptionHelpFormatter
 
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
 
                                    description=main.__doc__,
 
                                    epilog=epilog)
 
    parser.add_argument(
 
        '-f', '--file', metavar='PATH', required=True,
 
        help='path to input file'
 
    )
 
    parser.add_argument(
 
        '-l', '--loops', type=int, default=1,
 
        help='number of playback loops. 0=infinite.'
 
    )
 
    # remove ROS args and filename (sys.arv[0]) for argparse
 
    args = parser.parse_args(rospy.myargv()[1:])
 
</syntaxhighlight>
 
 
 
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.
 
 
 
<syntaxhighlight lang="python" line start="335" enclose="div">
 
    print("Initializing node... ")
 
    rospy.init_node("sdk_joint_trajectory_file_playback")
 
    print("Getting robot state... ")
 
    rs = intera_interface.RobotEnable(CHECK_VERSION)
 
    print("Enabling robot... ")
 
    rs.enable()
 
    print("Running. Ctrl-c to quit")
 
 
 
    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__":
 
    main()
 
</syntaxhighlight>
 
  
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.
+
[[Input Output Example|I/O Example]] - Flash the lights on the digital outputs.
  
 
</div>
 
</div>

Revision as of 10:59, 25 November 2016

This section describes the SDK example programs. These example programs are provided as a starting point, demonstrating usage of the Python SDK packages for control of your robot.

Prerequisites

Running Examples with the SDK

Initialize

Initialize your environment:

$ cd ~/ros_ws
$ ./intera.sh

Run a program!

$ rosrun intera_examples <example_program> [arguments]

Refer to the individual examples below for a complete list of arguments or run it as

$ rosrun intera_examples <example_program> -h

to find the list of arguments.

Clone the SDK Examples

The 'intera_examples' locate under intera_sdk repo, put 'intera_sdk' into your ROS development workspace ( ~/ros_ws/src ). This will allow you to dig into the examples, follow along with the code walkthroughs and base your custom code off of these examples.

$ cd ~/ros_ws/src
$ git clone https://github.com/RethinkRobotics/intera_sdk.git

SDK Examples

Movement

Joint Position Example - Joystick, keyboard and file record/playback examples using joint position control of Sawyer's arm.

Joint Torque Springs Example - Joint torque control example applying virtual spring torques.

Inverse Kinematics Service Example - Basic use of Inverse Kinematics solver service.

Simple Joint Trajectory Example - Simple demo using the joint trajectory interface.

Joint Trajectory Playback Example - Trajectory playback using the joint trajectory interface.

Head Movement Example - Simple demo moving and nodding the head.

Head Action Client Example - A demo to showcase the functionality of the head trajectory action server.

Gripper Example - Joystick and Keyboard control for the grippers.

Gripper Cuff Control Example - Simple cuff-interaction control with Zero-G mode.

Robot Configuration

URDF Configuration Example - A simple ROS node that shows how to add segment and joint subtrees to the robot's model.

Input and Output

Display Image Example - Example tool for displaying image files (png, jpeg) on the Head Screen.

View Cameras Example - Simple tool for viewing camera feed on development machine.

I/O Example - Flash the lights on the digital outputs.