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

Commit a48941c

Browse files
Samuel-DrewSamuel-Drew
committed
Update DHFactor.py
DHFactor successfully outputs DH parameters
1 parent 9063655 commit a48941c

File tree

1 file changed

+60
-58
lines changed

1 file changed

+60
-58
lines changed

‎roboticstoolbox/tools/DHFactor.py

Lines changed: 60 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -34,20 +34,20 @@ def __init__(
3434
elementIn=None,
3535
stringIn=None,
3636
eltype=None,
37-
constant=None,
37+
constant=0.0,
3838
sign=0):
3939

4040
self.var = None # eg. q1, for joint var types
4141
self.symconst = None # eg. L1, for lengths
4242

4343
# DH parameters, only set if type is DH_STANDARD/MODIFIED
44-
self.theta = None
45-
self.alpha = None
46-
self.A = None
47-
self.D = None
44+
self.theta = 0
45+
self.alpha = 0
46+
self.A = 0
47+
self.D = 0
4848
self.prismatic = None
4949
self.offset = None
50-
self.constant = None
50+
self.constant = 0.0
5151

5252
if stringIn:
5353
if elementIn or eltype or constant:
@@ -103,10 +103,9 @@ def __init__(
103103
# self.negate()
104104

105105
# one of TX, TY ... RZ, DH_STANDARD/MODIFIED
106-
if eltype:
106+
if eltype is not None:
107107
self.eltype = eltype
108-
if constant:
109-
self.constant = constant # eg. 90, for angles
108+
self.constant = constant # eg. 90, for angles
110109
if sign < 0:
111110
self.negate()
112111

@@ -156,14 +155,14 @@ def add(self, e):
156155
if self.eltype != Element.DH_STANDARD and self.eltype != Element.DH_MODIFIED:
157156
raise ValueError("wrong element type " + str(self))
158157
print(" adding: " + str(self) + " += " + str(e))
159-
print("prismatic= " + str(self.prismatic))
160-
print("var= " + str(self.var))
161-
print("offset= " + str(self.offset))
162-
print("theta= " + str(self.theta))
163-
print("a= " + str(self.A))
164-
print("d= " + str(self.D))
165-
print("e.symconst= " + str(e.symconst))
166-
print("e.var= " + str(e.var))
158+
# print("prismatic= " + str(self.prismatic))
159+
# print("var= " + str(self.var))
160+
# print("offset= " + str(self.offset))
161+
# print("theta= " + str(self.theta))
162+
# print("a= " + str(self.A))
163+
# print("d= " + str(self.D))
164+
# print("e.symconst= " + str(e.symconst))
165+
# print("e.var= " + str(e.var))
167166
if e.eltype == self.RZ:
168167
if e.isjoint():
169168
self.prismatic = 0
@@ -325,45 +324,45 @@ def substituteY(self, prev, next):
325324
# note that if the rotation is -90 we must make the displacement -ve
326325
if prev.eltype == Element.RX and self.eltype == Element.TY:
327326
# RX.TY -> TZ.RX
328-
s.append(Element(elementIn=self, eltype=Element.TZ, constant=prev.constant))
327+
s.append(Element(elementIn=self, eltype=Element.TZ, sign=prev.constant))
329328
s.append(Element(elementIn=prev))
330329
self.rules[0] += 1
331330
return s
332331
elif prev.eltype == Element.RX and self.eltype == Element.TZ:
333332
# RX.TZ -> TY.RX
334-
s.append(Element(elementIn=self, eltype=Element.TY, constant=-prev.constant))
333+
s.append(Element(elementIn=self, eltype=Element.TY, sign=-prev.constant))
335334
s.append(Element(elementIn=prev))
336335
self.rules[2] += 1
337336
return s
338337
elif prev.eltype == Element.RY and self.eltype == Element.TX:
339338
# RY.TX -> TZ.RY
340-
s.append(Element(elementIn=self, eltype=Element.TZ, constant=-prev.constant))
339+
s.append(Element(elementIn=self, eltype=Element.TZ, sign=-prev.constant))
341340
s.append(Element(elementIn=prev))
342341
self.rules[1] += 1
343342
return s
344343
elif prev.eltype == Element.RY and self.eltype == Element.TZ:
345344
# RY.TZ -> TX.RY
346-
s.append(Element(elementIn=self, eltype=Element.TX, constant=prev.constant))
345+
s.append(Element(elementIn=self, eltype=Element.TX, sign=prev.constant))
347346
s.append(Element(elementIn=prev))
348347
self.rules[11] += 1
349348
return s
350349
elif prev.eltype == Element.TY and self.eltype == Element.RX:
351350
# TY.RX -> RX.TZ
352351
s.append(Element(elementIn=prev))
353-
s.append(Element(elementIn=self, eltype=Element.TZ, constant=-self.constant))
352+
s.append(Element(elementIn=self, eltype=Element.TZ, sign=-self.constant))
354353
self.rules[5] += 1
355354
# return s
356355
return None
357356
elif prev.eltype == Element.TX and self.eltype == Element.RZ:
358357
# TX.RZ -> RZ.TY
359358
s.append(Element(elementIn=prev))
360-
s.append(Element(elementIn=self, eltype=Element.TY, constant=self.constant))
359+
s.append(Element(elementIn=self, eltype=Element.TY, sign=self.constant))
361360
self.rules[5] += 1
362361
return s
363362
elif prev.eltype == Element.RY and self.eltype == Element.RX:
364363
# RY(Q).RX -> RX.RZ(-Q)
365364
s.append(Element(elementIn=prev))
366-
s.append(Element(elementIn=self, eltype=Element.RZ, constant=-1))
365+
s.append(Element(elementIn=self, eltype=Element.RZ, sign=-1))
367366
self.rules[3] += 1
368367
return s
369368
elif prev.eltype == Element.RX and self.eltype == Element.RY:
@@ -384,7 +383,8 @@ def substituteY(self, prev, next):
384383
# negate the arguments of the element
385384
def negate(self):
386385

387-
self.constant = -self.constant
386+
if self.constant:
387+
self.constant = -self.constant
388388

389389
if self.symconst:
390390
s = list(self.symconst)
@@ -396,9 +396,9 @@ def negate(self):
396396
s[i] = "-"
397397
elif s[i] == "-":
398398
s[i] = "+"
399-
if s[0] == "+":
400-
s.pop(0)
401-
s = "".join(s)
399+
if s[0] == "+":
400+
s.pop(0)
401+
self.symconst = "".join(s)
402402

403403
'''
404404
Return a string representation of the parameters (argument)
@@ -408,8 +408,8 @@ def negate(self):
408408
def argString(self):
409409
s = ""
410410

411-
if self.eltype == Element.RX or Element.RY or Element.RZ or \
412-
Element.TX or Element.TY or Element.TZ:
411+
if self.eltype == Element.RX or self.eltype == Element.RY or self.eltype == Element.RZ or \
412+
self.eltype == Element.TX or self.eltype == Element.TY or self.eltype == Element.TZ:
413413
if self.var:
414414
s = self.var
415415
if self.symconst:
@@ -423,34 +423,35 @@ def argString(self):
423423
s = s + "+" + '{0:.3f}'.format(self.constant)
424424
else:
425425
s = s + '{0:.3f}'.format(self.constant)
426-
elif self.eltype == Element.DH_STANDARD or Element.DH_MODIFIED:
426+
elif self.eltype == Element.DH_STANDARD or self.eltype==Element.DH_MODIFIED:
427427
# theta, d, a, alpha
428428
# theta
429-
if self.prismatic == 0:
429+
if not self.prismatic:
430430
# revolute joint
431-
s = s + self.var
432-
if self.offset >= 0:
433-
s = s + "+" + '{0:.3f}'.format(self.offset)
434-
elif self.offset < 0:
435-
s = s + '{0:.3f}'.format(self.offset)
431+
s += str(self.var)
432+
if self.offset:
433+
if self.offset >= 0:
434+
s = s + "+" + '{0:.3f}'.format(self.offset)
435+
elif self.offset < 0:
436+
s = s + '{0:.3f}'.format(self.offset)
436437
else:
437438
# prismatic joint
438-
s = s + '{0:.3f}'.format(self.theta)
439-
s = s + ", "
439+
s += '{0:.3f}'.format(self.theta)
440+
s += ", "
440441

441442
# d
442-
if self.prismatic > 0:
443-
s = s + self.var
443+
if self.prismatic:
444+
s += str(self.var)
444445
else:
445-
s = s + self.D if self.D else s + "0"
446-
s = s + ", "
446+
s += str(self.D) if self.D else "0"
447+
s += ", "
447448

448449
# a
449-
s = s + self.A if self.A else s + "0"
450-
s = s + ", "
450+
s += str(self.A) if self.A else "0"
451+
s += ", "
451452

452453
# alpha
453-
s = s + '{0:.3f}'.format(self.alpha)
454+
s += '{0:.3f}'.format(self.alpha)
454455
else:
455456
raise ValueError("bad Element type")
456457
return s
@@ -843,11 +844,11 @@ class DHFactor():
843844

844845
def __init__(self, src):
845846
self.results = ElementList()
846-
try:
847-
self.results = self.parseString(src)
848-
print("In DHFactor, parseString is done")
849-
except ValueError:
850-
print("Value Error")
847+
# try:
848+
self.results = self.parseString(src)
849+
print("In DHFactor, parseString is done")
850+
# except ValueError:
851+
# print("Value Error")
851852
if not self.isValid():
852853
print("DHFactor: error: Incomplete factorization, no DH equivalent found")
853854

@@ -917,17 +918,18 @@ def dh(self):
917918
else:
918919
# revolute joint
919920
theta = "0"
920-
d = "0" if e.D is None else e.D
921+
d = "0" if e.D is None else str(e.D)
921922

922923
s += "["
923-
s += theta
924-
s += ", "
925-
s += d
924+
if e.prismatic:
925+
s += theta
926+
else:
927+
s += d
926928
s += ", "
927-
s += "0" if e.A is None else e.A
929+
s += "0" if e.A is None else str(e.A)
928930
s += ", "
929931
s += self.angle(e.alpha)
930-
s += ", " + e.prismatic
932+
s += ", " + str(e.prismatic)
931933
s += "],"
932934
s += "]"
933935
return s
@@ -1011,7 +1013,7 @@ def parseString(self, buffer):
10111013
l.simplify()
10121014
print(l)
10131015

1014-
l.factorize(Element.DH_STANDARD, 1)
1016+
l.factorize(Element.DH_STANDARD, verbose=0)
10151017
print(l)
10161018

10171019
return l

0 commit comments

Comments
 (0)
0