KeyboardController.py 26 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
VERBOSE = True

## Python libraries
from copy import deepcopy
7
import time
Cadmus's avatar
Cadmus committed
8
9
10
import sys

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

Cadmus's avatar
Cadmus committed
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
21
from geometry_msgs.msg import Pose
22
23
from moveit_msgs.srv import GetPositionFK,GetPositionFKRequest
from moveit_msgs.srv import GetPositionIK,GetPositionIKRequest
Cadmus's avatar
Cadmus committed
24
from moveit_msgs.msg import MoveItErrorCodes
25
from moveit_msgs.msg import DisplayTrajectory
Cadmus's avatar
Cadmus committed
26
27

## Our libraries
28
from Controller import Controller,getR_ET
29
from utils import speed_multiplier
Cadmus's avatar
Cadmus committed
30
31


32
33
34
35
36
37
38
39
# used for fast-calibration
def getTransformMat(quat, pos):
    T = np.identity(4)
    T[:3,:3] = Rotation.from_quat(quat).as_dcm()
    T[:3,3] = pos
    return np.linalg.inv(T)

# also used for fast-calibration-ik
40
_vert_or = [np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
41
42
                     [ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
                     [-9.99999404e-01, -8.34294187e-04,  6.73821778e-04,  2.74792069e-06],
43
44
45
46
47
                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]]),
            np.array([[-6.74517010e-04, -8.33315658e-04,  9.99999404e-01, -2.90597873e-05], # 2nd arm
                     [ 8.33732192e-04,  9.99999285e-01,  8.33877944e-04, -4.81068309e-05],
                     [-9.99999404e-01,  8.34294187e-04, -6.73821778e-04,  2.74792069e-06],
                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])]
48
49


Cadmus's avatar
Cadmus committed
50
51
52
53
54
55
56
57
class KeyboardController(Controller):
    """
    Controller to control robot arms by keyboard.

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

    def __init__(self,
58
        homeConf_deg=np.array([0,-90,45,-45,-90,-45]),
59
        waitForExtruder=True,
60
        timeScale=1.0,
61
62
        disableExtruder=False,
        robot=0):
Cadmus's avatar
Cadmus committed
63
64
        """
        """
65
66
67
        if DEBUG:
            print("Creating Controller")

Lab Computer's avatar
Lab Computer committed
68
        super(KeyboardController,self).__init__(
69
            timeScale=timeScale,
70
            blocking=waitForExtruder,
71
72
            disable_extruder=disableExtruder,
            robot=robot)
Cadmus's avatar
Cadmus committed
73
74
75
76
77
78

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


79
    def fk(self,conf):
Cadmus's avatar
Cadmus committed
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
        """
        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:
96
            moveitFK = rospy.ServiceProxy('compute_fk',GetPositionFK)
Cadmus's avatar
Cadmus committed
97
98
99
100
101
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s"%e)

        # Form service request
        req = GetPositionFKRequest()
Lab Computer's avatar
Lab Computer committed
102
        req.header.frame_id = 'base_link'
Cadmus's avatar
Cadmus committed
103
104
        req.fk_link_names = ['ee_link']
        req.robot_state.joint_state.name = self.group.get_active_joints()
Lab Computer's avatar
Lab Computer committed
105
106
107
108
109
110
        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
111

Lab Computer's avatar
Lab Computer committed
112
113
114
115
        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
116
117


118
    def ik(self,pos_RE,quat_RE,conf_hint = None):
Cadmus's avatar
Cadmus committed
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
        """
        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
137
            conf_hint = np.array(self.group.get_current_joint_values())
Cadmus's avatar
Cadmus committed
138
139
140

        rospy.wait_for_service('compute_ik')
        try:
141
            moveitIK = rospy.ServiceProxy('compute_ik',GetPositionIK)
Cadmus's avatar
Cadmus committed
142
143
144
        except rospy.ServiceException as e:
            rospy.logerr("Service call failed: %s"%e)

Lab Computer's avatar
Lab Computer committed
145
146

        name_conf = self.group.get_active_joints()
Cadmus's avatar
Cadmus committed
147
148
149
150

        req = GetPositionIKRequest()
        req.ik_request.ik_link_name = 'ee_link'
        req.ik_request.group_name = 'manipulator'
Lab Computer's avatar
Lab Computer committed
151
        req.ik_request.robot_state.joint_state.name = name_conf
Cadmus's avatar
Cadmus committed
152
        req.ik_request.robot_state.joint_state.position = conf_hint.tolist()
Lab Computer's avatar
Lab Computer committed
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
        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
170

Lab Computer's avatar
Lab Computer committed
171
        return conf
Cadmus's avatar
Cadmus committed
172
173
174
175
176
177
178


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


182
    def _translate_endpoint_base(self,dx_m,dy_m,dz_m,resolution_m=0.01):
Cadmus's avatar
Cadmus committed
183
184
185
186
        """
        Translate the endpoint of the arm from where it currently is,
        relative to the robot base.

