KeyboardController.py 22.9 KB
Newer Older
Cadmus's avatar
Cadmus committed
1
#!/usr/bin/env python
2
DEBUG = True
Cadmus's avatar
Cadmus committed
3
4
5
6
7
8
9
VERBOSE = True

## Python libraries
from copy import deepcopy
import sys

## 3rd party libraries
Lab Computer's avatar
Lab Computer committed
10
11
from pynput import keyboard

Cadmus's avatar
Cadmus committed
12
13
14
15
16
17
18
19
20
# Scipy (1.2.3)
from scipy.spatial.transform import Rotation

# Numpy (1.16.6)
import numpy as np

# ROS Libraries
import rospy
from geometry_msgs.msg import PoseStamped
21
22
from moveit_msgs.srv import GetPositionFK,GetPositionFKRequest
from moveit_msgs.srv import GetPositionIK,GetPositionIKRequest
Cadmus's avatar
Cadmus committed
23
24
25
from moveit_msgs.msg import MoveItErrorCodes

## Our libraries
26
from Controller import Controller,getR_ET
Cadmus's avatar
Cadmus committed
27
28
29
30
31
32
33
34
35
36
37


class KeyboardController(Controller):
    """
    Controller to control robot arms by keyboard.

    Most functions are implemented for convenience, not speed.
    """

    def __init__(self,
        homeConf_deg=np.array([-90,-90,45,-45,-90,-45]),
38
        waitForExtruder=True,
39
        timeScale=1.0,
40
41
        disableExtruder=False,
        robot=0):
Cadmus's avatar
Cadmus committed
42
43
        """
        """
44
45
46
        if DEBUG:
            print("Creating Controller")

Lab Computer's avatar
Lab Computer committed
47
        super(KeyboardController,self).__init__(
48
            timeScale=timeScale,
49
            blocking=waitForExtruder,
50
51
            disable_extruder=disableExtruder,
            robot=robot)
Cadmus's avatar
Cadmus committed
52
53
54
55
56
57

        # 1x6
        homeConf_rad = np.deg2rad(homeConf_deg)
        self.homeConf_rad = homeConf_rad.tolist()


58
    def fk(self,conf):
Cadmus's avatar
Cadmus committed
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
        """
        Compute forward kinematics using a ROS service.

        Joint angles in configurations should be in the order of getJointNames()
        Function for convenience, not for speed.

        conf: 6x numpy array of joint angles in radians
        Returns (pos_RE,quat_RE)
        pos_RE: 3x numpy array in metres
        quat_RE: 4x numpy array
        """
        if DEBUG:
            print('Controller::fk()')

        rospy.wait_for_service('compute_fk')
        try:
75
            moveitFK = rospy.ServiceProxy('compute_fk',GetPositionFK)
Cadmus's avatar
Cadmus committed
76
77
78
79
80
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s"%e)

        # Form service request
        req = GetPositionFKRequest()
Lab Computer's avatar
Lab Computer committed
81
        req.header.frame_id = 'base_link'
Cadmus's avatar
Cadmus committed
82
83
        req.fk_link_names = ['ee_link']
        req.robot_state.joint_state.name = self.group.get_active_joints()
Lab Computer's avatar
Lab Computer committed
84
85
86
87
88
89
        req.robot_state.joint_state.position = conf.tolist()

        resp = moveitFK.call(req)
        if resp.error_code.val != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Controller::fk(): Service returned {}'.format(resp.error_code.val))
            return None
Cadmus's avatar
Cadmus committed
90

Lab Computer's avatar
Lab Computer committed
91
92
93
94
        pos_RE = np.array([resp.pose_stamped.pose.position.x,resp.pose_stamped.pose.position.y,resp.pose_stamped.pose.position.z])
        quat_RE = np.array([resp.pose_stamped.pose.orientation.x,resp.pose_stamped.pose.orientation.y,resp.pose_stamped.pose.orientation.z,resp.pose_stamped.pose.orientation.w])

        return (pos_RE,quat_RE)
Cadmus's avatar
Cadmus committed
95
96


97
    def ik(self,pos_RE,quat_RE,conf_hint = None):
