Joint Position Example

Revision as of 13:12, 26 November 2016 by Swang (talk | contribs) (Edit Joint Postition Ex)
Jump to: navigation , search

The joint position example(keyboard, joystick) demonstrates basic joint position control. The key or button mapped to either increasing or decreasing the angle of a particular joint on Sawyer's arm.


Start the joint position keyboard example program, ex:

$ rosrun intera_examples

Start the joint position joystick example program, ex:

$ rosrun intera_examples

Upon startup, you will be prompted with the following:

Initializing node... 
Getting robot state... 
Enabling robot... 
[INFO] [WallTime: 1399575163.891211] Robot Enabled
Controlling joints. Press ? for help, Esc to quit.


Important Arguments:

See the joint position example available arguments on the command line by passing:

$ rosrun intera_examples -h
usage: [-h] [-l LIMB]

RSDK Joint Position Example: Keyboard Control
    Use your dev machine's keyboard to control joint positions.
    Each key corresponds to increasing or decreasing the angle
    of a joint on Sawyer's arm. The increasing and descreasing
    are represented by number key and letter key next to the number.

optional arguments:
  -h, --help            show this help message and exit
  -l LIMB, --limb LIMB
                        Limb on which to run the joint position keyboard example

For joint position joystick example:

$ rosrun intera_examples -h
usage: [-h] [-l LIMB] [-j JOYSTICK]

SDK Joint Position Example: Joystick Control
    Use a game controller to control the angular joint positions
    of Sawyer's arms.
    Attach a game controller to your dev machine and run this
    example along with the ROS joy_node to control the position
    of each joint in Sawyer's arm using the joystick. Be sure to
    provide your *joystick* type to setup appropriate key mappings.
    Each stick axis maps to a joint angle; which joints are currently
    controlled can be incremented by using the mapped increment buttons.
      (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)

required arguments:
  -j, --joystick        specify the type of joystick to use

optional arguments:
  -h, --help            show this help message and exit
  -l LIMB, --limb LIMB
                        Limb on which to run the joint position joystick example

Code Walkthrough

Now, let's break down the code.

33 import argparse
34 import sys
36 import rospy
38 import intera_interface

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

41 class Waypoints(object):
42     def __init__(self, speed, accuracy, limb="right"):
43         # Create intera_interface limb instance
44         self._arm = limb
45         self._limb = intera_interface.Limb(self._arm)
47         # Parameters which will describe joint position moves
48         self._speed = speed
49         self._accuracy = accuracy
51         # Recorded waypoints
52         self._waypoints = list()
54         # Recording state
55         self._is_recording = False
57         # Verify robot is enabled
58         print("Getting robot state... ")
59         self._rs = intera_interface.RobotEnable()
60         self._init_state = self._rs.state().enabled
61         print("Enabling robot... ")
62         self._rs.enable()
64         # Create Navigator I/O
65         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 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.

67     def _record_waypoint(self, value):
68         """
69         Stores joint position waypoints
71         Navigator 'OK/Wheel' button callback
72         """
73         if value != 'OFF':
74             print("Waypoint Recorded")
75             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.

77     def _stop_recording(self, value):
78         """
79         Sets is_recording to false
81         Navigator 'Rethink' button callback
82         """
83         # On navigator Rethink button press, stop recording
84         if value != 'OFF':
85             print("Recording Stopped")
86             self._is_recording = False

This Rethink button's press event is captured.

 88     def record(self):
 89         """
 90         Records joint position waypoints upon each Navigator 'OK/Wheel' button
 91         press.
 92         """
 93         rospy.loginfo("Waypoint Recording Started")
 94         print("Press Navigator 'OK/Wheel' button to record a new joint "
 95         "joint position waypoint.")
 96         print("Press Navigator 'Rethink' button when finished recording "
 97               "waypoints to begin playback")
 98         # Connect Navigator callbacks
 99         # Navigator scroll wheel button press
100         ok_id = self._navigator.register_callback(self._record_waypoint, 'right_button_ok')
101         # Navigator Rethink button press
102         show_id = self._navigator.register_callback(self._stop_recording, 'right_button_show')
104         # Set recording flag
105         self._is_recording = True
107         # Loop until waypoints are done being recorded ('Rethink' Button Press)
108         while not rospy.is_shutdown() and self._is_recording:
109             rospy.sleep(1.0)
111         # We are now done with the navigator callbacks, disconnecting them
112         self._navigator.deregister_callback(ok_id)
113         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.

115     def playback(self):
116         """
117         Loops playback of recorded joint position waypoints until program is
118         exited
119         """
120         rospy.sleep(1.0)
122         rospy.loginfo("Waypoint Playback Started")
123         print("  Press Ctrl-C to stop...")
125         # Set joint position speed ratio for execution
126         self._limb.set_joint_position_speed(self._speed)
128         # Loop until program is exited
129         loop = 0
130         while not rospy.is_shutdown():
131             loop += 1
132             print("Waypoint playback loop #%d " % (loop,))
133             for waypoint in self._waypoints:
134                 if rospy.is_shutdown():
135                     break
136                 self._limb.move_to_joint_positions(waypoint, timeout=20.0,
137                                                    threshold=self._accuracy)
138             # Sleep for a few seconds between playback loops
139             rospy.sleep(3.0)
141         # Set joint position speed back to default
142         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.

144     def clean_shutdown(self):
145         print("\nExiting example...")
146         if not self._init_state:
147             print("Disabling robot...")
148             self._rs.disable()
149         return True

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

152 def main():
153     """RSDK Joint Position Waypoints Example
155     Records joint positions each time the navigator 'OK/wheel'
156     button is pressed.
157     Upon pressing the navigator 'Rethink' button, the recorded joint positions
158     will begin playing back in a loop.
159     """
160     arg_fmt = argparse.RawDescriptionHelpFormatter
161     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
162                                      description=main.__doc__)
163     parser.add_argument(
164         '-s', '--speed', default=0.3, type=float,
165         help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
166     )
167     parser.add_argument(
168         '-a', '--accuracy',
169         default=intera_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
170         help='joint position accuracy (rad) at which waypoints must achieve'
171     )
172     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.

178     print("Initializing node... ")
179     rospy.init_node("rsdk_joint_position_waypoints")
181     waypoints = Waypoints(args.speed, args.accuracy)
183     # Register clean shutdown
184     rospy.on_shutdown(waypoints.clean_shutdown)
186     # Begin example program
187     waypoints.record()
188     waypoints.playback()
190 if __name__ == '__main__':
191     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.