Difference between revisions of "Joint Position Example"

Jump to: navigation , search
(Joint Position Joystick Code Walkthrough1)
(Joint Position Joystick Code Walkthrough2)
Line 472: Line 472:
  
 
Put comments TODO
 
Put comments TODO
 
</div>
 

Revision as of 10:49, 27 November 2016

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.

Usage

Start the joint position keyboard example program, ex:

$ rosrun intera_examples joint_position_keyboard.py

Start the joint position joystick example program, ex:

$ rosrun intera_examples joint_position_joystick.py

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.

Arguments

Important Arguments:

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

$ rosrun intera_examples joint_position_keyboard.py -h
usage: joint_position_keyboard.py [-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 joint_position_joystick.py -h
usage: joint_position_joystick.py [-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.
    Ex:
      (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


Joint Position Keyboard Code Walkthrough

Now, let's break down the code.

33 """
34 SDK Joint Position Example: keyboard
35 """
36 import argparse
37 
38 import rospy
39 
40 import intera_interface
41 import intera_external_devices
42 
43 from intera_interface import CHECK_VERSION
43 def map_keyboard(side):
44     limb = intera_interface.Limb(side)
45     
46     try:
47         gripper = intera_interface.Gripper(side)
48     except:
49         has_gripper = False
50         rospy.logerr("Could not initalize the gripper.")
51     else:
52         has_gripper = True
53 
54     joints = limb.joint_names()
56     def set_j(limb, joint_name, delta):
57         current_position = limb.joint_angle(joint_name)
58         joint_command = {joint_name: current_position + delta}
59         limb.set_joint_positions(joint_command)
60 
61     def set_g(action):
62         if has_gripper:
63             if action == "close":
64                 gripper.close()
65             elif action == "open":
66                 gripper.open()
67             elif action == "calibrate":
68                 gripper.calibrate()
70     bindings = {
71         '1': (set_j, [limb, joints[0], 0.1], joints[0]+" increase"),
72         'q': (set_j, [limb, joints[0], -0.1], joints[0]+" decrease"),
73         '2': (set_j, [limb, joints[1], 0.1], joints[1]+" increase"),
74         'w': (set_j, [limb, joints[1], -0.1], joints[1]+" decrease"),
75         '3': (set_j, [limb, joints[2], 0.1], joints[2]+" increase"),
76         'e': (set_j, [limb, joints[2], -0.1], joints[2]+" decrease"),
77         '4': (set_j, [limb, joints[3], 0.1], joints[3]+" increase"),
78         'r': (set_j, [limb, joints[3], -0.1], joints[3]+" decrease"),
79         '5': (set_j, [limb, joints[4], 0.1], joints[4]+" increase"),
80         't': (set_j, [limb, joints[4], -0.1], joints[4]+" decrease"),
81         '6': (set_j, [limb, joints[5], 0.1], joints[5]+" increase"),
82         'y': (set_j, [limb, joints[5], -0.1], joints[5]+" decrease"),
83         '7': (set_j, [limb, joints[6], 0.1], joints[6]+" increase"),
84         'u': (set_j, [limb, joints[6], -0.1], joints[6]+" decrease"),
85         '8': (set_g, "close", side+" gripper close"),
86         'i': (set_g, "open", side+" gripper open"),
87         '9': (set_g, "calibrate", side+" gripper calibrate")
88      }
89     done = False
90     print("Controlling joints. Press ? for help, Esc to quit.")
91     while not done and not rospy.is_shutdown():
92         c = intera_external_devices.getch()
93         if c:
94             #catch Esc or ctrl-c
95             if c in ['\x1b', '\x03']:
96                 done = True
97                 rospy.signal_shutdown("Example finished.")
 98             elif c in bindings:
 99                 cmd = bindings[c]
100                 if c == '8' or c == 'i' or c == '9':
101                     cmd[0](cmd[1])
102                     print("command: %s" % (cmd[2],))
103                 else:
104                     #expand binding to something like "set_j(right, 'j0', 0.1)"
105                     cmd[0](*cmd[1])
106                     print("command: %s" % (cmd[2],))
107             else:
108                 print("key bindings: ")
109                 print("  Esc: Quit")
110                 print("  ?: Help")
111                 for key, val in sorted(bindings.items(),
112                                        key=lambda x: x[1][2]):
113                     print("  %s: %s" % (key, val[2]))
115 def main():
116     """RSDK Joint Position Example: Keyboard Control
117     Use your dev machine's keyboard to control joint positions.
118     Each key corresponds to increasing or decreasing the angle
119     of a joint on Sawyer's arm. The increasing and descreasing
120     are represented by number key and letter key next to the number.
121     """
122     epilog = """
123 See help inside the example with the '?' key for key bindings.
124     """
125     rp = intera_interface.RobotParams()
126     valid_limbs = rp.get_limb_names()
127     if not valid_limbs:
128         rp.log_message(("Cannot detect any limb parameters on this robot. "
129                         "Exiting."), "ERROR")
130         return
131     arg_fmt = argparse.RawDescriptionHelpFormatter
132     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
133                                      description=main.__doc__,
134                                      epilog=epilog)
135     parser.add_argument(
136         "-l", "--limb", dest="limb", default=valid_limbs[0],
137         choices=valid_limbs,
138         help="Limb on which to run the joint position keyboard example"
139     )
140     args = parser.parse_args(rospy.myargv()[1:])
141 
142     print("Initializing node... ")
143     rospy.init_node("sdk_joint_position_keyboard")
144     print("Getting robot state... ")
145     rs = intera_interface.RobotEnable(CHECK_VERSION)
146     init_state = rs.state().enabled
150     def clean_shutdown():
151         print("\nExiting example.")
152         if not init_state:
153             print("Disabling robot...")
154             rs.disable()
155     rospy.on_shutdown(clean_shutdown)
157     rospy.loginfo("Enabling robot...")
158     rs.enable()
159     map_keyboard(args.limb)
160     print("Done.")
161 
162 
163 if __name__ == '__main__':
164     main()


Joint Position Joystick Code Walkthrough

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 
40 from intera_interface import CHECK_VERSION
43 def rotate(l):
44     """
45     Rotates a list left.
46     @param l: the list
47     """
48     if len(l):
49         v = l[0]
50         l[:-1] = l[1:]
51         l[-1] = v
54 def set_j(cmd, limb, joints, index, delta):
55     """
56     Set the selected joint to current pos + delta.
57     @param cmd: the joint command dictionary
58     @param limb: the limb to get the pos from
59     @param joints: a list of joint names
60     @param index: the index in the list of names
61     @param delta: delta to update the joint by
62     joint/index is to make this work in the bindings.
63     """
64     joint = joints[index]
65     cmd[joint] = delta + limb.joint_angle(joint)
69 def map_joystick(joystick, side):
70     """
71     Maps joystick input to joint position commands.
72     @param joystick: an instance of a Joystick
73     """
74     limb = intera_interface.Limb(side)
75     gripper = None
76     try:
77         gripper = intera_interface.Gripper(side)
78     except:
79         rospy.loginfo("Could not detect a connected electric gripper.")
82     def set_g(action):
83         if gripper is not None:
84             if action == "close":
85                 gripper.close()
86             elif action == "open":
87                 gripper.open()
88             elif action == "calibrate":
89                 gripper.calibrate()
 91     limb_cmd = {}
 92 
 93     #available joints
 94     joints = limb.joint_names()
 95 
 96     #abbreviations
 97     jhi = lambda s: joystick.stick_value(s) > 0
 98     jlo = lambda s: joystick.stick_value(s) < 0
 99     bdn = joystick.button_down
100     bup = joystick.button_up
102     def print_help(bindings_list):
103         print("Press Ctrl-C to quit.")
104         for bindings in bindings_list:
105             for (test, _cmd, doc) in bindings:
106                 if callable(doc):
107                     doc = doc()
108                 print("%s: %s" % (str(test[1][0]), doc))
109 
110     bindings_list = []
111     bindings = [
112         ((jlo, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, 0.1]),
113             lambda: "Increase " + joints[0]),
114         ((jhi, ['leftStickHorz']), (set_j, [limb_cmd, limb, joints, 0, -0.1]),
115             lambda: "Decrease " + joints[0]),
116         ((jlo, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, 0.1]),
117             lambda: "Increase " + joints[1]),
118         ((jhi, ['leftStickVert']), (set_j, [limb_cmd, limb, joints, 1, -0.1]),
119             lambda: "Decrease " + joints[1]),
120         ((jlo, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, 0.1]),
121             lambda: "Increase " + joints[2]),
122         ((jhi, ['rightStickHorz']), (set_j, [limb_cmd, limb, joints, 2, -0.1]),
123             lambda: "Decrease " + joints[2]),
124         ((jlo, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, 0.1]),
125             lambda: "Increase " + joints[3]),
126         ((jhi, ['rightStickVert']), (set_j, [limb_cmd, limb, joints, 3, -0.1]),
127             lambda: "Decrease " + joints[3]),
128         ((bdn, ['leftBumper']), (rotate, [joints]), side + ": cycle joint"),
129         ((bdn, ['function1']), (print_help, [bindings_list]), "help"),
130         ((bdn, ['function2']), (print_help, [bindings_list]), "help"),
131         ]
132     if gripper:
133         bindings.extend([
134             ((bdn, ['rightTrigger']), (set_g, ['close'], gripper), side + " gripper close"),
135             ((bup, ['rightTrigger']), (set_g, ['open'], gripper), side + " gripper open"),
136             ((bdn, ['btnLeft']), (set_g, ['calibrate'], gripper), "right calibrate")
137             ])
138     bindings_list.append(bindings)
139 
140     rate = rospy.Rate(100)
141     print_help(bindings_list)
142     print("Press Ctrl-C to stop. ")
143     while not rospy.is_shutdown():
144         for (test, cmd, doc) in bindings:
145             if test[0](*test[1]):
146                 cmd[0](*cmd[1])
147                 if callable(doc):
148                     print(doc())
149                 else:
150                     print(doc)
151         if len(limb_cmd):
152             limb.set_joint_positions(limb_cmd)
153             limb_cmd.clear()
154         rate.sleep()
155     return False
158 def main():
159     """SDK Joint Position Example: Joystick Control
160     Use a game controller to control the angular joint positions
161     of Sawyer's arms.
162     Attach a game controller to your dev machine and run this
163     example along with the ROS joy_node to control the position
164     of each joint in Sawyer's arm using the joystick. Be sure to
165     provide your *joystick* type to setup appropriate key mappings.
166     Each stick axis maps to a joint angle; which joints are currently
167     controlled can be incremented by using the mapped increment buttons.
168     Ex:
169       (x,y -> e0,e1) >>increment>> (x,y -> e1,e2)
170     """
171     epilog = """
172 See help inside the example with the "Start" button for controller
173 key bindings.
174     """
175     rp = intera_interface.RobotParams()
176     valid_limbs = rp.get_limb_names()
177     if not valid_limbs:
178         rp.log_message(("Cannot detect any limb parameters on this robot. "
179                         "Exiting."), "ERROR")
180         return
181     arg_fmt = argparse.RawDescriptionHelpFormatter
182     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
183                                      description=main.__doc__,
184                                      epilog=epilog)
185     required = parser.add_argument_group('required arguments')
186     required.add_argument(
187         '-j', '--joystick', required=True,
188         choices=['xbox', 'logitech', 'ps3'],
189         help='specify the type of joystick to use'
190     )
191     parser.add_argument(
192         "-l", "--limb", dest="limb", default=valid_limbs[0],
193         choices=valid_limbs,
194         help="Limb on which to run the joint position joystick example"
195     )
196     args = parser.parse_args(rospy.myargv()[1:])
197 
198     joystick = None
199     if args.joystick == 'xbox':
200         joystick = intera_external_devices.joystick.XboxController()
201     elif args.joystick == 'logitech':
202         joystick = intera_external_devices.joystick.LogitechController()
203     elif args.joystick == 'ps3':
204         joystick = intera_external_devices.joystick.PS3Controller()
205     else:
206         parser.error("Unsupported joystick type '%s'" % (args.joystick))
207 
208     print("Initializing node... ")
209     rospy.init_node("sdk_joint_position_joystick")
210     print("Getting robot state... ")
211     rs = intera_interface.RobotEnable(CHECK_VERSION)
212     init_state = rs.state().enabled
217     def clean_shutdown():
218         print("\nExiting example.")
219         if not init_state:
220             print("Disabling robot...")
221             rs.disable()
222     rospy.on_shutdown(clean_shutdown)
223 
224     rospy.loginfo("Enabling robot...")
225     rs.enable()
226 
227     map_joystick(joystick, args.limb)
228     print("Done.")
229 
230 
231 if __name__ == '__main__':
232     main()

Put comments TODO