Cadmus's avatar
Cadmus committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
        """
        Compute inverse kinematics using a ROS service.
        Gives configuration that is "close" to conf_hint if specified.

        Joint angles in configurations should be in the order of getJointNames()
        Function for convenience, not for speed.
        Note: ROS service does inverse kinematics with randomness,
              so the same pose will give different joint solutions.

        pos_RE: 3x numpy array in metres
        quat_RE: 4x numpy array
        conf_hint: 6x numpy array of joint angles in radians
        Returns 6x numpy array of joint angles in radians
        """
        if DEBUG:
            print('Controller::ik()')

        if conf_hint == None:
Lab Computer's avatar
Lab Computer committed
116
            conf_hint = np.array(self.group.get_current_joint_values())
Cadmus's avatar
Cadmus committed
117
118
119

        rospy.wait_for_service('compute_ik')
        try:
120
            moveitIK = rospy.ServiceProxy('compute_ik',GetPositionIK)
Cadmus's avatar
Cadmus committed
121
122
123
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s"%e)

Lab Computer's avatar
Lab Computer committed
124
125

        name_conf = self.group.get_active_joints()
Cadmus's avatar
Cadmus committed
126
127
128
129

        req = GetPositionIKRequest()
        req.ik_request.ik_link_name = 'ee_link'
        req.ik_request.group_name = 'manipulator'
Lab Computer's avatar
Lab Computer committed
130
        req.ik_request.robot_state.joint_state.name = name_conf
Cadmus's avatar
Cadmus committed
131
        req.ik_request.robot_state.joint_state.position = conf_hint.tolist()
Lab Computer's avatar
Lab Computer committed
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
        req.ik_request.pose_stamped.pose.position.x = pos_RE[0].item()
        req.ik_request.pose_stamped.pose.position.y = pos_RE[1].item()
        req.ik_request.pose_stamped.pose.position.z = pos_RE[2].item()
        req.ik_request.pose_stamped.pose.orientation.x = quat_RE[0].item()
        req.ik_request.pose_stamped.pose.orientation.y = quat_RE[1].item()
        req.ik_request.pose_stamped.pose.orientation.z = quat_RE[2].item()
        req.ik_request.pose_stamped.pose.orientation.w = quat_RE[3].item()

        resp = moveitIK.call(req)
        if resp.error_code.val == MoveItErrorCodes.NO_IK_SOLUTION:
            return None
        elif resp.error_code.val != MoveItErrorCodes.SUCCESS:
            import pdb; pdb.set_trace()
            rospy.logwarn('Controller::ik(): Service returned {}'.format(resp.error_code.val))
            return None

        conf = np.array(resp.solution.joint_state.position)
Cadmus's avatar
Cadmus committed
149

Lab Computer's avatar
Lab Computer committed
150
        return conf
Cadmus's avatar
Cadmus committed
151
152
153
154
155
156
157


    def move_home(self):
        """
        Move the arm to the home configuration.
        """
        self.move_to(self.homeConf_rad,
Lab Computer's avatar
Lab Computer committed
158
            near_deg=None)
Cadmus's avatar
Cadmus committed
159
160


161
    def _translate_endpoint_base(self,dx_m,dy_m,dz_m,resolution_m=0.01):
Cadmus's avatar
Cadmus committed
162
163
164
165
166
167
168
169
170
171
        """
        Translate the endpoint of the arm from where it currently is,
        relative to the robot base.

        dx_m: 
        dy_m: 
        dz_m: 
        resolution_m: Largest end effector distance between two configurations
        """
        ## Get current pose
Lab Computer's avatar
Lab Computer committed
172
173
        pose_curr = self.group.get_current_pose().pose
        pose_next = deepcopy(pose_curr)
Cadmus's avatar
Cadmus committed
174
175

        ## Shift pose
Lab Computer's avatar
Lab Computer committed
176
177
178
        pose_next.position.x += dx_m
        pose_next.position.y += dy_m
        pose_next.position.z += dz_m
Cadmus's avatar
Cadmus committed
179
180

        ## Compute plan to move the end effector in a "straight" line
