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.
| 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.
