8000 changes to Link to accept python style keyword args · navrobot/robotics-toolbox-python@eb9f9f2 · GitHub
[go: up one dir, main page]
Skip to content

Commit eb9f9f2

Browse files
Samuel-DrewSamuel-Drew
committed
changes to Link to accept python style keyword args
1 parent 9ac5543 commit eb9f9f2

File tree

8 files changed

+384
-311
lines changed

8 files changed

+384
-311
lines changed

‎roboticstoolbox/robot/Link.py

Lines changed: 57 additions & 273 deletions
Original file line numberDiff line numberDiff line change
@@ -1,297 +1,81 @@
11
"""
2-
Link object.
2+
Link object v2
33
Python implementation by Samuel Drew
44
"""
55

66
from numpy import *
77
from spatialmath.pose3d import *
8-
import argparse
98

109

1110
class Link(list):
1211
"""
13-
12+
Create a Link object
1413
"""
1514

16-
def __init__(self, *argv):
17-
"""
18-
Link Create robot link object
19-
20-
% This the class constructor which has several call signatures.
21-
%
22-
% L = Link() is a Link object with default parameters.
23-
%
24-
% L = Link(LNK) is a Link object that is a deep copy of the link
25-
% object LNK and has type Link, even if LNK is a subclass.
26-
%
27-
% L = Link(OPTIONS) is a link object with the kinematic and dynamic
28-
% parameters specified by the key/value pairs.
29-
%
30-
% Options::
31-
% 'theta',TH joint angle, if not specified joint is revolute
32-
% 'd',D joint extension, if not specified joint is prismatic
33-
% 'a',A joint offset (default 0)
34-
% 'alpha',A joint twist (default 0)
35-
% 'standard' defined using standard D&H parameters (default).
36-
% 'modified' defined using modified D&H parameters.
37-
% 'offset',O joint variable offset (default 0)
38-
% 'qlim',L joint limit (default [])
39-
% 'I',I link inertia matrix (3x1, 6x1 or 3x3)
40-
% 'r',R link centre of gravity (3x1)
41-
% 'm',M link mass (1x1)
42-
% 'G',G motor gear ratio (default 1)
43-
% 'B',B joint friction, motor referenced (default 0)
44-
% 'Jm',J motor inertia, motor referenced (default 0)
45-
% 'Tc',T Coulomb friction, motor referenced (1x1 or 2x1), (default [0 0])
46-
% 'revolute' for a revolute joint (default)
47-
% 'prismatic' for a prismatic joint 'p'
48-
% 'standard' for standard D&H parameters (default).
49-
% 'modified' for modified D&H parameters.
50-
% 'sym' consider all parameter values as symbolic not numeric
51-
%
52-
% Notes::
53-
% - It is an error to specify both 'theta' and 'd'
54-
% - The joint variable, either theta or d, is provided as an argument to
55-
% the A() method.
56-
% - The link inertia matrix (3x3) is symmetric and can be specified by giving
57-
% a 3x3 matrix, the diagonal elements [Ixx Iyy Izz], or the moments and products
58-
% of inertia [Ixx Iyy Izz Ixy Iyz Ixz].
59-
% - All friction quantities are referenced to the motor not the load.
60-
% - Gear ratio is used only to convert motor referenced quantities such as
61-
% friction and interia to the link frame.
62-
%
63-
% Old syntax::
64-
% L = Link(DH, OPTIONS) is a link object using the specified kinematic
65-
% convention and with parameters:
66-
% - DH = [THETA D A ALPHA SIGMA OFFSET] where SIGMA=0 for a revolute and 1
67-
% for a prismatic joint; and OFFSET is a constant displacement between the
68-
% user joint variable and the value used by the kinematic model.
69-
% - DH = [THETA D A ALPHA SIGMA] where OFFSET is zero.
70-
% - DH = [THETA D A ALPHA], joint is assumed revolute and OFFSET is zero.
71-
%
72-
% Options::
73-
%
74-
% 'standard' for standard D&H parameters (default).
75-
% 'modified' for modified D&H parameters.
76-
% 'revolute' for a revolute joint, can be abbreviated to 'r' (default)
77-
% 'prismatic' for a prismatic joint, can be abbreviated to 'p'
78-
%
79-
% Notes::
80-
% - The parameter D is unused in a revolute joint, it is simply a placeholder
81-
% in the vector and the value given is ignored.
82-
% - The parameter THETA is unused in a prismatic joint, it is simply a placeholder
83-
% in the vector and the value given is ignored.
84-
%
85-
% Examples::
86-
% A standard Denavit-Hartenberg link
87-
% L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
88-
% since 'theta' is not specified the joint is assumed to be revolute, and
89-
% since the kinematic convention is not specified it is assumed 'standard'.
90-
%
91-
% Using the old syntax
92-
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2], 'standard');
93-
% the flag 'standard' is not strictly necessary but adds clarity. Only 4 parameters
94-
% are specified so sigma is assumed to be zero, ie. the joint is revolute.
95-
%
96-
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
97-
% the flag 'standard' is not strictly necessary but adds clarity. 5 parameters
98-
% are specified and sigma is set to zero, ie. the joint is revolute.
99-
%
100-
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 1], 'standard');
101-
% the flag 'standard' is not strictly necessary but adds clarity. 5 parameters
102-
% are specified and sigma is set to one, ie. the joint is prismatic.
103-
%
104-
% For a modified Denavit-Hartenberg revolute joint
105-
% L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'modified');
106-
%
107-
% Notes::
108-
% - Link object is a reference object, a subclass of Handle object.
109-
% - Link objects can be used in vectors and arrays.
110-
% - The joint offset is a constant added to the joint angle variable before
111-
% forward kinematics and subtracted after inverse kinematics. It is useful
112-
% if you want the robot to adopt a 'sensible' pose for zero joint angle
113-
% configuration.
114-
% - The link dynamic (inertial and motor) parameters are all set to
115-
% zero. These must be set by explicitly assigning the object
116-
% properties: m, r, I, Jm, B, Tc.
117-
% - The gear ratio is set to 1 by default, meaning that motor friction and
118-
% inertia will be considered if they are non-zero.
119-
%
120-
% See also Revolute, Prismatic, RevoluteMDH, PrismaticMDH.
121-
"""
15+
def __init__(self, *argv,
16+
alpha=0,
17+
a=0,
18+
theta=0,
19+
d=0,
20+
jointtype='R',
21+
mdh=0,
22+
offset=0,
23+
qlim=None,
24+
flip=False,
25+
Tc=None,
26+
Jm=0,
27+
I=None,
28+
m=0,
29+
r=None,
30+
sym=False):
31+
32+
if r is None:
33+
self.r = [0, 0, 0]
34+
if Tc is None:
35+
self.Tc = [0, 0]
36+
if qlim is None:
37+
self.qlim = [-pi / 2, pi / 2]
38+
if I is None:
39+
self.I = zeros([3, 3])
12240

