8000 Create DHFactor.py · navrobot/robotics-toolbox-python@04f33c4 · GitHub
[go: up one dir, main page]
Skip to content

Commit 04f33c4

Browse files
Samuel-DrewSamuel-Drew
committed
Create DHFactor.py
1 parent 1d13a66 commit 04f33c4

File tree

1 file changed

+196
-0
lines changed

1 file changed

+196
-0
lines changed

‎roboticstoolbox/tools/DHFactor.py

Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
"""
2+
@author Samuel Drew
3+
"""
4+
5+
6+
class Element:
7+
8+
TX = 0
9+
TY = 1
10+
TZ = 2
11+
RX = 3
12+
RY = 4
13+
RZ = 5
14+
DH_STANDARD = 6
15+
DH_MODIFIED = 7
16+
17+
# an array of counters for the application of each rule
18+
# just for debugging.
19+
rules = [0] * 20
20+
21+
# mapping from type to string
22+
typeName = ["TX", "TY", "TZ", "RX", "RY", "RZ", "DH", "DHm"]
23+
24+
# order of elementary transform for each DH convention
25+
# in each tuple, the first element is the transform type,
26+
# the second is true if it can be a joint variable.
27+
dhStandard = (RZ, 1), (TX, 0), (TZ, 1), (RX, 0)
28+
dhModified = (RX, 0), (TX, 0), (RZ, 1), (TZ, 1)
29+
30+
31+
def __init__(
32+
self,
33+
elementIn=None,
34+
stringIn=None,
35+
eltype=None,
36+
constant=None,
37+
sign=0):
38+
39+
self.var = None # eg. q1, for joint var types
40+
self.symconst = None # eg. L1, for lengths
41+
42+
# DH parameters, only set if type is DH_STANDARD/MODIFIED
43+
self.theta = None
44+
self.alpha = None
45+
self.A = None
46+
self.D = None
47+
self.prismatic = None
48+
self.offset = None
49+
50+
if stringIn:
51+
if elementIn or eltype or constant:
52+
raise ValueError("if parsing a string, string must be the only input")
53+
i = None
54+
sType = stringIn[0:2] # Tx, Rx etc
55+
sRest = stringIn[2:] # the argument including brackets
56+
57+
if not (sRest[-1] == ")" and sRest[0] == "("):
58+
raise ValueError("brackets")
59+
60+
i = 0
61+
while i < 6:
62+
if sType.lower == self.typeName[i].lower:
63+
break
64+
i += 1
65+
if i >= 6:
66+
raise ValueError("bad transform name" + sType)
67+
self.eltype = i
68+
69+
sRest = sRest[1:-2] # get the argument from between brackets
70+
71+
# handle an optional minus sign
72+
negative = ""
73+
74+
if sRest[0] == '-':
75+
negative = "-"
76+
sRest = sRest[1]
77+
78+
if sRest[0] == "q":
79+
self.var = negative + sRest
80+
elif sRest[0] == 'L':
81+
self.symconst = negative + sRest
82+
else:
83+
try:
84+
constant = float(sRest)
85+
if negative == "-":
86+
constant = -constant
87+
negative = ""
88+
except:
89+
raise ValueError("bad argument in term " + stringIn)
90+
91+
elif elementIn:
92+
if not isinstance(elementIn, Element):
93+
raise TypeError("elementIn must be an existing Element object")
94 6D4E +
self.eltype = elementIn.eltype
95+
if elementIn.var:
96+
self.var = elementIn.var
97+
if elementIn.symconst:
98+
self.symconst = elementIn.symconst
99+
self.constant = elementIn.constant
100+
# if sign < 0:
101+
# self.negate()
102+
103+
104+
# one of TX, TY ... RZ, DH_STANDARD/MODIFIED
105+
if eltype:
106+
self.eltype = eltype
107+
108+
# transform parameters, only one of these is set
109+
if constant:
110+
self.constant = constant # eg. 90, for angles
111+
112+
@staticmethod
113+
def showRuleUsage():
114+
for i in range(20):
115+
if Element.rules[i] > 0:
116+
print("Rule " + str(i) + ": " + str(Element.rules[i]))
117+
118+
def istrans(self):
119+
return (self.eltype == self.TX) or (self.eltype == self.TY) or (self.eltype == self.TZ)
120+
121+
def isrot(self):
122+
return (self.eltype == self.RX) or (self.eltype == self.RY) or (self.eltype == self.RZ)
123+
124+
def isjoint(self):
125+
return self.var is not None
126+
127+
def axis(self):
128+
if self.eltype == self.RX or self.eltype == self.TX:
129+
return 0
130+
elif self.eltype == self.RY or self.eltype == self.TY:
131+
return 1
132+
elif self.eltype == self.RZ or self.eltype == self.TZ:
133+
return 2
134+
else:
135+
raise ValueError("bad transform type")
136+
137+
def symAdd(self, s1, s2):
138+
#TODO method for adding symbols
139+
print("symAdd not yet implemented")
140+
141+
142+
def add(self, e):
143+
if self.eltype != self.DH_Standard and self.eltype != self.DH_MODIFIED:
144+
raise ValueError("wrong element type " + str(self))
145+
print(" adding: " + str(self) + " += " + str(e))
146+
if e.eltype == self.RZ:
147+
if e.isjoint():
148+
self.prismatic = 0
149+
self.var = e.var
150+
self.offset = e.constant
151+
self.theta = 0.0
152+
else:
153+
self.theta = e.constant
154+
elif e.eltype == self.TX:
155+
self.A = e.symconst
156+
elif e.eltype == self.TZ:
157+
if e.isjoint():
158+
self.prismatic = 1
159+
self.var = e.var
160+
self.D = None
161+
else:
162+
self.D = e.symconst
163+
elif e.eltype == self.RX:
164+
self.alpha = e.constant
165+
else:
166+
raise ValueError("Can't factorise " + str(e))
167+
168+
# test if this particular element could be part of a DH term
169+
# eg. Rz(q1) can be, Rx(q1) cannot.
170+
def factorMatch(self, dhWhich, i, verbose):
171+
172+
dhFactors = None
173+
match = False
174+
175+
if dhWhich == self.DH_STANDARD:
176+
dhFactors = self.dhStandard
177+
elif dhWhich == self.DH_MODIFIED:
178+
dhFactors = self.dhModified
179+
else:
180+
raise ValueError("bad DH type")
181+
182+
match = (self.eltype == dhFactors[i][0]) and not ((dhFactors[i][1] == 0) and self.isjoint())
183+
184+
if verbose > 0:
185+
print(" matching " + str(self) + " (i=" + str(i) + ") " +
186+
" to " + self.typeName[dhFactors[i][0]] + "<" +
187+
str(dhFactors[i][1]) + ">" + " -> " + str(match))
188+
return match
189+
190+
191+
def merge(self, e):
192+
193+
"""
194+
don't merge if dissimilar transform or
195+
both are joint variables
196+
"""

0 commit comments

Comments
 (0)
0