187
188
189
        dx_m:
        dy_m:
        dz_m:
Cadmus's avatar
Cadmus committed
190
191
192
        resolution_m: Largest end effector distance between two configurations
        """
        ## Get current pose
Lab Computer's avatar
Lab Computer committed
193
194
        pose_curr = self.group.get_current_pose().pose
        pose_next = deepcopy(pose_curr)
Cadmus's avatar
Cadmus committed
195
196

        ## Shift pose
Lab Computer's avatar
Lab Computer committed
197
198
199
        pose_next.position.x += dx_m
        pose_next.position.y += dy_m
        pose_next.position.z += dz_m
Cadmus's avatar
Cadmus committed
200
201

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

        # Create new plan with duplicates removed
Lab Computer's avatar
Lab Computer committed
206
207
        cleanTraj = deepcopy(plan.joint_trajectory)
        cleanTraj.points = []
Cadmus's avatar
Cadmus committed
208
209
210
211

        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
212
                cleanTraj.points.append(point)
Cadmus's avatar
Cadmus committed
213
214
215
                prevTime = point.time_from_start

        ## Execute plan
Lab Computer's avatar
Lab Computer committed
216
        self.exec_traj(cleanTraj)
Cadmus's avatar
Cadmus committed
217
218


219
    def _translate_endpoint_ee(self,dx_m,dy_m,dz_m,resolution_m=0.01):
Cadmus's avatar
Cadmus committed
220
221
222
223
        """
        Translate the endpoint of the arm from where it currently is,
        relative to the end effector.

224
225
226
        dx_m:
        dy_m:
        dz_m:
Cadmus's avatar
Cadmus committed
227
228
229
        resolution_m: Largest end effector distance between two configurations
        """
        ## Get current pose
Lab Computer's avatar
Lab Computer committed
230
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
231
232
233

        ## Shift pose
        # Rotate translation to end effector frame
234
        trans_ee = np.array([dx_m,dy_m,dz_m])
Lab Computer's avatar
Lab Computer committed
235
236
        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)
237
        trans_base = R_curr.apply(trans_ee)
Cadmus's avatar
Cadmus committed
238

239
240
        # 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
241
242


243
244
245
246
247
    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.

248
249
250
        dx_m:
        dy_m:
        dz_m:
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
        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
266
267
268
269
270
271
272
273
274
275
276
277
278
        """
        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
279
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
280
281
282

        ## Get current orientation
        # 4x
Lab Computer's avatar
Lab Computer committed
283
        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
284

Lab Computer's avatar
Lab Computer committed
285
        R_curr = Rotation.from_quat(quat_curr)
Cadmus's avatar
Cadmus committed
286
287
288

        ## Get step rotation
        # 3x
289
        rotVec_base_deg = np.array([dx_deg,dy_deg,dz_deg])
Cadmus's avatar
Cadmus committed
290
291
292
293
294
        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
295
        R_step_ee = R_curr.inv() * R_step_base * R_curr;
Cadmus's avatar
Cadmus committed
296
297
298
299
300

        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
301
        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
302
303


304
    def _rotate_endpoint_ee(self,dx_deg,dy_deg,dz_deg):
