The Gripper Cuff Control example demonstrates basic cuff/gripper interaction on the robot. In this example, connects cuff buttons to gripper open/close commands: 'Circle' Button will open gripper, 'Dash' Button will close gripper, Cuff 'Squeeze' will turn on Nav lights.
Introduction
This program demonstrates the usage of gripper, cuff, lights interface to control the close and open actions of the gripper and navigation light. The main() function creates an instance of the GripperConnect class which connects the gripper buttons to various callback functions.
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 gripper cuff control example.
Usage
$ rosrun intera_examples gripper_cuff_control.py -h
Arguments
Important Arguments:
-g or --gripper : The gripper limb to control.
-n or --no-lights : Specify the argument will not trigger lights on cuff while grasping.
-v or --verbose : Specify the verbose will print the debug statements from rospy, default value is rospy.INFO
usage: gripper_cuff_control.py [-h] [-g {['right']}] [-n] [-v]
SDK Gripper Button Control Example
Connects cuff buttons to gripper open/close commands:
'Circle' Button - open gripper
'Dash' Button - close gripper
Cuff 'Squeeze' - turn on Nav lights
Run this example in the background or in another terminal
to be able to easily control the grippers by hand while
using the robot. Can be run in parallel with other code.
optional arguments:
-h, --help show this help message and exit
-g {['right']}, --gripper {['right']}
gripper limb to control (default: both)
-n, --no-lights do not trigger lights on cuff grasp
-v, --verbose print debug statements
Code Walkthrough
Now, let's break down the code.
30 import argparse
31 import sys
32
33 import rospy
34
35 from intera_interface import (
36 Gripper,
37 Lights,
38 Cuff,
39 RobotParams,
40 )
This imports the intera interface for accessing the Cuff, Lights and the Gripper class.
43 class GripperConnect(object):
44 """
45 Connects wrist button presses to gripper open/close commands.
46 Uses the Navigator callback feature to make callbacks to connected
47 action functions when the button values change.
48 """
49
50 def __init__(self, arm, lights=True):
51 """
52 @type arm: str
53 @param arm: arm of gripper to control
54 @type lights: bool
55 @param lights: if lights should activate on cuff grasp
56 """
57 self._arm = arm
58 # inputs
59 self._cuff = Cuff(limb=arm)
60 # connect callback fns to signals
61 self._lights = None
62 if lights:
63 self._lights = Lights()
64 self._cuff.register_callback(self._light_action,
65 '{0}_cuff'.format(arm))
Instances of the cuff and the lights are created.
67 try:
68 self._gripper = Gripper(arm)
69 if not (self._gripper.is_calibrated() or
70 self._gripper.calibrate() == True):
71 rospy.logerr("({0}_gripper) calibration failed.".format(
72 self._gripper.name))
73 raise
74 self._cuff.register_callback(self._close_action,
75 '{0}_button_upper'.format(arm))
76 self._cuff.register_callback(self._open_action,
77 '{0}_button_lower'.format(arm))
78 rospy.loginfo("{0} Cuff Control initialized...".format(
79 self._gripper.name))
80 except:
81 self._gripper = None
82 msg = ("{0} Gripper is not connected to the robot."
83 " Running cuff-light connection only.").format(arm.capitalize())
84 rospy.logwarn(msg)
If the gripper attached, create the instance of the gripper then verify if the gripper was already in a calibrated state, the cuff actions will be registered afterwards. Failed to calibrate the gripper will raise error. If the gripper is not attached, warning message will appear.
87 def _open_action(self, value):
88 if value and self._gripper.is_ready():
89 rospy.logdebug("gripper open triggered")
90 self._gripper.open()
91 if self._lights:
92 self._set_lights('red', False)
93 self._set_lights('green', True)
The method opens the gripper and set cuff light when button sends a True value.
95 def _close_action(self, value):
96 if value and self._gripper.is_ready():
97 rospy.logdebug("gripper close triggered")
98 self._gripper.close()
99 if self._lights:
100 self._set_lights('green', False)
101 self._set_lights('red', True)
The method closes the gripper and set cuff light when button sends a True value.
103 def _light_action(self, value):
104 if value:
105 rospy.logdebug("cuff grasp triggered")
106 else:
107 rospy.logdebug("cuff release triggered")
108 if self._lights:
109 self._set_lights('red', False)
110 self._set_lights('green', False)
111 self._set_lights('blue', value)
This method assigns the boolean value that was signalled to the light interface.
113 def _set_lights(self, color, value):
114 self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value))
115 self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color),
116 on=bool(value))
Set light color according to the provided params.
118 def main():
119 """SDK Gripper Button Control Example
120 Connects cuff buttons to gripper open/close commands:
121 'Circle' Button - open gripper
122 'Dash' Button - close gripper
123 Cuff 'Squeeze' - turn on Nav lights
124 Run this example in the background or in another terminal
125 to be able to easily control the grippers by hand while
126 using the robot. Can be run in parallel with other code.
127 """
128 rp = RobotParams()
129 valid_limbs = rp.get_limb_names()
130 if not valid_limbs:
131 rp.log_message(("Cannot detect any limb parameters on this robot. "
132 "Exiting."), "ERROR")
133 return
134 if len(valid_limbs) > 1:
135 valid_limbs.append("all_limbs")
136 arg_fmt = argparse.RawDescriptionHelpFormatter
137 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
138 description=main.__doc__)
139 parser.add_argument('-g', '--gripper', dest='gripper', default=valid_limbs[0],
140 choices=[valid_limbs],
141 help='gripper limb to control (default: both)')
142 parser.add_argument('-n', '--no-lights', dest='lights',
143 action='store_false',
144 help='do not trigger lights on cuff grasp')
145 parser.add_argument('-v', '--verbose', dest='verbosity',
146 action='store_const', const=rospy.DEBUG,
147 default=rospy.INFO,
148 help='print debug statements')
149 args = parser.parse_args(rospy.myargv()[1:])
The gripper option, the light and verbose options are parsed from the command line arguments, as entered by the user.
153 rospy.init_node('sdk_gripper_cuff_control_{0}'.format(args.gripper),
154 log_level=args.verbosity)
155
156 arms = (args.gripper,) if args.gripper != 'all_limbs' else valid_limbs[:-1]
157 grip_ctrls = [GripperConnect(arm, args.lights) for arm in arms]
158
159 print("Press cuff buttons for gripper control. Spinning...")
160 rospy.spin()
161 print("Gripper Button Control Finished.")
162 return 0
163
164 if __name__ == '__main__':
165 sys.exit(main())
The node is initialized and an instance of the GripperConnect is created.
