MAGIKS  1.1
Manipulator General Inverse Kinematic Solver
 All Classes Namespaces Files Functions Variables Pages
writer_pr2.py
Go to the documentation of this file.
1 ## @file writer_pr2.py
2 # @brief Contains a class inherited from Skilled_PR2 in skilled_pr2.py which is connected to a real-time robot,
3 # a real PR2 or PR2 in simulation that have special writing skills
4 # @author Nima Ramezani Taghiabadi
5 #
6 # PhD Researcher
7 # Faculty of Engineering and Information Technology
8 # University of Technology Sydney (UTS)
9 # Broadway, Ultimo, NSW 2007, Australia
10 # Phone No. : 04 5027 4611
11 # Email(1) : nima.ramezani@gmail.com
12 # Email(2) : Nima.RamezaniTaghiabadi@uts.edu.au
13 # @version 2.0
14 #
15 # Start date: 21 November 2014
16 # Last Revision: 06 January 2015
17 # \b Attention:
18 # All actions of this module do not have any collision avoidance feature. So be prepared to stop the robot if any collision happens !
19 
20 import numpy as np
21 import time, math
22 
23 import skilled_pr2 as spr
24 import pyride_interpreter as pint
25 
26 from math_tools import general_math as gen
27 from math_tools.geometry import trajectory as traj, rotation as rot, shape_trajectories as tsh
28 
29 '''
30 import sys
31 sys.path.append()
32 '''
33 
34 '''
35 writing posture:
36 array([ -1.26299826e-01, 1.77046412e+00, -1.02862191e+00,
37  -1.36864905e+00, -2.31195189e+00, -1.29253137e+00,
38  1.71195615e-01, 8.02176174e-01, -2.12167293e-03,
39  2.32811863e-04, 7.05358701e-03, 4.01010384e-01,
40  2.44565260e+00, 7.10476515e-01, -1.30808585e+00,
41  1.15357810e+00, -4.49485156e-01, -2.46943329e+00])
42 '''
43 class Writer_PR2(spr.Skilled_PR2):
44  def __init__(self):
45  super(Writer_PR2, self).__init__()
46  self.height = 0.05
47  self.width = 0.02
48  self.arm_speed = 0.01
49 
50  self.board_offset = np.array([ 0.092, 0.2 , 0.1])
51  self.larm_reference = True
52 
53  def write_A(self):
54  arm = self.reference_arm()
55  ori = arm.wrist_orientation()
56  self.arm_forward(self.depth, relative = True)
57  self.arm_right_up(dx = 0.5*self.width, dy = self.height, relative = True)
58  self.arm_right_down(dx = 0.5*self.width, dy = self.height, relative = True)
59 
60  self.arm_back(self.depth, relative = True)
61  self.arm_left_up(0.75*self.width, 0.5*self.height, relative = True)
62  self.arm_forward(self.depth, relative = True)
63  self.arm_right(0.5*self.width, relative = True)
64  self.arm_back(self.depth, relative = True)
65  self.arm_right_down(0.6*self.width, 0.5*self.height, relative = True)
66 
67  def write_B(self):
68  arm = self.reference_arm()
69  ori = arm.wrist_orientation()
70  self.arm_forward(self.depth, relative=True)
71  B = tsh.B(height = self.height, direction = ori)
72  self.arm_trajectory(B)
73  self.arm_back(self.depth, relative=True)
74  self.arm_right(0.25*self.height + 0.3*self.width, relative=True)
75 
76  def write_C(self):
77  arm = self.reference_arm()
78  ori = arm.wrist_orientation()
79  self.arm_right_up(dy = (self.height- self.width/2) , dx = self.width, relative=True)
80  self.arm_forward(self.depth, relative=True)
81  C = tsh.C(height = self.height, width = self.width, direction = ori, adjust = False)
82  self.arm_trajectory(C)
83  self.arm_back(self.depth, relative=True)
84  self.arm_right_down(dy = self.width/2 , dx = 0.3*self.width, relative=True)
85 
86  def write_D(self):
87  arm = self.reference_arm()
88  ori = arm.wrist_orientation()
89  self.arm_forward(self.depth, relative=True)
90  D = tsh.D(height = self.height, width = self.width, direction = ori)
91  self.arm_trajectory(D)
92  self.arm_back(self.depth, relative=True)
93  self.arm_right(1.3*self.width, relative=True)
94 
95  def write_E(self):
96  arm = self.reference_arm()
97  ori = arm.wrist_orientation()
98  self.arm_right(self.width, relative=True)
99  self.arm_forward(self.depth, relative=True)
100  self.arm_left(self.width, relative=True)
101  self.arm_up(self.height, relative=True)
102  self.arm_right(self.width, relative=True)
103  self.arm_back(self.depth, relative=True)
104  self.arm_left_down(self.width, 0.5*self.height, relative=True)
105  self.arm_forward(self.depth, relative=True)
106  self.arm_right(0.8*self.width, relative=True)
107  self.arm_back(self.depth, relative=True)
108  self.arm_right_down(dx = 0.6*self.width, dy = 0.5*self.height, relative = True)
109 
110  def write_F(self):
111  arm = self.reference_arm()
112  ori = arm.wrist_orientation()
113  self.arm_forward(self.depth, relative=True)
114  self.arm_up(self.height, relative=True)
115  self.arm_right(self.width, relative=True)
116  self.arm_back(self.depth, relative=True)
117  self.arm_left_down(self.width, 0.5*self.height, relative=True)
118  self.arm_forward(self.depth, relative=True)
119  self.arm_right(0.8*self.width, relative=True)
120  self.arm_back(self.depth, relative=True)
121  self.arm_right_down(dx = 0.6*self.width, dy = 0.5*self.height, relative = True)
122 
123  def write_G(self):
124  arm = self.reference_arm()
125  ori = arm.wrist_orientation()
126  self.arm_right_up(dx = self.width, dy = self.height- self.width/2, relative=True)
127  self.arm_forward(self.depth, relative=True)
128  G = tsh.G(height = self.height, width = self.width, direction = ori, adjust = False)
129  self.arm_trajectory(G)
130  '''
131  self.arm_back(self.depth, relative=True)
132  self.arm_left(dx= 0.25*self.height, relative=True)
133  self.arm_forward(self.depth, relative=True)
134  self.arm_right(dx= 0.4*self.height, relative=True)
135  '''
136  self.arm_back(self.depth, relative=True)
137  self.arm_right_down(dx= 0.55*self.width, dy=self.height/2.0, relative=True)
138 
139  def write_H(self):
140  arm = self.reference_arm()
141  ori = arm.wrist_orientation()
142  self.arm_forward(self.depth, relative=True)
143  self.arm_up(self.height, relative=True)
144  self.arm_back(self.depth, relative=True)
145  self.arm_down(0.5*self.height, relative=True)
146  self.arm_forward(self.depth, relative=True)
147  self.arm_right(self.width, relative=True)
148  self.arm_back(self.depth, relative=True)
149  self.arm_up(0.5*self.height, relative=True)
150  self.arm_forward(self.depth, relative=True)
151  self.arm_down(self.height, relative=True)
152  self.arm_back(self.depth, relative=True)
153  self.arm_right(0.3*self.width, relative=True)
154 
155  def write_I(self):
156  arm = self.reference_arm()
157  ori = arm.wrist_orientation()
158  self.arm_forward(self.depth, relative=True)
159  self.arm_right(0.6*self.width, relative=True)
160  self.arm_back(self.depth, relative=True)
161  self.arm_left(0.3*self.width, relative=True)
162  self.arm_forward(self.depth, relative=True)
163  self.arm_up(self.height, relative=True)
164  self.arm_back(self.depth, relative=True)
165  self.arm_left(0.3*self.width, relative=True)
166  self.arm_forward(self.depth, relative=True)
167  self.arm_right(0.6*self.width, relative=True)
168  self.arm_back(self.depth, relative=True)
169  self.arm_right_down(0.3*self.width, self.height, relative=True)
170 
171  def write_J(self):
172  arm = self.reference_arm()
173  ori = arm.wrist_orientation()
174  self.arm_up(self.width/2, relative=True)
175  self.arm_forward(self.depth, relative=True)
176  J = tsh.J(width = self.width, height = self.height, direction = ori)
177  self.arm_trajectory(J)
178  self.arm_back(self.depth, relative=True)
179  self.arm_right_down(dy=self.height, dx = 0.4*self.width, relative=True)
180 
181  def write_U(self):
182  arm = self.reference_arm()
183  ori = arm.wrist_orientation()
184  self.arm_up(self.height, relative=True)
185  self.arm_forward(self.depth, relative=True)
186  U = tsh.U(width = self.width, height = self.height, direction = ori)
187  self.arm_trajectory(U)
188  self.arm_back(self.depth, relative=True)
189  self.arm_right_down(dy=self.height, dx = 0.4*self.width, relative=True)
190 
191  def write_K(self):
192  arm = self.reference_arm()
193  ori = arm.wrist_orientation()
194  self.arm_forward(self.depth, relative=True)
195  self.arm_up(self.height, relative=True)
196  self.arm_back(self.depth, relative=True)
197  self.arm_right(self.width, relative=True)
198  self.arm_forward(self.depth, relative=True)
199  self.arm_left_down(self.width, self.height/2, relative=True)
200  self.arm_right_down(self.width, self.height/2, relative=True)
201  self.arm_back(self.depth, relative=True)
202  self.arm_right(0.3*self.width, relative=True)
203 
204  def write_L(self):
205  arm = self.reference_arm()
206  ori = arm.wrist_orientation()
207  self.arm_up(self.height, relative=True)
208  self.arm_forward(self.depth, relative=True)
209  self.arm_down(self.height, relative=True)
210  self.arm_right(self.width, relative=True)
211  self.arm_back(self.depth, relative=True)
212  self.arm_right(0.3*self.width, relative=True)
213 
214  def write_M(self):
215  arm = self.reference_arm()
216  ori = arm.wrist_orientation()
217  self.arm_forward(self.depth, relative=True)
218  self.arm_up(self.height, relative=True)
219  self.arm_right_down(dx = 0.6*self.width, dy = 0.5*self.height, relative=True)
220  self.arm_right_up( dx = 0.6*self.width, dy = 0.5*self.height, relative=True)
221  self.arm_down(self.height, relative=True)
222  self.arm_back(self.depth, relative=True)
223  self.arm_right(0.3*self.width, relative=True)
224 
225  def write_N(self):
226  arm = self.reference_arm()
227  ori = arm.wrist_orientation()
228  self.arm_forward(self.depth, relative=True)
229  N = tsh.N(width = self.width, height = self.height, direction = ori)
230  self.arm_trajectory(N)
231  self.arm_back(self.depth, relative=True)
232  self.arm_right_down(dy=self.height, dx=0.3*self.width, relative=True)
233 
234  def write_O(self):
235  arm = self.reference_arm()
236  ori = arm.wrist_orientation()
237  self.arm_up(self.height - self.width/2, relative=True)
238  self.arm_forward(self.depth, relative=True)
239  O = tsh.O(height = self.height, width = self.width, direction = ori, adjust = False)
240  self.arm_trajectory(O)
241  self.arm_back(self.depth, relative=True)
242  self.arm_right_down(dx= 1.2*self.width, dy = self.height - self.width/2, relative=True)
243 
244  def write_P(self):
245  arm = self.reference_arm()
246  ori = arm.wrist_orientation()
247  self.arm_forward(self.depth, relative=True)
248  P = tsh.P(width = 0.4*self.height, height = self.height, direction = ori)
249  self.arm_trajectory(P)
250  self.arm_back(self.depth, relative=True)
251  self.arm_right_down(dy=0.5*self.height, dx=0.25*self.height + 0.3*self.width, relative=True)
252 
253  def write_Q(self):
254  arm = self.reference_arm()
255  ori = arm.wrist_orientation()
256  self.arm_up(self.height - self.width/2, relative=True)
257  self.arm_forward(self.depth, relative=True)
258  O = tsh.O(height = self.height, width = self.width, direction = ori, adjust = False)
259  self.arm_trajectory(O)
260  self.arm_back(self.depth, relative=True)
261  self.arm_right_down(dx= 0.3*self.height, dy = 0.6*self.height, relative=True)
262  self.arm_forward(self.depth, relative=True)
263  self.arm_right_down(dx= 0.3*self.height, dy = 0.1*self.height, relative=True)
264  self.arm_back(self.depth, relative=True)
265  self.arm_right_down(dx= 0.3*self.width, dy = - self.width/2 + 0.3*self.height, relative=True)
266 
267  def write_R(self):
268  arm = self.reference_arm()
269  ori = arm.wrist_orientation()
270  self.arm_forward(self.depth, relative=True)
271  P = tsh.P(width = 0.4*self.height, height = self.height, direction = ori)
272  self.arm_trajectory(P)
273  self.arm_right_down(dy=0.5*self.height, dx=0.25*self.height, relative=True)
274  self.arm_back(self.depth, relative=True)
275  self.arm_right(dx=0.3*self.width, relative=True)
276 
277  def write_S(self):
278  cth = math.cos(math.pi/3)
279  sth = math.sin(math.pi/3)
280  arm = self.reference_arm()
281  ori = arm.wrist_orientation()
282 
283  self.arm_right_up(dy = 0.25*self.height*(3+cth), dx = 0.4*self.width + 0.25*self.height*(1+sth), relative=True)
284  self.arm_forward(self.depth, relative=True)
285  S = tsh.S(height = self.height, direction = ori)
286  self.arm_trajectory(S)
287  self.arm_back(self.depth, relative=True)
288  self.arm_right_down(dy = 0.25*self.height*(1-cth), dx = 0.4*self.width + 0.6*self.width*(1+sth), relative=True)
289 
290  def write_T(self):
291  arm = self.reference_arm()
292  ori = arm.wrist_orientation()
293  self.arm_right(self.width/2, relative=True)
294  self.arm_forward(self.depth, relative=True)
295  self.arm_up(self.height, relative=True)
296  self.arm_back(self.depth, relative=True)
297  self.arm_left(self.width/2, relative=True)
298  self.arm_forward(self.depth, relative=True)
299  self.arm_right(self.width, relative=True)
300  self.arm_back(self.depth, relative=True)
301  self.arm_right_down(0.3*self.width, self.height, relative=True)
302 
303  def write_V(self):
304  arm = self.reference_arm()
305  ori = arm.wrist_orientation()
306  self.arm_up(self.height, relative=True)
307  self.arm_forward(self.depth, relative=True)
308  self.arm_right_down(0.5*self.width, self.height, relative=True)
309  self.arm_right_up(0.5*self.width, self.height, relative=True)
310  self.arm_back(self.depth, relative=True)
311  self.arm_right_down(0.3*self.width, self.height, relative=True)
312 
313  def write_W(self):
314  arm = self.reference_arm()
315  ori = arm.wrist_orientation()
316  self.arm_up(self.height, relative=True)
317  self.arm_forward(self.depth, relative=True)
318  W = tsh.W(width = 0.4*self.height, height = self.height, direction = ori)
319  self.arm_trajectory(W)
320  self.arm_back(self.depth, relative=True)
321  self.arm_right_down(dy=self.height, dx=0.3*self.width, relative=True)
322 
323  def write_X(self):
324  arm = self.reference_arm()
325  ori = arm.wrist_orientation()
326  self.arm_forward(self.depth, relative=True)
327  self.arm_right_up(self.width, self.height, relative=True)
328  self.arm_back(self.depth, relative=True)
329  self.arm_left(self.width, relative=True)
330  self.arm_forward(self.depth, relative=True)
331  self.arm_right_down(self.width, self.height, relative=True)
332  self.arm_back(self.depth, relative=True)
333  self.arm_right(0.3*self.width, relative=True)
334 
335  def write_Y(self):
336  arm = self.reference_arm()
337  ori = arm.wrist_orientation()
338  self.arm_up(self.height, relative=True)
339  self.arm_forward(self.depth, relative=True)
340  self.arm_right_down(self.width/2, self.height/2, relative=True)
341  self.arm_right_up(self.width/2, self.height/2, relative=True)
342  self.arm_back(self.depth, relative=True)
343  self.arm_left_down(self.width/2, self.height/2, relative=True)
344  self.arm_forward(self.depth, relative=True)
345  self.arm_down(self.height/2, relative=True)
346  self.arm_back(self.depth, relative=True)
347  self.arm_right(0.8*self.width, relative=True)
348 
349  def write_Z(self):
350  arm = self.reference_arm()
351  ori = arm.wrist_orientation()
352  self.arm_up(self.height, relative=True)
353  self.arm_forward(self.depth, relative=True)
354  self.arm_right(self.width, relative=True)
355  self.arm_left_down(self.width, self.height, relative=True)
356  self.arm_right(self.width, relative=True)
357  self.arm_back(self.depth, relative=True)
358  self.arm_right(0.3*self.width, relative=True)
359 
360  def write_M(self):
361  arm = self.reference_arm()
362  ori = arm.wrist_orientation()
363  self.arm_forward(self.depth, relative=True)
364  self.arm_up(self.height, relative=True)
365  self.arm_right_down(dx = 0.5*self.width, dy = 0.5*self.height, relative=True)
366  self.arm_right_up( dx = 0.5*self.width, dy = 0.5*self.height, relative=True)
367  self.arm_down(self.height, relative=True)
368  self.arm_back(self.depth, relative=True)
369  self.arm_right(0.3*self.width, relative=True)
370 
371  def write_char(self, char):
372  # ori = self.endeffector_orientation()
373  self.sync_object()
374  if char == "A":
375  self.write_A()
376  elif char == "B":
377  self.write_B()
378  elif char == "C":
379  self.write_C()
380  elif char == "D":
381  self.write_D()
382  elif char == "E":
383  self.write_E()
384  elif char == "F":
385  self.write_F()
386  elif char == "G":
387  self.write_G()
388  elif char == "H":
389  self.write_H()
390  elif char == "I":
391  self.write_I()
392  elif char == "J":
393  self.write_J()
394  elif char == "K":
395  self.write_K()
396  elif char == "L":
397  self.write_L()
398  elif char == "M":
399  self.write_M()
400  elif char == "N":
401  self.write_N()
402  elif char == "O":
403  self.write_O()
404  elif char == "P":
405  self.write_P()
406  elif char == "Q":
407  self.write_Q()
408  elif char == "R":
409  self.write_R()
410  elif char == "S":
411  self.write_S()
412  elif char == "T":
413  self.write_T()
414  elif char == "U":
415  self.write_U()
416  elif char == "V":
417  self.write_V()
418  elif char == "W":
419  self.write_W()
420  elif char == "X":
421  self.write_X()
422  elif char == "Y":
423  self.write_Y()
424  elif char == "Z":
425  self.write_Z()
426  elif char == " ":
427  self.arm_right(0.7*self.width, relative=True)
428 
429  else:
430  print "Error from Skilled_PR2.write_char(): Charachter Unknown !"
431 
432  def write(self, s):
433  for i in range(len(s)):
434  self.write_char(s[i])
435 
436 
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_W
def write_W
Definition: writer_pr2.py:313
math_tools.geometry
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_A
def write_A
Definition: writer_pr2.py:53
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_C
def write_C
Definition: writer_pr2.py:76
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_R
def write_R
Definition: writer_pr2.py:267
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_H
def write_H
Definition: writer_pr2.py:139
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_E
def write_E
Definition: writer_pr2.py:95
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.arm_speed
arm_speed
Definition: writer_pr2.py:48
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.board_offset
board_offset
Definition: writer_pr2.py:50
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_J
def write_J
Definition: writer_pr2.py:171
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_T
def write_T
Definition: writer_pr2.py:290
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2
Definition: writer_pr2.py:43
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.larm_reference
larm_reference
Definition: writer_pr2.py:51
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_M
def write_M
Definition: writer_pr2.py:214
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_D
def write_D
Definition: writer_pr2.py:86
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_V
def write_V
Definition: writer_pr2.py:303
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.width
width
Definition: writer_pr2.py:47
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_X
def write_X
Definition: writer_pr2.py:323
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_S
def write_S
Definition: writer_pr2.py:277
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.__init__
def __init__
Definition: writer_pr2.py:44
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_F
def write_F
Definition: writer_pr2.py:110
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_B
def write_B
Definition: writer_pr2.py:67
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write
def write
Definition: writer_pr2.py:432
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_char
def write_char
Definition: writer_pr2.py:371
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_L
def write_L
Definition: writer_pr2.py:204
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_Z
def write_Z
Definition: writer_pr2.py:349
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_K
def write_K
Definition: writer_pr2.py:191
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_P
def write_P
Definition: writer_pr2.py:244
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.height
height
Definition: writer_pr2.py:46
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_G
def write_G
Definition: writer_pr2.py:123
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_O
def write_O
Definition: writer_pr2.py:234
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_U
def write_U
Definition: writer_pr2.py:181
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_Q
def write_Q
Definition: writer_pr2.py:253
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_I
def write_I
Definition: writer_pr2.py:155
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_Y
def write_Y
Definition: writer_pr2.py:335
magiks.specific_geometries.pr2.writer_pr2.Writer_PR2.write_N
def write_N
Definition: writer_pr2.py:225
Generated on Sat Jun 4 2016 20:04:30 for MAGIKS by   doxygen 1.8.8