Cadmus's avatar
Cadmus committed
305
306
307
308
309
310
311
312
313
314
315
316
317
318
        """
        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
319
        pose_curr = self.group.get_current_pose().pose
Cadmus's avatar
Cadmus committed
320
321
322

        ## Get current orientation
        # 4x
Lab Computer's avatar
Lab Computer committed
323
        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
324

Lab Computer's avatar
Lab Computer committed
325
        R_curr = Rotation.from_quat(quat_curr)
Cadmus's avatar
Cadmus committed
326
327
328

        ## Get step rotation
        # 3x
329
        rotVec_deg = np.array([dx_deg,dy_deg,dz_deg])
Cadmus's avatar
Cadmus committed
330
331
332
333
334
        rotVec_rad = np.deg2rad(rotVec_deg)

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

Lab Computer's avatar
Lab Computer committed
335
        R_next = R_curr * R_step;
Cadmus's avatar
Cadmus committed
336

Lab Computer's avatar
Lab Computer committed
337
338
        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
339
340

        ## Convert to joint angles
Lab Computer's avatar
Lab Computer committed
341
342
        conf_next = self.ik(pos_next,quat_next)
        if conf_next is None:
Cadmus's avatar
Cadmus committed
343
344
345
346
347
348
            return False

        ## Visualise plan to confirm
        # TODO

        ## Execute plan
Lab Computer's avatar
Lab Computer committed
349
350
        success = self.move_to(conf_next,
            near_deg=2*np.linalg.norm(rotVec_deg))
Cadmus's avatar
Cadmus committed
351
352
353
354

        return success


355
    def _move_callback(self,key,configurations,params):
Cadmus's avatar
Cadmus committed
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
        """
        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.

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

Cadmus's avatar
Cadmus committed
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
        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))
394
            self._translate_endpoint_base(0,-step_m,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
395
        elif key in (keyboard.KeyCode.from_char('s'),keyboard.KeyCode.from_char('S')):
Cadmus's avatar
Cadmus committed
396
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
397
                print('Step {} mm in y-direction wrt robot base'.format(step_m*1000))
398
            self._translate_endpoint_base(0,step_m,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
399
        elif key in (keyboard.KeyCode.from_char('a'),keyboard.KeyCode.from_char('A')):
Cadmus's avatar
Cadmus committed
400
401
            if VERBOSE:
                print('Step {} mm in x-direction wrt robot base'.format(step_m*1000))
402
            self._translate_endpoint_base(step_m,0,0,step_m/100)
Lab Computer's avatar
Lab Computer committed
403
404
405
        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))
