Joint Position Waypoint Example

Jump to: navigation , search

The example demonstrates basic joint position control. A joint position waypoint is a configuration of the arm in joint space (ie. simultaneous joint angles for the arm's seven degrees of freedom). In this example, we enable the robot moving the limb in zero-g mode. Interacting with the arm's navigator buttons, we can record a sequence of joint position waypoints. Upon completion of recording, we will continuously command the limb loop through the recorded joint sequence.


This example demonstrates the usage of the position controller for simple joint position moves. The main() function calls the record() method which records the waypoints as desired by the user. Then, the playback() method is invoked which demonstrates a smooth motion playback through the recorded joint positions.
Interfaces -

  • Limb.move_to_joint_positions()
  • Limb.set_joint_position_speed()
  • Limb.joint_angles()
  • Navigator.wheel()
  • Navigator.button_state()

If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link for joint position waypoints example.


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

$ rosrun intera_interface

Start the joint position waypoints example program, you can specify the joint position motion speed ratio -s or joint position accuracy(rad) -a on which we will run the example.

For example, run the example with 0.1 speed ratio and 0.1 accuracy:

$ rosrun intera_examples -s 0.1 -a 0.1

Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that navigator wheel.

You will then be prompted with instructions for recording joint position waypoints.

Press Navigator 'OK/Wheel' button to record a new joint joint position waypoint.
Press Navigator 'Rethink' button when finished recording waypoints to begin playback.

Moving the arm in zero-g mode, when we reach a joint position configuration we would like to record, press that navigator wheel.

You will get the feedback that the joint position waypoint has been recorded:

Waypoint Recorded

When done recording waypoints, pressing the navigator's 'Rethink' button will begin playback.

[INFO] [WallTime: 1399571721.540300] Waypoint Playback Started
  Press Ctrl-C to stop...

The program will begin looping through the recorded joint positions. When the previous joint position command is fully achieved (within the accuracy threshold), the next recorded joint position will be commanded.

Waypoint playback loop #1
Waypoint playback loop #2

Pressing Control-C at any time will stop playback and exit the example.


Important Arguments:

-s or --speed : The joint position motion speed ratio [0.0-1.0] (default:= 0.3)

-a or --accuracy : The threshold in Radians at which the current joint position command is considered successful before sending the next following joint position command. (default:= 0.008726646)

See the joint position waypoint's available arguments on the command line by passing the -h, help argument:

$ rosrun intera_examples -h
usage: [-h] [-s SPEED] [-a ACCURACY]

RSDK Joint Position Waypoints Example

    Records joint positions each time the navigator 'OK/wheel'
    button is pressed.
    Upon pressing the navigator 'Rethink' button, the recorded joint positions
    will begin playing back in a loop.

optional arguments:
  -h, --help            show this help message and exit
  -s SPEED, --speed SPEED
                        joint position motion speed ratio [0.0-1.0] (default:=
  -a ACCURACY, --accuracy ACCURACY
                        joint position accuracy (rad) at which waypoints must

Code Walkthrough

Now, let's break down the code.

  1. import argparse
  2. import sys
  4. import rospy
  6. import intera_interface

This imports the intera interface for accessing the limb and the gripper class.

  1. class Waypoints(object):
  2.     def __init__(self, speed, accuracy, limb="right"):
  3.         # Create intera_interface limb instance
  4.         self._arm = limb
  5.         self._limb = intera_interface.Limb(self._arm)
  7.         # Parameters which will describe joint position moves
  8.         self._speed = speed
  9.         self._accuracy = accuracy
  11.         # Recorded waypoints
  12.         self._waypoints = list()
  14.         # Recording state
  15.         self._is_recording = False
  17.         # Verify robot is enabled
  18.         print("Getting robot state... ")
  19.         self._rs = intera_interface.RobotEnable()
  20.         self._init_state = self._rs.state().enabled
  21.         print("Enabling robot... ")
  22.         self._rs.enable()
  24.         # Create Navigator I/O
  25.         self._navigator_io = intera_interface.Navigator(self._arm)

An instance of the limb interface for the side under interest is created. An instance of the interface for the corresponding navigator, _navigator_io, is also created. The robot is then enabled. It is important to note that the robot should be enabled in order to control the limb's movement.

  1.     def _record_waypoint(self, value):
  2.         """
  3.        Stores joint position waypoints
  5.        Navigator 'OK/Wheel' button callback
  6.        """
  7.         if value != 'OFF':
  8.             print("Waypoint Recorded")
  9.             self._waypoints.append(self._limb.joint_angles())

The joint_angles() method retrieves the current joint angles and they are then appended to the _waypoints list.

  1.     def _stop_recording(self, value):
  2.         """
  3.        Sets is_recording to false
  5.        Navigator 'Rethink' button callback
  6.        """
  7.         # On navigator Rethink button press, stop recording
  8.         if value != 'OFF':
  9.             print("Recording Stopped")
  10.             self._is_recording = False

This Rethink button's press event is captured.

  1.     def record(self):
  2.         """
  3.        Records joint position waypoints upon each Navigator 'OK/Wheel' button
  4.        press.
  5.        """
  6.         rospy.loginfo("Waypoint Recording Started")
  7.         print("Press Navigator 'OK/Wheel' button to record a new joint "
  8.         "joint position waypoint.")
  9.         print("Press Navigator 'Rethink' button when finished recording "
  10.               "waypoints to begin playback")
  11.         # Connect Navigator callbacks
  12.         # Navigator scroll wheel button press
  13.         ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok')
  14.         # Navigator Rethink button press
  15.         show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show')
  17.         # Set recording flag
  18.         self._is_recording = True
  20.         # Loop until waypoints are done being recorded ('Rethink' Button Press)
  21.         while not rospy.is_shutdown() and self._is_recording:
  22.             rospy.sleep(1.0)
  24.         # We are now done with the navigator callbacks, disconnecting them
  25.         self._navigator.deregister_callback(ok_id)
  26.         self._navigator.deregister_callback(show_id)

The navigator 'OK/Wheel' button invokes the _record_waypoint() method on a press event, which records the current joint positions as explained above. Similarly, the navigator 'Rethink' button invokes the _stop_recording method on a press event, which sets the is_recording variable to false.

  1.     def playback(self):
  2.         """
  3.        Loops playback of recorded joint position waypoints until program is
  4.        exited
  5.        """
  6.         rospy.sleep(1.0)
  8.         rospy.loginfo("Waypoint Playback Started")
  9.         print("  Press Ctrl-C to stop...")
  11.         # Set joint position speed ratio for execution
  12.         self._limb.set_joint_position_speed(self._speed)
  14.         # Loop until program is exited
  15.         loop = 0
  16.         while not rospy.is_shutdown():
  17.             loop += 1
  18.             print("Waypoint playback loop #%d " % (loop,))
  19.             for waypoint in self._waypoints:
  20.                 if rospy.is_shutdown():
  21.                     break
  22.                 self._limb.move_to_joint_positions(waypoint, timeout=20.0,
  23.                                                    threshold=self._accuracy)
  24.             # Sleep for a few seconds between playback loops
  25.             rospy.sleep(3.0)
  27.         # Set joint position speed back to default
  28.         self._limb.set_joint_position_speed(0.3)

The set_joint_position_speed() method sets the ratio of max joint speed to use during joint position moves. The method move_to_joint_positions(), moves the joints to the commanded position. It is important to note that there is not trajectory planning involved here. Instead, this is passed onto a low pass filter and the intermediate positions between the start and goal positions are obtained. They are then published as a joint command message using the set_joint_positions() method. Thus, all the waypoints that were stored are visited along a smooth trajectory.

  1.     def clean_shutdown(self):
  2.         print("\nExiting example...")
  3.         if not self._init_state:
  4.             print("Disabling robot...")
  5.             self._rs.disable()
  6.         return True

On shutdown, the robot is sent to its initial state that was captured.

  1. def main():
  2.     """RSDK Joint Position Waypoints Example
  4.    Records joint positions each time the navigator 'OK/wheel'
  5.    button is pressed.
  6.    Upon pressing the navigator 'Rethink' button, the recorded joint positions
  7.    will begin playing back in a loop.
  8.    """
  9.     arg_fmt = argparse.RawDescriptionHelpFormatter
  10.     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
  11.                                      description=main.__doc__)
  12.     parser.add_argument(
  13.         '-s', '--speed', default=0.3, type=float,
  14.         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
  15.     )
  16.     parser.add_argument(
  17.         '-a', '--accuracy',
  18.         default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
  19.         help='joint position accuracy (rad) at which waypoints must achieve'
  20.     )
  21.     args = parser.parse_args(rospy.myargv()[1:])

The speed and accuracy on which the example is to be demonstrated is captured from the command line arguments.

  1.     print("Initializing node... ")
  2.     rospy.init_node("rsdk_joint_position_waypoints")
  4.     waypoints = Waypoints(args.speed, args.accuracy)
  6.     # Register clean shutdown
  7.     rospy.on_shutdown(waypoints.clean_shutdown)
  9.     # Begin example program
  10.     waypoints.record()
  11.     waypoints.playback()
  13. if __name__ == '__main__':
  14.     main()

The node is initialized and an instance of the Waypoints class is created. The user defined waypoints are recorded using the record() method and are played back using the playback() method as explained above.