181
        (plan,frac) = self.group.compute_cartesian_path([pose_curr,pose_next],resolution_m,0.0)
Cadmus's avatar
Cadmus committed
182
183
184
        # NOTE: This plan gives duplicate points in the joint trajectory, so...

        # Create new plan with duplicates removed
Lab Computer's avatar
Lab Computer committed
185
186
        cleanTraj = deepcopy(plan.joint_trajectory)
        cleanTraj.points = []
Cadmus's avatar
Cadmus committed
187
188
189
190

        prevTime = None
        for point in plan.joint_trajectory.points:
            if prevTime is None or point.time_from_start.secs != prevTime.secs or point.time_from_start.nsecs != prevTime.nsecs:
Lab Computer's avatar
Lab Computer committed
191
                cleanTraj.points.append(point)
Cadmus's avatar
Cadmus committed
192
193
194
                prevTime = point.time_from_start

        ## Execute plan
Lab Computer's avatar
Lab Computer committed
195
        self.exec_traj(cleanTraj)
Cadmus's avatar
Cadmus committed
196
197


198
    def _translate_endpoint_ee(self,dx_m,dy_m,dz_m,resolution_m=0.01):
Cadmus's avatar
Cadmus committed
199
200
201
202
203
204
205
206
207
208
        """
        Translate the endpoint of the arm from where it currently is,
        relative to the end effector.

        dx_m: 
        dy_m: 
        dz_m: 
        resolution_m: Largest end effector distance between two configurations
        """
        ## Get current pose
Lab Computer's avatar
Lab Computer committed
209
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
210
211
212

        ## Shift pose
        # Rotate translation to end effector frame
213
        trans_ee = np.array([dx_m,dy_m,dz_m])
Lab Computer's avatar
Lab Computer committed
214
215
        quat_curr = np.array([pose_curr.orientation.x,pose_curr.orientation.y,pose_curr.orientation.z,pose_curr.orientation.w])
        R_curr = Rotation.from_quat(quat_curr)
216
        trans_base = R_curr.apply(trans_ee)
Cadmus's avatar
Cadmus committed
217

218
219
        # Translate as defined at base
        self._translate_endpoint_base(trans_base[0].item(),trans_base[1].item(),trans_base[2].item(),resolution_m)
Cadmus's avatar
Cadmus committed
220
221


222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
    def _translate_endpoint_tool(self,dx_m,dy_m,dz_m,resolution_m=0.01):
        """
        Translate the endpoint of the arm from where it currently is,
        relative to the tool tip.

        dx_m: 
        dy_m: 
        dz_m: 
        resolution_m: Largest end effector distance between two configurations
        """
        ## Get orientation of tool tip relative to end effector
        R_ET = getR_ET()

        ## Shift pose
        # Rotate translation to end effector frame
        trans_tool = np.array([dx_m,dy_m,dz_m])
        trans_ee = R_ET.apply(trans_tool)

        # Translate as defined at end effector
        self._translate_endpoint_ee(trans_ee[0].item(),trans_ee[1].item(),trans_ee[2].item(),resolution_m)


    def _rotate_endpoint_base(self,dx_deg,dy_deg,dz_deg):
Cadmus's avatar
Cadmus committed
245
246
247
248
249
250
251
252
253
254
255
256
257
        """
        Rotate the endpoint of the arm from where it currently is,
        relative to the robot base.

        Rotation is specified as a rotation vector
        See: https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector

        dx_deg:
        dy_deg:
        dz_deg:
        Returns True if arm is moved, False otherwise
        """
        ## Get current pose
Lab Computer's avatar
Lab Computer committed
258
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
259
260
261

        ## Get current orientation
        # 4x
Lab Computer's avatar
Lab Computer committed
262
        quat_curr = np.array([pose_curr.orientation.x,pose_curr.orientation.y,pose_curr.orientation.z,pose_curr.orientation.w])
Cadmus's avatar
Cadmus committed
263

Lab Computer's avatar
Lab Computer committed
264
        R_curr = Rotation.from_quat(quat_curr)