406
            self._translate_endpoint_base(-step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
407
408
409
410

        elif key == keyboard.Key.shift_l:
            if VERBOSE:
                print('Step {} mm in z-direction wrt robot base'.format(step_m*1000))
411
            self._translate_endpoint_base(0,0,step_m,step_m/100)
Cadmus's avatar
Cadmus committed
412
413
414
        elif key == keyboard.Key.ctrl_l:
            if VERBOSE:
                print('Step {} mm in z-direction wrt robot base'.format(-step_m*1000))
415
            self._translate_endpoint_base(0,0,-step_m,step_m/100)
Cadmus's avatar
Cadmus committed
416
417
418
419
420
421

        # 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
422
        elif key in (keyboard.KeyCode.from_char('k'),keyboard.KeyCode.from_char('K')):
Cadmus's avatar
Cadmus committed
423
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
424
425
426
                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
427
428
429
            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
430
431
432
433
        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
434
435
436
437
438
439
440
441
442
443
444
445
446
447

        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
448
                print('Step {} mm in z-direction wrt end effector'.format(step_m*1000))
449
            self._translate_endpoint_ee(0,0,step_m,step_m/100)
Cadmus's avatar
Cadmus committed
450
451
        elif key == keyboard.Key.down:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
452
                print('Step {} mm in z-direction wrt end effector'.format(-step_m*1000))
453
            self._translate_endpoint_ee(0,0,-step_m,step_m/100)
Cadmus's avatar
Cadmus committed
454
455
        elif key == keyboard.Key.left:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
456
                print('Step {} mm in y-direction wrt end effector'.format(-step_m*1000))
457
            self._translate_endpoint_ee(0,step_m,0,step_m/100)
Cadmus's avatar
Cadmus committed
458
459
        elif key == keyboard.Key.right:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
460
                print('Step {} mm in y-direction wrt end effector'.format(step_m*1000))
461
            self._translate_endpoint_ee(0,-step_m,0,step_m/100)
Cadmus's avatar
Cadmus committed
462
463
464

        elif key == keyboard.Key.shift_r:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
465
                print('Step {} mm in x-direction wrt end effector'.format(-step_m*1000))
466
            self._translate_endpoint_ee(-step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
467
468
        elif key == keyboard.Key.ctrl_r:
            if VERBOSE:
Lab Computer's avatar
Lab Computer committed
469
                print('Step {} mm in x-direction wrt end effector'.format(step_m*1000))
470
            self._translate_endpoint_ee(step_m,0,0,step_m/100)
Cadmus's avatar
Cadmus committed
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513

        # 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']))

514
515
516
517
518
519
520
521
522
        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
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
        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


549
    def prompt_configurations(self, prompt=None, prior_calibs=None, arm=0):
Cadmus's avatar
Cadmus committed
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
        """
        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
579
            'step_deg': 11.25,
Cadmus's avatar
Cadmus committed
580
581
            'success': True}

582
583
584
585
586
587
588
589
590
591
        # if previous calib data, create transformation matrix to calc approx IKs
        if prior_calibs is not None and len(prior_calibs)>0:
            world2armT = getTransformMat(prior_calibs['quat_WR'], prior_calibs['pos_WR'])
            tip2armT = getTransformMat([0,0,0,1], prior_calibs['pos_ET'])
            priors = True
        else:
            priors = False

        if isinstance(prompt, list):

Cadmus's avatar
Cadmus committed
592
            for p in prompt:
593
594
595
596
597
                print('Move robot tip to {}'.format(str(p)))

                if priors:
                    # calculate IK for next point and move there
                    mat = np.identity(4)
598
                    #p[2] += 0.01 # go 1cm above bed for safety
599
                    mat[:3,3] = p
600
                    mat = world2armT.dot(mat).dot(_vert_or[arm]).dot(tip2armT)
601
602
603
604
605
606
                    q = Rotation.from_dcm(mat[:3,:3]).as_quat()
                    p_r = mat[0:3,3]
                    gpose = Pose()
                    gpose.position.x, gpose.position.y, gpose.position.z = p_r
                    gpose.orientation.x, gpose.orientation.y, \
                            gpose.orientation.z, gpose.orientation.w = q
607

608
609
610
611
612
613
614
615
616
617
618
619
                    # check angle diff from current pos
                    cur_pose = self.group.get_current_pose().pose
                    orientation_diff = sum([
                        (cur_pose.orientation.x - gpose.orientation.x)**2,
                        (cur_pose.orientation.y - gpose.orientation.y)**2,
                        (cur_pose.orientation.z - gpose.orientation.z)**2,
                        (cur_pose.orientation.w - gpose.orientation.w)**2])**0.5

                    # go home in between if significant angle diff from vertical
                    if orientation_diff > 0.1:
                        self.move_home()

620
621
622
623
624
625
                    # compute path to next config
                    waypoints = [self.group.get_current_pose().pose, gpose]
                    plan, fraction = self.group.compute_cartesian_path(
                            waypoints,
                            0.01, # ee_step
                            0)
626
627
                    speed_multiplier(plan.joint_trajectory, 4) # 4x slower
                    self.group.execute(plan, wait=True)
628

Cadmus's avatar
Cadmus committed
629
630
631
632
633
634
                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')
635

Cadmus's avatar
Cadmus committed
636
637
                if not params['success']:
                    sys.exit(0)
638
639

        elif isinstance(prompt, int):
Cadmus's avatar
Cadmus committed
640
641
642
            for iConfig in range(prompt):
                print('Enter configuration {}'.format(iConfig))
                with keyboard.Listener(
Lab Computer's avatar
Lab Computer committed
643
644
                        on_release=lambda key:self._move_callback(key,configurations,params),
                        suppress=True) as listener:
Cadmus's avatar
Cadmus committed
645
646
647
648
649
                    listener.join()
                    if DEBUG:
                        print('Keyboard listener thread joined')
                if not params['success']:
                    sys.exit(0)
650

Cadmus's avatar
Cadmus committed
651
652
653
654
655
656
        else:
            while params['success']:
                params['success'] = False

                print('Enter next configuration')
                with keyboard.Listener(
Lab Computer's avatar
Lab Computer committed
657
658
                        on_release=lambda key:self._move_callback(key,configurations,params),
                        suppress=True) as listener:
Cadmus's avatar
Cadmus committed
659
660
661
662
663
664
                    listener.join()

        return configurations


if __name__=="__main__":
665
    #con = KeyboardController(disableExtruder=True)
666
    #con = KeyboardController()
667
668
    con1 = KeyboardController(robot=1)
    con2 = KeyboardController(robot=2, disableExtruder=True)
Cadmus's avatar
Cadmus committed
669
670

    print('Collecting configrations...')
671
672
673
    #configuations = con.prompt_configurations()
    configuations = con1.prompt_configurations()
    configuations = con2.prompt_configurations()
Cadmus's avatar
Cadmus committed
674
675
    print('Your configurations:')
    print(configuations)