123-
print("Link constructor called with ", len(argv), " arguments:", argv)
124-
if len(argv) == 0:
125-
print("Creating Link class object with default parameters")
41+
if not argv:
42+
print("Creating Link class object")
12643
"""
127-
Create an 'empty' Link object
44+
Create a Link object using named arguments
12845
This call signature is needed to support arrays of Links
12946
"""
130-
# kinematic parameters
131-
132-
self.alpha = 0
133-
self.a = 0
134-
self.theta = 0
135-
self.d = 0
136-
self.jointtype = 'R'
137-
self.mdh = 0
138-
self.offset = 0
139-
self.flip = False
140-
self.qlim = []
47+
assert d == 0 or theta == 0, "Bad argument, cannot specify both d and theta"
48+
assert jointtype == 'R' or jointtype == 'P', "Bad argument, jointtype must be 'R' or 'P'"
14149

142-
"""
143-
Dynamic parameters
144-
These parameters must be set by the user if dynamics is used
145-
"""
146-
self.m = 0
147-
self.r = [0, 0, 0]
148-
self.I = zeros([3, 3])
50+
if theta != 0:
51+
# constant value of theta means it must be prismatic
52+
jointtype = 'P'
14953

150-
# Dynamic params with default(zero friction)
151-
self.Jm = 0
152-
self.G = 1
153-
self.B = 0
154-
self.Tc = [0, 0]
54+
if jointtype == 'R':
55+
self.jointtype = 'R'
56+
assert theta == 0, "Bad argument, cannot specify 'theta' for revolute joint"
57+
elif jointtype == 'P':
58+
self.jointtype = 'P'
59+
assert d == 0, "Bad argument, cannot specify 'd' for prismatic joint"
60+
61+
self.alpha = alpha
62+
self.a = a
63+
self.theta = theta
64+
self.d = d
65+
self.offset = offset
66+
self.qlim = qlim
67+
self.flip = flip
68+
self.Tc = Tc
69+
self.Jm = Jm
70+
self.I = I
71+
self.m = m
72+
self.r = r
73+
self.sym = sym
74+
self.mdh = mdh
15575

