Skip to content

Commit c7a655b

Browse files
petercorkepetercorke
committed
Move parsedemo out of utility, eliminate module include loop.
Upgrade parsedemo to more closely parse matlab syntax, and update 2 demo scripts to suit.
1 parent 8ca7ded commit c7a655b

File tree

4 files changed

+134
-66
lines changed

4 files changed

+134
-66
lines changed

‎robotics-toolbox-python/demo/rtjademo.py

Lines changed: 30 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,12 @@
1-
# Copyright (C) 1993-2002, by Peter I. Corke
2-
echo off
3-
# 6/99 change arg to maniplty() to 'yoshikawa'
4-
# $Log: rtjademo.m,v $
5-
# Revision 1.2 2002/04/01 11:47:17 pic
6-
# General cleanup of code: help comments, see also, copyright, remnant dh/dyn
7-
# references, clarification of functions.
8-
#
9-
# $Revision: 1.2 $
10-
echo on
11-
#
1+
2+
import robot.parsedemo as p;
3+
import sys
4+
5+
print sys.modules
6+
7+
if __name__ == '__main__':
8+
9+
s = '''
1210
# Jacobian and differential motion demonstration
1311
#
1412
# A differential motion can be represented by a 6-element vector with elements
@@ -23,19 +21,19 @@
2321
#
2422
# but a more direct approach is to use the function diff2tr()
2523
#
26-
D = [.1 .2 0 -.2 .1 .1]';
27-
diff2tr(D)
24+
D = mat([.1, .2, 0, -.2, .1, .1]).T;
25+
skew(D)
2826
pause % any key to continue
2927
#
3028
# More commonly it is useful to know how a differential motion in one
3129
# coordinate frame appears in another frame. If the second frame is
3230
# represented by the transform
33-
T = transl(100, 200, 300) * roty(pi/8) * rotz(-pi/4);
31+
T = transl(100, 200, 300) * troty(pi/8) * trotz(-pi/4);
3432
#
3533
# then the differential motion in the second frame would be given by
3634
3735
DT = tr2jac(T) * D;
38-
DT'
36+
DT.T
3937
#
4038
# tr2jac() has computed a 6x6 Jacobian matrix which transforms the differential
4139
# changes from the first frame to the next.
@@ -49,12 +47,15 @@
4947
#
5048
# For an n-joint manipulator the manipulator Jacobian is a 6 x n matrix and
5149
# is used is many manipulator control schemes. For a 6-axis manipulator like
52-
# the Puma 560 the Jacobian is square
50+
# the Puma 560 the Jacobian is square.
51+
#
52+
# We import the robot model
53+
from robot.puma560 import *
5354
#
5455
# Two Jacobians are frequently used, which express the Cartesian velocity in
5556
# the world coordinate frame,
5657
57-
q = [0.1 0.75 -2.25 0 .75 0]
58+
q = [0.1, 0.75, -2.25, 0, .75, 0]
5859
J = jacob0(p560, q)
5960
#
6061
# or the T6 coordinate frame
@@ -69,7 +70,7 @@
6970
#
7071
# Many control schemes require the inverse of the Jacobian. The Jacobian
7172
# in this example is not singular
72-
det(J)
73+
linalg.det(J)
7374
#
7475
# and may be inverted
7576
Ji = inv(J)
@@ -81,9 +82,13 @@
8182
#
8283
# where dX/dt is the desired Cartesian velocity, and dQ/dt is the required
8384
# joint velocity to achieve this.
84-
vel = [1 0 0 0 0 0]'; % translational motion in the X direction
85+
#
86+
# We demand pure translational motion in the X direction
87+
vel = mat([1, 0, 0, 0, 0, 0]).T;
88+
89+
# and "resolve" this into joint rates
8590
qvel = Ji * vel;
86-
qvel'
91+
qvel.T
8792
#
8893
# This is an alternative strategy to computing a Cartesian trajectory
8994
# and solving the inverse kinematics. However like that other scheme, this
@@ -104,7 +109,7 @@
104109
T6 = fkine(p560, q); % compute the end-effector transform
105110
d6X = tr2jac(T6) * vel; % translate world frame velocity to T6 frame
106111
qvel = Ji * d6X; % compute required joint velocity as before
107-
qvel'
112+
qvel.T
108113
#
109114
# Note that this value of joint velocity is quite different to that calculated
110115
# above, which was for motion in the T6 X-axis direction.
@@ -117,7 +122,7 @@
117122
rank( jacobn(p560, qr) )
118123
#
119124
# and the singular values are
120-
svd( jacobn(p560, qr) )
125+
linalg.svd( jacobn(p560, qr) )
121126
pause % any key to continue
122127
#
123128
# When not actually at a singularity the Jacobian can provide information
@@ -146,4 +151,6 @@
146151
# based on the pseudo-inverse of the Jacobian (which will not be square) or
147152
# singular value decomposition of the Jacobian.
148153
#
149-
echo off
154+
'''
155+
156+
p.parsedemo(s);

