8000 Update DHFactor.py · ctc-eng/robotics-toolbox-python@fab4625 · GitHub
[go: up one dir, main page]
Skip to content

Commit fab4625

Browse files
Samuel-DrewSamuel-Drew
committed
Update DHFactor.py
Element Class v1
1 parent 18b61e8 commit fab4625

File tree

1 file changed

+146
-12
lines changed

1 file changed

+146
-12
lines changed

‎roboticstoolbox/tools/DHFactor.py

Lines changed: 146 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -60,13 +60,13 @@ def __init__(
6060
match = False
6161
for i in range(6):
6262
check = self.typeName[i].lower()
63-
if sType.lower == check:
63+
if sType.lower() == check:
6464
match = True
65+
self.eltype = i
6566
if not match:
6667
raise ValueError("bad transform name: " + sType)
67-
self.eltype = i
6868

69-
sRest = sRest[1:-2] # get the argument from between brackets
69+
sRest = sRest[1:-1] # get the argument from between brackets
7070

7171
# handle an optional minus sign
7272
negative = ""
@@ -100,14 +100,13 @@ def __init__(
100100
# if sign < 0:
101101
# self.negate()
102102

103-
104103
# one of TX, TY ... RZ, DH_STANDARD/MODIFIED
105104
if eltype:
106105
self.eltype = eltype
107-
108-
# transform parameters, only one of these is set
109-
if constant:
110-
self.constant = constant # eg. 90, for angles
106+
if constant:
107+
self.constant = constant # eg. 90, for angles
108+
if sign < 0:
109+
self.negate()
111110

112111
@staticmethod
113112
def showRuleUsage():
@@ -135,12 +134,21 @@ def axis(self):
135134
raise ValueError("bad transform type")
136135

137136
def symAdd(self, s1, s2):
138-
#TODO method for adding symbols
139-
print("symAdd not yet implemented")
140137

138+
if s1 is None and s2 is None:
139+
return None
140+
elif s1 and s2 is None:
141+
return s1
142+
elif s1 is None and s2:
143+
return s2
144+
else:
145+
if s2[0] == "-":
146+
return s1 + s2
147+
else:
148+
return s1 + "+" + s2
141149

142150
def add(self, e):
143-
if self.eltype != self.DH_Standard and self.eltype != self.DH_MODIFIED:
151+
if self.eltype != Element.DH_STANDARD and self.eltype != Element.DH_MODIFIED:
144152
raise ValueError("wrong element type " + str(self))
145153
print(" adding: " + str(self) + " += " + str(e))
146154
if e.eltype == self.RZ:
@@ -189,8 +197,134 @@ def factorMatch(self, dhWhich, i, verbose):
189197

190198

191199
def merge(self, e):
192-
200+
assert type(e) == Element, "merge(Element e)"
193201
"""
194202
don't merge if dissimilar transform or
195203
both are joint variables
196204
"""
205+
if e.eltype != self.eltype or e.isjoint() and self.isjoint():
206+
return self
207+
208+
sum = Element(self)
209+
210+
sum.var = self.symAdd(self.var, e.var)
211+
sum.symconst = self.symAdd(self.symconst, e.symconst)
212+
sum.constant = self.constant + e.constant
213+
214+
if not sum.isjoint() and sum.symconst is None and sum.constant == 0:
215+
print("Eliminate: " + self + " " + e)
216+
return None
217+
else:
218+
print("Merge: " + self + " " + e + " := " + sum)
219+
return sum
220+
221+
def swap(self, next, dhWhich):
222+
assert type(next) == Element, "type(next) == Element"
223+
224+
# don't swap if both are joint variables
225+
if self.isjoint() and next.isjoint():
226+
return False
227+
228+
if dhWhich == Element.DH_STANDARD:
229+
order = [2, 0, 3, 4, 0, 1]
230+
if self.eltype == Element.TZ and next.eltype == Element.TX or \
231+
self.eltype == Element.TX and next.eltype == Element.RX and next.isjoint() or \
232+
self.eltype == Element.TY and next.eltype == Element.RY and next.isjoint() or \
233+
self.eltype == Element.TZ and next.eltype == Element.RZ and next.isjoint() or \
234+
not self.isjoint() and self.eltype == Element.RX and next.eltype == Element.TX or \
235+
not self.isjoint() and self.eltype == Element.RY and next.eltype == Element.TY or \
236+
not self.isjoint() and not next.isjoint() and self.eltype == Element.TZ and next.eltype == Element.RZ or \
237+
self.eltype == Element.TY and next.eltype == Element.TZ or \
238+
self.eltype == Element.TY and next.eltype == Element.TX:
239+
print("Swap: " + self + " <-> " + next)
240+
return True
241+
elif dhWhich == Element.DH_MODIFIED:
242+
if self.eltype == Element.RX and next.eltype == Element.TX or \
243+
self.eltype == Element.RY and next.eltype == Element.TY or \
244+
self.eltype == Element.RZ and next.eltype == Element.TZ or \
245+
self.eltype == Element.TZ and next.eltype == Element.TX:
246+
print("Swap: " + self + " <-> " + next)
247+
return True
248+
else:
249+
raise ValueError("bad DH type")
250+
return False
251+
252+
# negate the arguments of the element
253+
def negate(self):
254+
255+
self.constant = -self.constant
256+
257+
if self.symconst:
258+
s = list(self.symconst)
259+
# if no leading sign character insert one (so we can flip it)
260+
if s[0] != "+" and s[0] != "-":
261+
s.insert(0, "+")
262+
for i in range(len(s)):
263+
if s[i] == "+":
264+
s[i] = "-"
265+
elif s[i] == "-":
266+
s[i] = "+"
267+
if s[0] == "+":
268+
s.pop(0)
269+
s = "".join(s)
270+
271+
'''
272+
Return a string representation of the parameters (argument)
273+
of the element, which can be a number, symbolic constant,
274+
or a joint variable.
275+
'''
276+
def argString(self):
277+
s = ""
278+
279+
if self.eltype == Element.RX or Element.RY or Element.RZ or \
280+
Element.TX or Element.TY or Element.TZ:
281+
if self.var:
282+
s = self.var
283+
if self.symconst:
284+
if self.var:
285+
if self.symconst[0] != "-":
286+
s = s + "+"
287+
s = s + self.symconst
288+
# constants always displayed with a sign character
289+
if self.constant != 0.0:
290+
if self.constant >= 0.0:
291+
s = s + "+" + '{0:.3f}'.format(self.constant)
292+
else:
293+
s = s + '{0:.3f}'.format(self.constant)
294+
elif self.eltype == Element.DH_STANDARD or Element.DH_MODIFIED:
295+
# theta, d, a, alpha
296+
# theta
297+
if self.prismatic == 0:
298+
# revolute joint
299+
s = s + self.var
300+
if self.offset >= 0:
301+
s = s + "+" + '{0:.3f}'.format(self.offset)
302+
elif self.offset < 0:
303+
s = s + '{0:.3f}'.format(self.offset)
304+
else:
305+
# prismatic joint
306+
s = s + '{0:.3f}'.format(self.theta)
307+
s = s + ", "
308+
309+
# d
310+
if self.prismatic > 0:
311+
s = s + self.var
312+
else:
313+
s = s + self.D if self.D else s + "0"
314+
s = s + ", "
315+
316+
# a
317+
s = s + self.A if self.A else s + "0"
318+
s = s + ", "
319+
320+
# alpha
321+
s = s + '{0:.3f}'.format(self.alpha)
322+
else:
323+
raise ValueError("bad Element type")
324+
return s
325+
326+
def toString(self):
327+
s = Element.typeName[self.eltype] + "("
328+
s = s + self.argString()
329+
s = s + ")"
330+
return s

0 commit comments

Comments
 (0)
0