8000 Merge pull request #22 from petercorke/sam-dev · navrobot/robotics-toolbox-python@af41538 · GitHub
[go: up one dir, main page]
Skip to content

Commit af41538

Browse files
Samuel-DrewSamuel-Drew
authored
Merge pull request petercorke#22 from petercorke/sam-dev
Sam dev
2 parents c72ce54 + ef1a595 commit af41538

File tree

7 files changed

+1272
-521
lines changed

7 files changed

+1272
-521
lines changed

‎roboticstoolbox/robot/Link.py

Lines changed: 344 additions & 302 deletions
Large diffs are not rendered by default.

‎roboticstoolbox/robot/Link_old.py

Lines changed: 312 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,312 @@
1+
"""
2+
Link object.
3+
Python implementation by: Luis Fernando Lara Tobar and Peter Corke.
4+
Based on original Robotics Toolbox for Matlab code by Peter Corke.
5+
Permission to use and copy is granted provided that acknowledgement of
6+
the authors is made.
7+
@author: Luis Fernando Lara Tobar and Peter Corke
8+
"""
9+
10+
from numpy import *
11+
from abc import ABC
12+
import copy
13+
14+
15+
class Link(ABC):
16+
17+
"""
18+
Link abstract class
19+
"""
20+
21+
def __init__(self, theta, d, a, alpha, offset, jointtype, mdh):
22+
"""
23+
initialises the Link object
24+
:param theta:
25+
:param d:
26+
:param a:
27+
:param alpha:
28+
:param offset:
29+
:param jointtype: 'R' or 'P' as input. 'R' for Revolute. 'P' for Prismatic.
30+
:param mdh:
31+
"""
32+
self.theta = theta
33+
self.d = d
34+
self.a = a
35+
self.alpha = alpha
36+
self.offset = offset
37+
self.mdh = mdh
38+
39+
# we know nothing about the dynamics
40+
self.m = None
41+
self.r = None
42+
self.v = None
43+
self.I = None
44+
self.Jm = None
45+
self.G = None
46+
self.B = None
47+
self.Tc = None
48+
self.qlim = None
49+
50+
return None
51+
52+
def __repr__(self):
53+
54+
if self.convention == Link.LINK_DH:
55+
conv = 'std'
56+
else:
57+
conv = 'mod'
58+
59+
if self.sigma == 0:
60+
jtype = 'R'
61+
else:
62+
jtype = 'P'
63+
64+
if self.D == None:
65+
return "alpha=%f, A=%f, theta=%f jtype: (%c) conv: (%s)" % (self.alpha,
66+
self.A, self.theta, jtype, conv)
67+
elif self.theta == None:
68+
return "alpha=%f, A=%f, D=%f jtype: (%c) conv: (%s)" % (self.alpha,
69+
self.A, self.D, jtype, conv)
70+
else:
71+
return "alpha=%f, A=%f, theta=%f, D=%f jtype: (%c) conv: (%s)" % (self.alpha,
72+
self.A, self.theta, self.D, jtype, conv)
73+
74+
# invoked at print
75+
def __str__(self):
76+
if self.convention == Link.LINK_DH:
77+
conv = 'std'
78+
else:
79+
conv = 'mod'
80+
81+
if self.sigma == 0:
82+
jtype = 'R'
83+
else:
84+
jtype = 'P'
85+
86+
if self.D == None:
87+
return "alpha = %f\tA = %f\ttheta = %f\t--\tjtype: %c\tconv: (%s)" % (
88+
self.alpha, self.A, self.theta, jtype, conv)
89+
elif self.theta == None:
90+
return "alpha = %f\tA = %f\t--\tD = %f\tjtype: %c\tconv: (%s)" % (
91+
self.alpha, self.A, self.D, jtype, conv)
92+
else:
93+
return "alpha = %f\tA = %f\ttheta = %f\tD=%f\tjtype: %c\tconv: (%s)" % (
94+
self.alpha, self.A, self.theta, self.D, jtype, conv)
95+
96+
97+
def display(self):
98+
99+
print(self)
100+
print
101+
102+
if self.m != None:
103+
print("m:", self.m)
104+
if self.r != None:
105+
print("r:", self.r)
106+
if self.I != None:
107+
print("I:\n", self.I)
108+
if self.Jm != None:
109+
print("Jm:", self.Jm)
110+
if self.B != None:
111+
print("B:", self.B)
112+
if self.Tc != None:
113+
print("Tc:", self.Tc)
114+
if self.G != None:
115+
print("G:", self.G)
116+
if self.qlim != None:
117+
print("qlim:\n", self.qlim)
118+
119+
def copy(self):
120+
"""
121+
Return copy of this Link
122+
"""
123+
return copy.copy(self);
124+
125+
def friction(self, qd):
126+
"""
127+
Compute friction torque for joint rate C{qd}.
128+
Depending on fields in the Link object viscous and/or Coulomb friction
129+
are computed.
130+
131+
@type qd: number
132+
@param qd: joint rate
133+
@rtype: number
134+
@return: joint friction torque
135+
"""
136+
tau = 0.0
137+
if isinstance(qd, (ndarray, matrix)):
138+
qd = qd.flatten().T
139+
if self.B == None:
140+
self.B = 0
141+
tau = self.B * qd
142+
if self.Tc == None:
143+
self.Tc = mat([0,0])
144+
tau = tau + (qd > 0) * self.Tc[0,0] + (qd < 0) * self.Tc[0,1]< 10000 /div>
145+
return tau
146+
147+
def nofriction(self, all=False):
148+
"""
149+
Return a copy of the Link object with friction parameters set to zero.
150+
151+
@type all: boolean
152+
@param all: if True then also zero viscous friction
153+
@rtype: Link
154+
@return: Copy of original Link object with zero friction
155+
@see: L{robot.nofriction}
156+
"""
157+
158+
l2 = self.copy()
159+
160+
l2.Tc = array([0, 0])
161+
if all:
162+
l2.B = 0
163+
return l2;
164+
165+
166+
# methods to set kinematic or dynamic parameters
167+
168+
fields = ["alpha", "A", "theta", "D", "sigma", "offset", "m", "Jm", "G", "B", "convention"];
169+
170+
def __setattr__(self, name, value):
171+
"""
172+
Set attributes of the Link object
173+
174+
- alpha; scalar
175+
- A; scalar
176+
- theta; scalar
177+
- D; scalar
178+
- sigma; scalar
179+
- offset; scalar
180+
- m; scalar
181+
- Jm; scalar
182+
- G; scalar
183+
- B; scalar
184+
- r; 3-vector
185+
- I; 3x3 matrix, 3-vector or 6-vector
186+
- Tc; scalar or 2-vector
187+
- qlim; 2-vector
188+
189+
Inertia, I, can be specified as:
190+
- 3x3 inertia tensor
191+
- 3-vector, the diagonal of the inertia tensor
192+
- 6-vector, the unique elements of the inertia tensor [Ixx Iyy Izz Ixy Iyz Ixz]
193+
194+
Coloumb friction, Tc, can be specifed as:
195+
- scalar, for the symmetric case when Tc- = Tc+
196+
- 2-vector, the assymetric case [Tc- Tc+]
197+
198+
Joint angle limits, qlim, is a 2-vector giving the lower and upper limits
199+
of motion.
200+
"""
201+
202+
if value == None:
203+
self.__dict__[name] = value;
204+
return;
205+
206+
if name in self.fields:
207+
# scalar parameter
208+
if isinstance(value, (ndarray,matrix)) and value.shape != (1,1):
209+
raise(ValueError, "Scalar required")
210+
if not isinstance(value, (int,float,int32,float64)):
211+
raise(ValueError)
212+
self.__dict__[name] = value
213+
214+
elif name == "r":
215+
r = arg2array(value);
216+
if len(r) != 3:
217+
raise (ValueError, "matrix required")
218+
219+
self.__dict__[name] = mat(r)
220+
221+
elif name == "I":
222+
if isinstance(value, matrix) and value.shape == (3,3):
223+
self.__dict__[name] = value;
224+
else:
225+
v = arg2array(value);
226+
if len(v) == 3:
227+
self.__dict__[name] = mat(diag(v))
228+
elif len(v) == 6:
229+
self.__dict__[name] = mat([
230+
[v[0],v[3],v[5]],
231+
[v[3],v[1],v[4]],
232+
[v[5],v[4],v[2]]])
233+
else:
234+
raise(ValueError, "matrix required")
235+
236+
elif name == "Tc":
237+
v = arg2array(value)
238+
239+
if len(v) == 1:
240+
self.__dict__[name] = mat([-v[0], v[0]])
241+
elif len(v) == 2:
242+
self.__dict__[name] = mat(v)
243+
else:
244+
raise ValueError;
245+
246+
elif name == "qlim":
247+
v = arg2array(value);
248+
if len(v) == 2:
249+
self.__dict__[name] = mat(v);
250+
else:
251+
raise ValueError
252+
else:
253+
raise(NameError, "Unknown attribute <%s> of link" % name)
254+
255+
256+
# LINK.islimit(q) return if limit is exceeded: -1, 0, +1
257+
def islimit(self,q):
258+
"""
259+
Check if joint limits exceeded. Returns
260+
- -1 if C{q} is less than the lower limit
261+
- 0 if C{q} is within the limits
262+
- +1 if C{q} is greater than the high limit
263+
264+
@type q: number
265+
@param q: Joint coordinate
266+
@rtype: -1, 0, +1
267+
@return: joint limit status
268+
"""
269+
if not self.qlim:
270+
return 0
271+
272+
return (q > self.qlim[1,0]) - (q < self.qlim[0,0])
273+
274+
def tr(self, q):
275+
"""
276+
Compute the transformation matrix for this link. This is a function
277+
of kinematic parameters, the kinematic model (DH or MDH) and the joint
278+
coordinate C{q}.
279+
280+
@type q: number
281+
@param q: joint coordinate
282+
@rtype: homogeneous transformation
283+
@return: Link transform M{A(q)}
284+
"""
285+
286+
an = self.A
287+
dn = self.D
288+
theta = self.theta
289+
290+
if self.sigma == 0:
291+
theta = q # revolute
292+
else:
293+
dn = q # prismatic
294+
295+
sa = sin(self.alpha); ca = cos(self.alpha);
296+
st = sin(theta); ct = cos(theta);
297+
298+
if self.convention == Link.LINK_DH:
299+
# standard
300+
t = mat([[ ct, -st*ca, st*sa, an*ct],
301+
[st, ct*ca, -ct*sa, an*st],
302+
[0, sa, ca, dn],
303+
[0, 0, 0, 1]]);
304+
305+
else:
306+
# modified
307+
t = mat([[ ct, -st, 0, an],
308+
[st*ca, ct*ca, -sa, -sa*dn],
309+
[st*sa, ct*sa, ca, ca*dn],
310+
[0, 0, 0, 1]]);
311+
312+
return t;

0 commit comments

Comments
 (0)
0