8000 Updates to manipulability and test suite. · Lewishw/robotics-toolbox-python@4f2dd33 · GitHub
[go: up one dir, main page]
Skip to content

Commit 4f2dd33

Browse files
petercorkepetercorke
committed
Updates to manipulability and test suite.
1 parent 56cb53b commit 4f2dd33

File tree

3 files changed

+58
-19
lines changed

3 files changed

+58
-19
lines changed

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,8 @@
2929
# import kinematics section
3030
from kinematics import *
3131

32+
from manipulability import *
33+
3234
# import trajectories section
3335
from trajectory import *
3436

‎robotics-toolbox-python/robot/maniplty.py renamed to ‎robotics-toolbox-python/robot/manipulability.py

Lines changed: 20 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,12 @@
88

99

1010
from numpy import *
11-
from jacob0 import *
12-
from numpy.linalg import inv,eig,det
13-
from inertia import *
14-
from numrows import *
11+
from robot.jacobian import jacob0
12+
from robot.dynamics import inertia
13+
from numpy.linalg import inv,eig,det,svd
14+
from robot.utility import *
1515

16-
def maniplty(robot, q, which = 'yoshikawa'):
16+
def manipulability(robot, q, which = 'yoshikawa'):
1717
'''
1818
MANIPLTY Manipulability measure
1919
@@ -47,19 +47,19 @@ def maniplty(robot, q, which = 'yoshikawa'):
4747
@see: inertia, jacob0
4848
'''
4949
n = robot.n
50-
q = mat(q)
51-
w = array([])
52-
if 'yoshikawa'.startswith(which):
53-
if numrows(q)==1:
54-
return yoshi(robot,q)
55-
for Q in q:
56-
w = concatenate((w,array([yoshi(robot,Q)])))
57-
elif 'asada'.startswith(which):
58-
if numrows(q)==1:
59-
return asada(robot,q)
60-
for Q in q:
61-
w = concatenate((w,array([asada(robot,Q)])))
62-
return mat(w)
50+
q = mat(q)
51+
w = array([])
52+
if 'yoshikawa'.startswith(which):
53+
if numrows(q)==1:
54+
return yoshi(robot,q)
55+
for Q in q:
56+
w = concatenate((w,array([yoshi(robot,Q)])))
57+
elif 'asada'.startswith(which):
58+
if numrows(q)==1:
59+
return asada(robot,q)
60+
for Q in q:
61+
w = concatenate((w,array([asada(robot,Q)])))
62+
return mat(w)
6363

6464
def yoshi(robot,q):
6565
J = jacob0(robot,q)
@@ -71,4 +71,5 @@ def asada(robot,q):
7171
M = inertia(robot,q)
7272
Mx = Ji.T*M*Ji
7373
e = eig(Mx)[0]
74-
return e.min(0)/e.max(0)
74+
75+
return real( e.min(0)/e.max(0) )

‎robotics-toolbox-python/test/manip.py

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
'''Test manipulability'''
2+
3+
from robot import *
4+
from robot.puma560 import *
5+
6+
set_printoptions(precision=4, suppress=True);
7+
8+
tests = """
9+
p560
10+
11+< C011 /span>
q = qn
12+
manipulability(p560, q)
13+
manipulability(p560, q, 'yoshi')
14+
manipulability(p560, q, 'y')
15+
manipulability(p560, q, 'asada')
16+
a=manipulability(p560, q, 'a')
17+
real(a)
18+
imag(a)
19+
manipulability(p560, q, 'z')
20+
21+
qq = vstack((q, q, q, q))
22+
qq
23+
manipulability(p560, qq, 'yoshi')
24+
manipulability(p560, qq, 'asada')
25+
""";
26+
27+
for line in tests.split('\n'):
28+
if line == '' or line[0] in '%#':
29+
continue;
30+
print '::', line;
31+
if '=' in line:
32+
exec line;
33+
else:
34+
print eval(line);
35+
print
36+

0 commit comments

Comments
 (0)
0