15676
elif len(argv) == 1 and isinstance(argv, Link):
15777
# Clone the passed Link object
158-
self = argv
159-
160-
else:
161-
# format input into argparse
162-
argstr = ""
163-
known = ['theta', 'a', 'd', 'alpha', 'G', 'B', 'Tc', 'Jm', 'I', 'm', 'r',
164-
'offset', 'qlim', 'type', 'convention', 'sym', 'flip', 'help']
165-
for arg in argv:
166-
if arg in known:
167-
argstr += "--" + arg + " "
168-
else:
169-
argstr += str(arg) + " "
170-
171-
# Create a new Link based on parameters
172-
# parse all possible options
173-
parser = argparse.ArgumentParser()
174-
parser.add_argument('--theta', help="joint angle, if not specified joint is revolute",
175-
type=float, default=0)
176-
parser.add_argument("--a", help="joint offset (default 0)",
177-
type=float, default=0)
178-
parser.add_argument("--d", help="joint extension, if not specified joint is prismatic",
179-
type=float, default=0)
180-
parser.add_argument("--alpha", help="joint twist (default 0)",
181-
type=float, default=0)
182-
parser.add_argument("--G", help="motor gear ratio (default 1)",
183-
type=float, default=0)
184-
parser.add_argument("--B", help="joint friction, motor referenced (default 0)",
185-
type=float, default=0)
186-
parser.add_argument("--Tc", help="Coulomb friction, motor referenced (1x1 or 2x1), (default [0, 0])",
187-
type=list, default=[0, 0])
188-
parser.add_argument("--Jm", help="motor inertia, motor referenced (default 0))",
189-
type=float, default=0)
190-
parser.add_argument("--I", help="link inertia matrix (3x1, 6x1 or 3x3)",
191-
type=ndarray, default=zeros([3, 3]))
192-
parser.add_argument("--m", help="link mass (1x1)",
193-
type=float, default=0)
194-
parser.add_argument("--r", help="link centre of gravity (3x1)",
195-
type=list, default=[0, 0, 0])
196-
parser.add_argument("--offset", help="joint variable offset (default 0)",
197-
type=float, default=0)
198-
parser.add_argument("--qlim", help="joint limit",
199-
type=list, default=[-pi/2, pi/2])
200-
parser.add_argument("--type", help="joint type, 'revolute', 'prismatic' or 'fixed'",
201-
choices=['', 'revolute', 'prismatic', 'fixed'], default='')
202-
parser.add_argument("--convention", help="D&h parameters, 'standard' or 'modified'",
203-
choices=['standard', 'modified'], default='standard')
204-
parser.add_argument("--sym", help="consider all parameter values as symbolic not numeric'",
205-
action="store_true")
206-
parser.add_argument("--flip", help="TODO add help for 'flip'",
207-
action="store_true")
208-
(opt, args) = parser.parse_known_args(argstr.split())
209-
210-
if not args:
211-
212-
assert opt.d == 0 or opt.theta == 0, "Bad argument, cannot specify both d and theta"
213-
214-
if opt.type == 'revolute':
215-
print('Revolute joint')
216-
self.jointtype = 'R'
217-
assert opt.theta == 0, "Bad argument, cannot specify 'theta' for revolute joint"
218-
elif opt.type == 'prismatic':
219-
print('Prismatic joint')
220-
self.jointtype = 'P'
221-
assert opt.d == 0, "Bad argument, cannot specify 'd' for prismatic joint"
222-
223-
if opt.theta != 0:
224-
# constant value of theta means it must be prismatic
225-
self.jointtype = 'P'
226-
print('Prismatic joint, theta =', opt.theta)
227-
if opt.d != 0:
228-
# constant value of d means it must be revolute
229-
self.jointtype = 'R'
230-
print('Revolute joint, d =', opt.d)
231-
232-
self.theta = opt.theta
233-
self.d = opt.d
234-
self.a = opt.a
235-
self.alpha = opt.alpha
236-
237-
self.offset = opt.offset
238-
self.flip = opt.flip
239-
self.qlim = argcheck.getvector(opt.qlim)
240-
241-
self.m = opt.m
242-
self.r = opt.r
243-
self.I = opt.I
244-
self.Jm = opt.Jm
245-
self.G = opt.G
246-
self.B = opt.B
247-
self.Tc = opt.Tc
248-
self.mdh = ['standard', 'modified'].index(opt.convention)
249-
250-
else:
251-
"""
252-
This is the old call format, where all parameters are given by
253-
a vector containing kinematic-only, or kinematic plus dynamic
254-
parameters
255-
256-
eg. L3 = Link([0, 0.15005, 0.0203, -pi/2, 0], 'standard')
257-
"""
258-
print("old format")
259-
dh = argv[0]
260-
assert len(dh) >= 4, "Bad argument, must provide params (theta d a alpha)"
261-
262-
# set the kinematic parameters
263-
self.theta = dh[0]
264-
self.d = dh[1]
265-
self.a = dh[2]
266-
self.alpha = dh[3]
267-
268-
self.jointtype = 'R'
269-
self.offset = 0
270-
self.flip = False
271-
self.mdh = 0
272-
273-
# optionally set jointtype and offset
274-
if len(dh) >= 5:
275-
if dh[4] == 1:
276-
self.jointtype = 'P'
277-
if len(dh) == 6:
278-
self.offset = dh[5]
279-
280-
else:
281-
# we know nothing about the dynamics
282-
self.m = 0
283-
self.r = [0, 0, 0]
284-
self.I = zeros([3, 3])
285-
self.Jm = 0
286-
self.G = 1
287-
self.B = 0
288-
self.Tc = [0, 0]
289-
self.qlim = 0
290-
291-
self.mdh = 0
292-
if len(argv) == 2:
293-
if argv[1] == 'modified':
294-
self.mdh == 1
78+
self = argv[0]
29579

29680
def __repr__(self):
29781

0 commit comments

Comments
 (0)
0