From 8e79f476b96896907276a9c1260f97f5fdef811f Mon Sep 17 00:00:00 2001 From: JackOfSome Date: Thu, 8 Aug 2019 22:54:03 -0700 Subject: [PATCH] Further helpers to add / subtract xforms --- tiny_tf/tf.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/tiny_tf/tf.py b/tiny_tf/tf.py index f40e644..6907c0b 100644 --- a/tiny_tf/tf.py +++ b/tiny_tf/tf.py @@ -149,6 +149,14 @@ def from_position_euler(cls, x, y, z, roll, pitch, yaw): qx, qy, qz, qw = tft.quaternion_from_euler(roll, pitch, yaw) return cls(x, y, z, qx, qy, qz, qw) + @classmethod + def from_xyt(cls, x, y, t): + return Transform.from_position_euler(x, y, 0, 0, 0, t) + + @classmethod + def from_pose2d(cls, p): + return Transform.from_position_euler(p.x, p.y, 0, 0, 0, p.yaw) + @classmethod def from_dict(cls, indict): x, y, z = indict['xyz'] @@ -171,6 +179,12 @@ def rotation_matrix(self): def position(self): return self.x, self.y, self.z + def __add__(self, o): + return Transform.from_matrix(self.matrix @ o.matrix) + + def __sub__(self, o): + return Transform.from_matrix(o.inverse().matrix @ self.matrix) + @property def euler(self): return tft.euler_from_quaternion(self.quaternion)