reworked fkine_all for DH class
@@ -926,23 +926,10 @@ class DHRobot(Robot):
"""
q = self._getq(q)
- if self._base is not None:
- Tj = self._base
- else:
- Tj = SE3()
- first = True
+ Tj = self.base.copy()
Tall = Tj
for q, L in zip(q, self.links):
- if first:
-
- Tj *= L.A(q)
- if old:
- Tall = Tj
- else:
- Tall.append(Tj)
- first = False
- else:
Tj *= L.A(q)
Tall.append(Tj)
return Tall