Cadmus's avatar
Cadmus committed
265
266
267

        ## Get step rotation
        # 3x
268
        rotVec_base_deg = np.array([dx_deg,dy_deg,dz_deg])
Cadmus's avatar
Cadmus committed
269
270
271
272
273
        rotVec_base_rad = np.deg2rad(rotVec_base_deg)

        ## Calculate amount of rotation in end effector frame
        R_step_base = Rotation.from_rotvec(rotVec_base_rad)

Lab Computer's avatar
Lab Computer committed
274
        R_step_ee = R_curr.inv() * R_step_base * R_curr;
Cadmus's avatar
Cadmus committed
275
276
277
278
279

        rotVec_ee_rad = R_step_ee.as_rotvec()
        rotVec_ee_deg = np.rad2deg(rotVec_ee_rad)

        ## Rotate as defined at end effector
Lab Computer's avatar
Lab Computer committed
280
        return self._rotate_endpoint_ee(rotVec_ee_deg[0].item(),rotVec_ee_deg[1].item(),rotVec_ee_deg[2].item())
Cadmus's avatar
Cadmus committed
281
282


283
    def _rotate_endpoint_ee(self,dx_deg,dy_deg,dz_deg):
Cadmus's avatar
Cadmus committed
284
285
286
287
288
289
290
291
292
293
294
295
296
297
        """
        Rotate the endpoint of the arm from where it currently is, relative to the end effector

        Arguments as rotation vector
        See: https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector

        dx:
        dy:
        dz:
        Returns True if arm is moved, False otherwise
        """
        # Number of intermediate steps to get to the final configuration

        ## Get current pose
Lab Computer's avatar
Lab Computer committed
298
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
299
300
301

        ## Get current orientation
        # 4x
Lab Computer's avatar
Lab Computer committed
302
        quat_curr = np.array([pose_curr.orientation.x,pose_curr.orientation.y,pose_curr.orientation.z,pose_curr.orientation.w])
Cadmus's avatar
Cadmus committed
303

Lab Computer's avatar
Lab Computer committed
304
        R_curr = Rotation.from_quat(quat_curr)
Cadmus's avatar
Cadmus committed
305
306
307

        ## Get step rotation
        # 3x
308
        rotVec_deg = np.array([dx_deg,dy_deg,dz_deg])
Cadmus's avatar
Cadmus committed
309
310
311
312
313
        rotVec_rad = np.deg2rad(rotVec_deg)

        ## Calculate intermediate shift orientation
        R_step = Rotation.from_rotvec(rotVec_rad)

Lab Computer's avatar
Lab Computer committed
314
        R_next = R_curr * R_step;
Cadmus's avatar
Cadmus committed
315

Lab Computer's avatar
Lab Computer committed
316
317
        quat_next = R_next.as_quat()
        pos_next = np.array([pose_curr.position.x,pose_curr.position.y,pose_curr.position.z])
Cadmus's avatar
Cadmus committed
318
319

        ## Convert to joint angles
Lab Computer's avatar
Lab Computer committed
320
321
        conf_next = self.ik(pos_next,quat_next)
        if conf_next is None:
Cadmus's avatar
Cadmus committed
322
323
324
325
326
327
            return False

        ## Visualise plan to confirm
        # TODO

        ## Execute plan
Lab Computer's avatar
Lab Computer committed
328
329
        success = self.move_to(conf_next,
            near_deg=2*np.linalg.norm(rotVec_deg))
Cadmus's avatar
Cadmus committed
330
331
332
333

        return success


334
    def _move_callback(self,key,configurations,params):
