8000 add sympy version · LMCallMe/robotics-toolbox-python@fcd1d10 · GitHub
[go: up one dir, main page]
Skip to content

Commit fcd1d10

Browse files
LMCallMeLMCallMe
committed
add sympy version
1 parent 7f26762 commit fcd1d10

File tree

6 files changed

+1411
-0
lines changed

6 files changed

+1411
-0
lines changed

‎sympyrobot/Link.py

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

0 commit comments

Comments
 (0)
0