Allow for closest_point
@@ -227,7 +227,8 @@ class DHRobot(Robot):
table.row(
angle(L.theta), qstr(j, L), f"{L.a:.4g}", angle(L.alpha), *ql)
else:
- table.row(qstr(j, L), f"{L.d:.4g}", f"{L.a:.4g}", angle(L.alpha), *ql)
+ table.row(qstr(j, L), f"{L.d:.4g}",
+ f"{L.a:.4g}", angle(L.alpha), *ql)
s += str(table)
@@ -302,6 +303,41 @@ class DHRobot(Robot):
# return new
+# --------------------------------------------------------------------- #
+
+ def _set_link_fk(self, q):
+ '''
+ robot._set_link_fk(q) evaluates fkine for each link within a
+ robot and stores that pose in a private variable within the link.
+
+ This method is not for general use.
+
+ :param q: The joint angles/configuration of the robot
+ :type q: float ndarray(n)
+
+ .. note::
+
+ - The robot's base transform, if present, are incorporated
+ into the result.
+ '''
+
+ q = getvector(q, self.n)
+
+ t = self.base
+
+ tall = self.fkine_all(q, old=True)
+
+ for i, link in enumerate(self.links):
+
+ # Update the link model transforms
+ for col in link.collision:
+ col.wT = tall[i]
+
+ for gi in link.geometry:
+ gi.wT = tall[i]
+
+# --------------------------------------------------------------------- #
+
@property
def mdh(self):
"""
@@ -525,8 +561,6 @@ class DHRobot(Robot):
"""
return 1
-
-
def A(self, j, q=None):
"""
Link forward kinematics
@@ -900,7 +934,6 @@ class DHRobot(Robot):
return T
-
def segments(self):
segments = [None]
@@ -1133,7 +1166,6 @@ class DHRobot(Robot):
raise ValueError('bad half specified')
return J0
-
def hessian0(self, q=None, J0=None, start=None, end=None):
r"""
Manipulator Hessian in base frame
@@ -1734,6 +1766,8 @@ class DHRobot(Robot):
if all([a not in config for a in allowable]):
config += allowable[0]
return config
+
+
class SerialLink(DHRobot):
def __init__(self, *args, **kwargs):
warnings.warn(
