Difference between revisions of "Gripper Example"

Jump to: navigation , search
(Gripper Joystick Example Code Walkthrough)
 
(6 intermediate revisions by the same user not shown)
Line 4: Line 4:
  
 
</div>
 
</div>
 +
 +
{{TOClimit|limit=2}}
  
 
<div class="content-block">
 
<div class="content-block">
Line 22: Line 24:
 
$ rosrun intera_interface robot_enable.py
 
$ rosrun intera_interface robot_enable.py
 
</syntaxhighlight>
 
</syntaxhighlight>
 
  
 
Start gripper control from an RSDK terminal session:
 
Start gripper control from an RSDK terminal session:
Line 109: Line 110:
 
Where <code>joystick_type</code> is 'xbox', 'logitech', or 'ps3'. ''(If using a ps3, make sure you run the node from the ROS ps3joy package in a separate sudo terminal.  See instructions here: [http://www.ros.org/wiki/ps3joy/Tutorials/PairingJoystickAndBluetoothDongle ps3joy] )''
 
Where <code>joystick_type</code> is 'xbox', 'logitech', or 'ps3'. ''(If using a ps3, make sure you run the node from the ROS ps3joy package in a separate sudo terminal.  See instructions here: [http://www.ros.org/wiki/ps3joy/Tutorials/PairingJoystickAndBluetoothDongle ps3joy] )''
  
 +
'''NOTE:''' This method uses an included ROS launch file to start both the gripper example and joy_node using the roslaunch tool. You can exit the gripper example by hitting any keyboard key, however you will have to <code>ctrl-c</code> to also cleanup the joy_node.
 +
 +
'''NOTE:''' Don't forget to calibrate gripper first.
  
'''Note:''' This method uses an included ROS launch file to start both the gripper example and joy_node using the roslaunch tool. You can exit the gripper example by hitting any keyboard key, however you will have to <code>ctrl-c</code> to also cleanup the joy_node.
 
Joystick Button Mappings
 
'''Note:''' Don't forget to calibrate gripper first.
 
 
'''NOTE:''' The 'joystick left <-> robot right' mappings are not typos; they assume the user is in front of the robot when using the joystick.
 
'''NOTE:''' The 'joystick left <-> robot right' mappings are not typos; they assume the user is in front of the robot when using the joystick.
Press any keyboard key to quit.
 
  
 
[[File:Joystick.jpg|700px]]
 
[[File:Joystick.jpg|700px]]
Line 167: Line 167:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the <code>intera_interface</code> for accessing the limb and the gripper class. The <code>intera_external_devices</code> is imported to use its <code>getch</code> function that captures the key presses on the joystick.
+
This imports the <code>intera_interface</code> for accessing the <code>limb</code> and the <code>gripper</code> class. The <code>intera_external_devices</code> is imported to use its <code>getch</code> function that captures the key presses on the joystick.
  
 
<syntaxhighlight lang="python" line start="41" enclose="div">
 
<syntaxhighlight lang="python" line start="41" enclose="div">
Line 186: Line 186:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>init_state</code> variable captures the current state of the robot. The Gripper instance class is created, if the gripper is not attached, a logerr message will show up.
+
The <code>init_state</code> variable captures the current state of the robot. The gripper instance class, <code>gripper</code>, is created, if the gripper is not attached, a logerr message will show up.
  
 
<syntaxhighlight lang="python" line start="56" enclose="div">
 
<syntaxhighlight lang="python" line start="56" enclose="div">
Line 278: Line 278:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The while loop iterates till the "esc" or "ctrl-c" is inputted or ros-shutdown signal is given. If "esc" or "ctrl-c" is given then shutdown signal is sent.
+
The while loop iterates till the <code>Esc</code> or <code>ctrl-c</code> is inputted or ros-shutdown signal is given. If <code>Esc</code> or <code>ctrl-c</code> is given then shutdown signal is sent.
  
 
<syntaxhighlight lang="python" line start="125" enclose="div">
 
<syntaxhighlight lang="python" line start="125" enclose="div">
Line 353: Line 353:
 
== Gripper Keyboard Example Code Walkthrough ==  
 
== Gripper Keyboard Example Code Walkthrough ==  
  
This example demonstrates the usage of the gripper control via intera interface. The main() function invokes the map_keyboard() function. It is at this function where the keyboard keys are mapped to individual gripper actions and the commands are executed periodically.
+
This example demonstrates the usage of the gripper control via intera interface. The <code>main()</code> function invokes the <code>map_keyboard()</code> function. It is at this function where the keyboard keys are mapped to individual gripper actions and the commands are executed periodically.
  
 
Now, let's break down the code.
 
Now, let's break down the code.
Line 382: Line 382:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The init_state variable captures the current state of the robot. The Gripper instance class is created. If no gripper detect, the logerr message will show up.
+
The <code>init_state</code> variable captures the current state of the robot. The <code>Gripper</code> instance class is created. If no gripper detect, the logerr message will show up.
  
 
<syntaxhighlight lang="python" line start="51" enclose="div">
 
<syntaxhighlight lang="python" line start="51" enclose="div">
Line 453: Line 453:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The while loop iterates till the "esc" or "ctrl-c" is inputted or ros-shutdown signal is given. If "esc" or "ctrl-c" is given then shutdown signal is sent.
+
The while loop iterates till the <code>Esc</code> or <code>Ctrl-c</code> is inputted or ros-shutdown signal is given. If <code>Esc</code> or <code>Ctrl-c</code> is given then shutdown signal is sent.
  
 
<syntaxhighlight lang="python" line start="106" enclose="div">
 
<syntaxhighlight lang="python" line start="106" enclose="div">

Latest revision as of 20:21, 16 January 2017

Use gripper control as an example of controlling Sawyer's gripper.

Overview

Uses the keyboard or joystick to control Sawyer's gripper. Position, velocity, holding, and moving force can be controlled and sensed. Both logitech and xbox game controllers are supported. If you would like to follow along with the actual source code for these examples on GitHub, it can be found through this link for gripper_joystick and this link for gripper_keyboard.

Usage

The robot should always enabled after start, try the command from an SDK terminal session if the robot is not enabled:

$ rosrun intera_interface robot_enable.py

Start gripper control from an RSDK terminal session:

Gripper keyboard Example:

$ rosrun intera_examples gripper_keyboard.py

Gripper Joystick Example:

$ rosrun intera_examples gripper_joystick.py

IMPORTANT: You will have to calibrate gripper before using any of the other commands using C/c commands:

Once calibrated, future calibrate commands will not do anything unless you send a 'reboot' first.

Key Bindings

Get a list of commands by entering '?'

For gripper keyboard example:

key: description,
'r': "reboot",
'c': "calibrate",
'q': "close",
'o': "open",
'+': "set 100% velocity",
'-': "set 30% velocity",
's': "stop",
'h': "decrease holding force",
'j': "increase holding force",
'u': "decrease position",
'i': "increase position",


For gripper joystick example:

command: description,
'btnLeft'      : "reboot",
'btnUp'        : "calibrate",
'leftTrigger'  : "close",
'leftTrigger'  : "open (release)",
'leftBumper'   : "stop",
'leftStickHorz': "decrease position by step when stick value less than 0, increase position by step when stick value larger than 0",
'leftStickVert': "decrease holding force by step when stick value less than 0, increase holding force by step when stick value larger than 0",
'function1' or 'function2': "help"


You can monitor the changes you are making using the following rostopic which you can monitor from a different shell:

$ rostopic echo /robot/end_effector/right_gripper/command

Joystick Control

To use the example gripper program using a joystick game controller to control the gripper:

First ensure that joy drivers are installed.

$ rospack find joy

If not run:

$ sudo apt-get install ros-indigo-joystick-drivers

To run the example:

$ roslaunch intera_examples gripper_joystick.launch joystick:=<joystick_type>

Where joystick_type is 'xbox', 'logitech', or 'ps3'. (If using a ps3, make sure you run the node from the ROS ps3joy package in a separate sudo terminal. See instructions here: ps3joy )

NOTE: This method uses an included ROS launch file to start both the gripper example and joy_node using the roslaunch tool. You can exit the gripper example by hitting any keyboard key, however you will have to ctrl-c to also cleanup the joy_node.

NOTE: Don't forget to calibrate gripper first.

NOTE: The 'joystick left <-> robot right' mappings are not typos; they assume the user is in front of the robot when using the joystick.

Joystick.jpg

Buttons Action
Function 1 or 2 (e.g. Select/Select) Help
Left Button (X) right: gripper calibrate
Top Button (Y)
Left Trigger [PRESS] right: gripper close
Left Trigger [RELEASE] right: gripper open
Left Bumper right: cycle <current joints> +1
<Any Keyboard key> Quit
Stick Axes Action
Left Stick Horizontal right: increase/decrease <current joint 1> (j0)
Left Stick Vertical right: increase/decrease <current joint 2> (j1)

Gripper Joystick Example Code Walkthrough

This example demonstrates the usage of the gripper control via intera interface. The main() function invokes the map_joystick() function. It is at this function where the joystick buttons are mapped to individual gripper actions and the commands are executed periodically.

Now, let's break down the code.

33 import argparse
34 
35 import rospy
36 
37 import intera_interface
38 import intera_external_devices

This imports the intera_interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its getch function that captures the key presses on the joystick.

41 def map_joystick(joystick, limb):
42     """
43     maps joystick input to gripper commands
44 
45     @param joystick: an instance of a Joystick
46     """
47     print("Getting robot state... ")
48     rs = intera_interface.RobotEnable(CHECK_VERSION)
49     init_state = rs.state()
50     try:
51         gripper = intera_interface.Gripper(limb)
52     except ValueError:
53         rospy.logerr("Could not detect a gripper attached to the robot.")
54         return

The init_state variable captures the current state of the robot. The gripper instance class, gripper, is created, if the gripper is not attached, a logerr message will show up.

56     def clean_shutdown():
57         print("\nExiting example...")
58         if not init_state:
59             print("Disabling robot...")
60             rs.disable()
61     rospy.on_shutdown(clean_shutdown)

On shutdown request, Sawyer's state is sent back to its initial state.

63     # decrease position dead_zone
64     gripper.set_dead_zone(2.5)
65 
66     # abbreviations
67     jhi = lambda s: joystick.stick_value(s) > 0
68     jlo = lambda s: joystick.stick_value(s) < 0
69     bdn = joystick.button_down
70     bup = joystick.button_up

Setup the gripper dead zone and joystick abbreviations.

72     def print_help(bindings_list):
73         print("Press Ctrl-C to quit.")
74         for bindings in bindings_list:
75             for (test, _cmd, doc) in bindings:
76                 if callable(doc):
77                     doc = doc()
78                 print("%s: %s" % (str(test[1]), doc))

The print_help function print helpful commands and docs.

80     def offset_position(offset):
81         current = gripper.get_position()
82         gripper.set_position(current + offset)
83 
84     def offset_holding(offset):
85         current = gripper.get_force()
86         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

 88     num_steps = 10.0
 89     bindings_list = []
 90     bindings = (
 91         #(test, command, description)
 92         ((bdn, ['btnLeft']), (gripper.reboot, []), "reboot"),
 93         ((bdn, ['btnUp']), (gripper.calibrate, []), "calibrate"),
 94         ((bdn, ['leftTrigger']), (gripper.close, []), "close"),
 95         ((bup, ['leftTrigger']), (gripper.open, []), "open (release)"),
 96         ((bdn, ['leftBumper']), (gripper.stop, []), "stop"),
 97         ((jlo, ['leftStickHorz']), (offset_position, [-(gripper.MAX_POSITION / num_steps)]),
 98                                     "decrease position"),
 99         ((jhi, ['leftStickHorz']), (offset_position, [gripper.MAX_POSITION / num_steps]),
100                                      "increase position"),
101         ((jlo, ['leftStickVert']), (offset_holding, [-(gripper.MAX_FORCE / num_steps)]),
102                                     "decrease holding force"),
103         ((jhi, ['leftStickVert']), (offset_holding, [gripper.MAX_FORCE / num_steps]),
104                                     "increase holding force"),
105         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
106         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
107     )
108     bindings_list.append(bindings)

The bindings is a dictionary that holds the set of characters in the joystick and their corresponding gripper functions.

110     rospy.loginfo("Enabling robot...")
111     rs.enable()
112     rate = rospy.Rate(100)
113     print_help(bindings_list)
114     print("Press <Start> button for help; Ctrl-C to stop...")
115     while not rospy.is_shutdown():
116         # test each joystick condition and call binding cmd if true
117         for (test, cmd, doc) in bindings:
118             if test[0](*test[1]):
119                 cmd[0](*cmd[1])
120                 print(doc)
121         rate.sleep()
122     rospy.signal_shutdown("Example finished.")

The while loop iterates till the Esc or ctrl-c is inputted or ros-shutdown signal is given. If Esc or ctrl-c is given then shutdown signal is sent.

125 def main():
126     """RSDK Gripper Example: Joystick Control
127 
128     Use a game controller to control the grippers.
129 
130     Attach a game controller to your dev machine and run this
131     example along with the ROS joy_node to control gripper
132     using the joysticks and buttons. Be sure to provide
133     the *joystick* type you are using as an argument to setup
134     appropriate key mappings.
135 
136     Uses the intera_interface.Gripper class and the helper classes
137     in intera_external_devices.Joystick.
138     """
139     epilog = """
140 See help inside the example with the "Start" button for controller
141 key bindings.
142     """
143     rp = intera_interface.RobotParams()
144     valid_limbs = rp.get_limb_names()
145     if not valid_limbs:
146         rp.log_message(("Cannot detect any limb parameters on this robot. "
147                         "Exiting."), "ERROR")
148         return
149     arg_fmt = argparse.RawDescriptionHelpFormatter
150     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
151                                      description=main.__doc__,
152                                      epilog=epilog)
153     required = parser.add_argument_group('required arguments')
154     required.add_argument(
155         '-j', '--joystick', required=True, choices=['xbox', 'logitech', 'ps3'],
156         help='specify the type of joystick to use'
157     )
158     parser.add_argument(
159         "-l", "--limb", dest="limb", default=valid_limbs[0],
160         choices=valid_limbs,
161         help="Limb on which to run the gripper joystick example"
162     )
163     args = parser.parse_args(rospy.myargv()[1:])

Initialized the node and the joystick argument needs to be provided.

125     joystick = None
126     if args.joystick == 'xbox':
127         joystick = intera_external_devices.joystick.XboxController()
128     elif args.joystick == 'logitech':
129         joystick = intera_external_devices.joystick.LogitechController()
130     elif args.joystick == 'ps3':
131         joystick = intera_external_devices.joystick.PS3Controller()
132     else:
133         # Should never reach this case with proper argparse usage
134         parser.error("Unsupported joystick type '%s'" % (args.joystick))
135 
136     print("Initializing node... ")
137     rospy.init_node("sdk_gripper_joystick")
138 
139     map_joystick(joystick, args.limb)
140 
141 
142 if __name__ == '__main__':

Joystick device selection.

Gripper Keyboard Example Code Walkthrough

This example demonstrates the usage of the gripper control via intera interface. The main() function invokes the map_keyboard() function. It is at this function where the keyboard keys are mapped to individual gripper actions and the commands are executed periodically.

Now, let's break down the code.

33 import argparse
34 
35 import rospy
36 
37 import intera_interface
38 import intera_external_devices
39 from intera_interface import CHECK_VERSION

This imports the intera interface for accessing the limb and the gripper class. The intera_external_devices is imported to use its getch function that captures the key presses on the keyboard. 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.

41 def map_keyboard(limb):
42     # initialize interfaces
43     print("Getting robot state...")
44     rs = intera_interface.RobotEnable(CHECK_VERSION)
45     init_state = rs.state()
46     try:
47         gripper = intera_interface.Gripper(limb)
48     except ValueError:
49         rospy.logerr("Could not detect a gripper attached to the robot.")
50         return

The init_state variable captures the current state of the robot. The Gripper instance class is created. If no gripper detect, the logerr message will show up.

51     def clean_shutdown():
52         if not init_state:
53             print("Disabling robot...")
54             rs.disable()
55         print("Exiting example.")
56     rospy.on_shutdown(clean_shutdown)

On shutdown request, Sawyer's state is sent back to its initial state.

57     def offset_position(offset):
58         current = gripper.get_position()
59         gripper.set_position(current + offset)
60 
61     def offset_holding(offset):
62         current = gripper.get_force()
63         gripper.set_holding_force(current + offset)

Setup gripper position and holding force.

65     num_steps = 10.0
66     thirty_percent_velocity = 0.3*(gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY
67     bindings = {
68     #   key: (function, args, description)
69         'r': (gripper.reboot, [], "reboot"),
70         'c': (gripper.calibrate, [], "calibrate"),
71         'q': (gripper.close, [], "close"),
72         'o': (gripper.open, [], "open"),
73         '+': (gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"),
74         '-': (gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"),
75         's': (gripper.stop, [], "stop"),
76         'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)], "decrease holding force"),
77         'j': (offset_holding, [gripper.MAX_FORCE / num_steps], "increase holding force"),
78         'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)], "decrease position"),
79         'i': (offset_position, [gripper.MAX_POSITION / num_steps], "increase position"),
80     }

The bindings is a dictionary that holds the set of characters in the keyboard and their corresponding gripper functions.

 82     done = False
 83     rospy.loginfo("Enabling robot...")
 84     rs.enable()
 85     print("Controlling grippers. Press ? for help, Esc to quit.")
 86     while not done and not rospy.is_shutdown():
 87         c = intera_external_devices.getch()
 88         if c:
 89             if c in ['\x1b', '\x03']:
 90                 done = True
 91             elif c in bindings:
 92                 cmd = bindings[c]
 93                 cmd[0](*cmd[1])
 94                 print("command: %s" % (cmd[2],))
 95             else:
 96                 print("key bindings: ")
 97                 print("  Esc: Quit")
 98                 print("  ?: Help")
 99                 for key, val in sorted(bindings.items(),
100                                        key=lambda x: x[1][2]):
101                     print("  %s: %s" % (key, val[2]))
102     # force shutdown call if caught by key handler
103     rospy.signal_shutdown("Example finished.")

The while loop iterates till the Esc or Ctrl-c is inputted or ros-shutdown signal is given. If Esc or Ctrl-c is given then shutdown signal is sent.

106 def main():
107     """RSDK Gripper Example: Keyboard Control
108 
109     Use your dev machine's keyboard to control and configure grippers.
110 
111     Run this example to command various gripper movements while
112     adjusting gripper parameters, including calibration, velocity,
113     and force. Uses the intera_interface.Gripper class and the
114     helper function, intera_external_devices.getch.
115     """
116     epilog = """
117 See help inside the example with the '?' key for key bindings.
118     """
119     rp = intera_interface.RobotParams()
120     valid_limbs = rp.get_limb_names()
121     if not valid_limbs:
122         rp.log_message(("Cannot detect any limb parameters on this robot. "
123                         "Exiting."), "ERROR")
124         return
125     arg_fmt = argparse.RawDescriptionHelpFormatter
126     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
127                                      description=main.__doc__,
128                                      epilog=epilog)
129     parser.add_argument(
130         "-l", "--limb", dest="limb", default=valid_limbs[0],
131         choices=valid_limbs,
132         help="Limb on which to run the gripper keyboard example"
133     )
134     args = parser.parse_args(rospy.myargv()[1:])
135 
136     print("Initializing node... ")
137     rospy.init_node("sdk_gripper_keyboard")
138 
139     map_keyboard(args.limb)
140 
141 
142 if __name__ == '__main__':

Initialized the node then call map_keyboard function.