From 7a7d4fc4f5e61ac542f8bc31faaa04435a660d33 Mon Sep 17 00:00:00 2001
From: Matthijs van der Burgh <matthijs.van.der.burgh@vbti.nl>
Date: Fri, 8 Sep 2023 10:01:37 +0200
Subject: [PATCH] (pykdl_ros) add Zero/Identity classmethods

---
 pykdl_ros/pykdl_ros/__init__.py | 48 +++++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py
index 558ddff..f3ee2c6 100644
--- a/pykdl_ros/pykdl_ros/__init__.py
+++ b/pykdl_ros/pykdl_ros/__init__.py
@@ -42,6 +42,18 @@ def __ne__(self, other):
     def __hash__(self):
         return hash((self.frame, self.header.frame_id))
 
+    @classmethod
+    def Identity(cls, stamp: Time, frame_id: str) -> FrameStamped:
+        """
+        Construct a FrameStamped object with identity frame.
+
+        :param stamp: TimeStamp
+        :param frame_id: Frame ID
+        :return: Filled object
+        """
+        frame = kdl.Frame.Identity()
+        return cls(frame, stamp, frame_id)
+
     @classmethod
     def from_xyz_rpy(
         cls, x: float, y: float, z: float, roll: float, pitch: float, yaw: float, stamp: Time, frame_id: str
@@ -101,6 +113,18 @@ def __ne__(self, other):
     def __hash__(self):
         return hash((self.twist, self.header.frame_id))
 
+    @classmethod
+    def Zero(cls, stamp: Time, frame_id: str) -> TwistStamped:
+        """
+        Construct a TwistStamped object with zero velocity and angular velocity.
+
+        :param stamp: TimeStamp
+        :param frame_id: Frame ID
+        :return: Filled object
+        """
+        twist = kdl.Twist.Zero()
+        return cls(twist, stamp, frame_id)
+
     @classmethod
     def from_xyz_rpy(cls, vx: float, vy: float, vz: float, wx: float, wy: float, wz: float, stamp: Time, frame_id: str):
         """
@@ -157,6 +181,18 @@ def __ne__(self, other):
     def __hash__(self):
         return hash((self.vector, self.header.frame_id))
 
+    @classmethod
+    def Zero(cls, stamp: Time, frame_id: str) -> VectorStamped:
+        """
+        Construct a VectorStamped object with zero vector.
+
+        :param stamp: TimeStamp
+        :param frame_id: Frame ID
+        :return: Filled object
+        """
+        vector = kdl.Vector.Zero()
+        return cls(vector, stamp, frame_id)
+
     @classmethod
     def from_xyz(cls, x: float, y: float, z: float, stamp: Time, frame_id: str) -> VectorStamped:
         """
@@ -219,6 +255,18 @@ def __ne__(self, other):
     def __hash__(self):
         return hash((self.wrench, self.header.frame_id))
 
+    @classmethod
+    def Zero(cls, stamp: Time, frame_id: str) -> WrenchStamped:
+        """
+        Construct a WrenchStamped object with zero force and torque.
+
+        :param stamp: TimeStamp
+        :param frame_id: Frame ID
+        :return: Filled object
+        """
+        wrench = kdl.Wrench.Zero()
+        return cls(wrench, stamp, frame_id)
+
     @classmethod
     def from_fxfyfz_txtytz(
         cls, fx: float, fy: float, fz: float, tx: float, ty: float, tz: float, stamp: Time, frame_id: str