Cadmus's avatar
Cadmus committed
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
        """
        Callback function to operate the robot arm.

        Relative to robot base:
        - the robot can be translated in the:
            - XY plane with WASD
            - Z direction with left shift and left ctrl
        - the robot can be rotated in the:
            - XY directions with IJKL
            - Z direction with U and O

        Relative to end effector:
        - the robot can be translated in the:
            - XY plane with arrow keys
            - Z direction with right shift and right ctrl
        - the robot can be rotated in the:
            - XY directions with the 8456 keys (numpad)
            - Z direction with 7 and 9

        The -/_ and =/+ keys will adjust the amount to translate and rotate.

356
357
        The page up/down keys will retract and load filament from the print tip.

Cadmus's avatar
Cadmus committed
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
        Enter key will save the joint angles and pose.
        Esc key will stop the gathering of configurations.
        Home key will move the robot to the home configuration.
        """
        if DEBUG:
            print('Controller::_move_callback()')

        step_m = params['step_m']
        step_deg = min(params['step_deg'],45)

        ## Base frame controls
        # Translation
        if key in (keyboard.KeyCode.from_char('w'),keyboard.KeyCode.from_char('W')):
            if VERBOSE:
                print('Step {} mm in y-direction wrt robot base'.format(-step_m*1000))