‎robotics-toolbox-python/demo/rttrdemo.py

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@
66
#
77
# $Revision: 1.2 $
88

9-
from robot import *;
9+
import robot.parsedemo as p;
10+
import sys
1011

12+
print sys.modules
1113

1214
if __name__ == '__main__':
1315

@@ -54,4 +56,4 @@
5456
# which are quite different.
5557
'''
5658

57-
parsedemo(s);
59+
p.parsedemo(s);

‎robotics-toolbox-python/robot/parsedemo.py

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
from numpy import *
2+
from robot import *
3+
import textwrap;
4+
import sys;
5+
import re;
6+
7+
# we use termios to pretty up the continuation prompting, only available under Unix
8+
has_termios = False;
9+
try:
10+
import termios;
11+
has_termios = True;
12+
except:
13+
pass;
14+
15+
def parsedemo(s):
16+
"""
17+
Helper to write demo files, based on minimum change from their Matlab format.
18+
The string contains
19+
- help text which is left justified and displayed directly to the screen
20+
- indented text is interpretted as a command, executed, and the output sent to
21+
screen
22+
23+
The argument is Matlab code as per the demo files rtXXdemo.
24+
25+
@note: Requires some minor changes to the demo file.
26+
@type s: string
27+
@param s: Demo string.
28+
"""
29+
rstrip = re.compile(r'''\s*%.*$''')
30+
lines = s.split('\n');
31+
32+
name = __file__;
33+
print name;
34+
print len(name)*'=';
35+
if has_termios:
36+
fd = sys.stdin.fileno()
37+
old = termios.tcgetattr(fd)
38+
new = termios.tcgetattr(fd)
39+
new[3] = new[3] & ~termios.ECHO # lflags
40+
try:
41+
if has_termios:
42+
termios.tcsetattr(fd, termios.TCSADRAIN, new)
43+
44+
text = '';
45+
for line in lines:
46+
if len(line) == 0:
47+
print;
48+
elif line[0] == '#':
49+
## help text found, add it to the string
50+
text += line[1:].lstrip() + '\n';
51+
52+
# a blank line means paragraph break
53+
if len(line) == 1 and text:
54+
print textwrap.fill(text, fix_sentence_endings=True);
55+
text = '';
56+
print
57+
else:
58+
## command encountered
59+
60+
# flush the remaining help text
61+
if text:
62+
print textwrap.fill(text, fix_sentence_endings=True);
63+
text = '';
64+
65+
cmd = line.strip();
66+
67+
# special case, pause prompts the user to continue
68+
if cmd.startswith('pause'):
69+
# prompt for continuation
70+
sys.stderr.write('more? ')
71+
raw_input();
72+
73+
# remove the prompt from the screen
74+
sys.stderr.write('\r \r');
75+
continue;
76+
print
77+
78+
# show the command we are about to execute
79+
print '>>>', cmd;
80+
81+
# if it involves an assignment then we use exec else use eval.
82+
# we mimic the matlab behaviour in which a trailing semicolon inhibits
83+
# display of the result
84+
if cmd.startswith('from'):
85+
exec cmd;
86+
elif '=' in cmd:
87+
e = cmd.split('=');
88+
exec rstrip.sub('', cmd);
89+
if cmd[-1] != ';':
90+
print eval(e[0]);
91+
else:
92+
result = eval(cmd.rstrip(';'));
93+
if cmd[-1] != ';':
94+
print result;
95+
96+
finally:
97+
if has_termios:
98+
termios.tcsetattr(fd, termios.TCSADRAIN, old)

‎robotics-toolbox-python/robot/utility.py

Lines changed: 2 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,8 @@
66
@copyright: Peter Corke
77
"""
88

9-
from numpy import *;
9+
from numpy import *
10+
1011

1112
def ishomog(tr):
1213
"""
@@ -140,46 +141,6 @@ def func(*args):
140141
raise ValueError;
141142

142143

143-
def parsedemo(s):
144-
"""
145-
Helper to write demo files, based on minimum change from their Matlab format.
146-
The string contains
147-
- help text which is left justified and displayed directly to the screen
148-
- indented text is interpretted as a comamnd, executed, and the output sent to
149-
screen
150-
151-
The argument is Matlab code as per the demo files rtXXdemo.
152-
153-
@note: Requires some minor changes to the demo file.
154-
@type s: string
155-
@param s: Demo string.
156-
"""
157-
158-
lines = s.split('\n');
159-
160-
name = __file__;
161-
print name;
162-
print len(name)*'=';
163-
164-
for line in lines:
165-
if len(line) == 0:
166-
print;
167-
elif line[0] == '#':
168-
print line[1:].lstrip();
169-
else:
170-
cmd = line.strip();
171-
if cmd.startswith('pause'):
172-
print "more?",;
173-
raw_input();
174-
continue;
175-
print
176-
print '>>>', cmd;
177-
if '=' in cmd:
178-
exec cmd;
179-
print t
180-
else:
181-
result = eval(cmd);
182-
print result;
183144

184145
import traceback;
185146

0 commit comments

Comments
 (0)