Further helpers to add / subtract xforms

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)