373
            self._translate_endpoint_base(0,-step_m,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
374
        elif key in (keyboard.KeyCode.from_char('s'),keyboard.KeyCode.from_char('S')):
Cadmus's avatar
Cadmus committed
375
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
376
                print('Step {} mm in y-direction wrt robot base'.format(step_m*1000))
377
            self._translate_endpoint_base(0,step_m,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
378
        elif key in (keyboard.KeyCode.from_char('a'),keyboard.KeyCode.from_char('A')):
Cadmus's avatar
Cadmus committed
379
380
            if VERBOSE:
                print('Step {} mm in x-direction wrt robot base'.format(step_m*1000))
381
            self._translate_endpoint_base(step_m,0,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
382
383
384
        elif key in (keyboard.KeyCode.from_char('d'),keyboard.KeyCode.from_char('D')):
            if VERBOSE:
                print('Step {} mm in x-direction wrt robot base'.format(-step_m*1000))
385
            self._translate_endpoint_base(-step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
386
387
388
389

        elif key == keyboard.Key.shift_l:
            if VERBOSE:
                print('Step {} mm in z-direction wrt robot base'.format(step_m*1000))
390
            self._translate_endpoint_base(0,0,step_m,step_m/100)
Cadmus's avatar
Cadmus committed
391
392
393
        elif key == keyboard.Key.ctrl_l:
            if VERBOSE:
                print('Step {} mm in z-direction wrt robot base'.format(-step_m*1000))
394
            self._translate_endpoint_base(0,0,-step_m,step_m/100)
Cadmus's avatar
Cadmus committed
395
396
397
398
399
400

        # Rotation
        elif key in (keyboard.KeyCode.from_char('i'),keyboard.KeyCode.from_char('I')):
            if VERBOSE:
                print('Rotate {} deg in the x-direction wrt robot base'.format(step_deg))
            self._rotate_endpoint_base(step_deg,0,0)
Lab Computer's avatar
Lab Computer committed
401
        elif key in (keyboard.KeyCode.from_char('k'),keyboard.KeyCode.from_char('K')):
Cadmus's avatar
Cadmus committed
402
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
403
404
405
                print('Rotate {} deg in the x-direction wrt robot base'.format(-step_deg))
            self._rotate_endpoint_base(-step_deg,0,0)
        elif key in (keyboard.KeyCode.from_char('j'),keyboard.KeyCode.from_char('J')):
Cadmus's avatar
Cadmus committed
406
407
408
            if VERBOSE:
                print('Rotate {} deg in the y-direction wrt robot base'.format(step_deg))
            self._rotate_endpoint_base(0,step_deg,0)
Lab Computer's avatar
Lab Computer committed
409
410
411
412
        elif key in (keyboard.KeyCode.from_char('l'),keyboard.KeyCode.from_char('L')):
            if VERBOSE:
                print('Rotate {} deg in the y-direction wrt robot base'.format(-step_deg))
            self._rotate_endpoint_base(0,-step_deg,0)
Cadmus's avatar
Cadmus committed
413
414
415
416
417
418
419
420
421
422
423
424
425
426

        elif key in (keyboard.KeyCode.from_char('u'),keyboard.KeyCode.from_char('U')):
            if VERBOSE:
                print('Rotate {} deg in the z-direction wrt robot base'.format(step_deg))
            self._rotate_endpoint_base(0,0,step_deg)
        elif key in (keyboard.KeyCode.from_char('o'),keyboard.KeyCode.from_char('O')):
            if VERBOSE:
                print('Rotate {} deg in the z-direction wrt robot base'.format(-step_deg))
            self._rotate_endpoint_base(0,0,-step_deg)

        ## End effector frame controls
        # Translation
        elif key == keyboard.Key.up:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
427
                print('Step {} mm in z-direction wrt end effector'.format(step_m*1000))
428
            self._translate_endpoint_ee(0,0,step_m,step_m/100)
Cadmus's avatar
Cadmus committed
429
430
        elif key == keyboard.Key.down:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
431
                print('Step {} mm in z-direction wrt end effector'.format(-step_m*1000))
432
            self._translate_endpoint_ee(0,0,-step_m,step_m/100)
Cadmus's avatar
Cadmus committed
433
434
        elif key == keyboard.Key.left:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
435
                print('Step {} mm in y-direction wrt end effector'.format(-step_m*1000))
436
            self._translate_endpoint_ee(0,step_m,0,step_m/100)
Cadmus's avatar
Cadmus committed
437
438
        elif key == keyboard.Key.right:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
439
                print('Step {} mm in y-direction wrt end effector'.format(step_m*1000))
440
            self._translate_endpoint_ee(0,-step_m,0,step_m/100)
Cadmus's avatar
Cadmus committed
441
442
443

        elif key == keyboard.Key.shift_r:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
444
                print('Step {} mm in x-direction wrt end effector'.format(-step_m*1000))
445
            self._translate_endpoint_ee(-step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
446
447
        elif key == keyboard.Key.ctrl_r:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
448
                print('Step {} mm in x-direction wrt end effector'.format(step_m*1000))
449
            self._translate_endpoint_ee(step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492

        # Rotation
        elif key == keyboard.KeyCode.from_char('8'):
            if VERBOSE:
                print('Rotate {} deg in the y-direction wrt end effector'.format(step_deg))
            self._rotate_endpoint_ee(0,step_deg,0)
        elif key == keyboard.KeyCode.from_char('5') or (hasattr(key,'vk') and key.vk is not None and int(key.vk) == 65437):
            if VERBOSE:
                print('Rotate {} deg in the y-direction wrt end effector'.format(-step_deg))
            self._rotate_endpoint_ee(0,-step_deg,0)
        elif key == keyboard.KeyCode.from_char('4'):
            if VERBOSE:
                print('Rotate {} deg in the z-direction wrt end effector'.format(-step_deg))
            self._rotate_endpoint_ee(0,0,-step_deg)
        elif key == keyboard.KeyCode.from_char('6'):
            if VERBOSE:
                print('Rotate {} deg in the z-direction wrt end effector'.format(step_deg))
            self._rotate_endpoint_ee(0,0,step_deg)

        elif key == keyboard.KeyCode.from_char('7'):
            if VERBOSE:
                print('Rotate {} deg in the x-direction wrt end effector'.format(-step_deg))
            self._rotate_endpoint_ee(-step_deg,0,0)
        elif key == keyboard.KeyCode.from_char('9'):
            if VERBOSE:
                print('Rotate {} deg in the x-direction wrt end effector'.format(step_deg))
            self._rotate_endpoint_ee(step_deg,0,0)

        elif key in (keyboard.KeyCode.from_char('-'),keyboard.KeyCode.from_char('_')): # Decrease step resolution
            if params['step_m']/2 != 0:
                params['step_m'] = params['step_m']/2
            if params['step_deg']/2 != 0:
                params['step_deg'] = params['step_deg']/2
            if VERBOSE:
                print('Decreased step size to {} mm/{} deg'.format(params['step_m']*1000,params['step_deg']))
        elif key in (keyboard.KeyCode.from_char('+'),keyboard.KeyCode.from_char('=')): # Increase step resolution
            if params['step_m']*2 != float('inf'):
                params['step_m'] = params['step_m']*2
            if params['step_deg']*2 != float('inf'):
                params['step_deg'] = params['step_deg']*2
            if VERBOSE:
                print('Increased step size to {} mm/{} deg'.format(params['step_m']*1000,params['step_deg']))

493
494
495
496
497
498
499
500
501
        elif key == keyboard.Key.page_up:
            if VERBOSE:
                print('Retract')
            self.postprint_retract()
        elif key == keyboard.Key.page_down:
            if VERBOSE:
                print('Extrude')
            self.preprint_load()

Cadmus's avatar
Cadmus committed
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
        elif key == keyboard.Key.home:
            if VERBOSE:
                print('Go home')
            self.move_home()
        elif key == keyboard.Key.enter:
            if VERBOSE:
                print('Saving configuration')
            params['success'] = True
            configurations.append(
                (self.group.get_current_joint_values(),
                self.group.get_current_pose()))
            if DEBUG:
                print('Returning control to program')
            return False
        elif key == keyboard.Key.esc:
            if VERBOSE:
                print('Stopping keyboard controls')
            params['success'] = False
            if DEBUG:
                print('Returning control to program')
            return False
        else:
            # Do nothing for now
            pass


    def prompt_configurations(self,prompt=None):
        """
        Prompts user to control the robot to a series of configurations.
        Relative to robot base, the robot can be translated in the:
        - XY plane with WASD
        - Z direction with shift and ctrl
        The -/_ and =/+ keys will adjust the amount to translate (default is 10mm).

        Relative to the end effector, the robot can be rotated 90 degrees in the:
        - XY directions with the arrow keys
        - Z direction with Q and E

        Enter key will save the joint angles and pose.
        Esc key will SystemExit.
        Home key will move the robot to the home configuration.

        The variable prompt can take multiple types:
        - List of strings: Prompts for the user before every configuration.
          Will stop gathering when all the prompts are addressed.
        - Positive integer: Stops gathering after a number of configurations.
        - None: Will keep gathering until Esc, which will not SystemExit.

        Returns list of (list of joint angle, ROS pose message) tuples
        """
        if DEBUG:
            print('Controller::prompt_configurations()')

        configurations = []
        params = {
            'step_m': 0.016,
Lab Computer's avatar
Lab Computer committed
558
            'step_deg': 11.25,
Cadmus's avatar
Cadmus committed
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
            'success': True}

        if isinstance(prompt,list):
            for p in prompt:
                print(p)
                with keyboard.Listener(
                        on_release = lambda key:self._move_callback(key,configurations,params),
                        suppress=True) as listener:
                    listener.join()
                    if DEBUG:
                        print('Keyboard listener thread joined')
                if not params['success']:
                    sys.exit(0)
        elif isinstance(prompt,int):
            for iConfig in range(prompt):
                print('Enter configuration {}'.format(iConfig))
                with keyboard.Listener(
Lab Computer's avatar
Lab Computer committed
576
577
                        on_release=lambda key:self._move_callback(key,configurations,params),
                        suppress=True) as listener:
Cadmus's avatar
Cadmus committed
578
579
580
581
582
583
584
585
586
587
588
                    listener.join()
                    if DEBUG:
                        print('Keyboard listener thread joined')
                if not params['success']:
                    sys.exit(0)
        else:
            while params['success']:
                params['success'] = False

                print('Enter next configuration')
                with keyboard.Listener(
Lab Computer's avatar
Lab Computer committed
589
590
                        on_release=lambda key:self._move_callback(key,configurations,params),
                        suppress=True) as listener:
Cadmus's avatar
Cadmus committed
591
592
593
594
595
596
                    listener.join()

        return configurations


if __name__=="__main__":
597
    #con = KeyboardController(disableExtruder=True)
598
599
600
    #con = KeyboardController()
    #con1 = KeyboardController(robot=1)
    con2 = KeyboardController(robot=2)
Cadmus's avatar
Cadmus committed
601
602

    print('Collecting configrations...')
603
    #configuations = con.prompt_configurations()
Jayant Khatkar's avatar
Jayant Khatkar committed
604
    #configuations = con1.prompt_configurations()
605
    configuations = con2.prompt_configurations()
Cadmus's avatar
Cadmus committed
606
607
    print('Your configurations:')
    print(configuations)