Added gripper GetKinematics (#928)

Co-authored-by: viambot <[email protected]>

Author: gabegottlob

examples/server/v1/components.py

@@ -513,6 +513,7 @@ class ExampleGripper(Gripper):
     def __init__(self, name: str):
         self.opened = False
         self.is_stopped = True
+        self.kinematics = (KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA, b"\x00\x01\x02")
         super().__init__(name)
 
     async def open(self, extra: Optional[Dict[str, Any]] = None, **kwargs):
@@ -533,6 +534,9 @@ async def is_moving(self):
     async def get_geometries(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> List[Geometry]:
         return GEOMETRIES
 
+    async def get_kinematics(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
+        return self.kinematics
+
 
 class ExampleMotor(Motor):
     def __init__(self, name: str):

src/viam/components/arm/arm.py

@@ -194,7 +194,7 @@ async def is_moving(self) -> bool:
 
     @abc.abstractmethod
     async def get_kinematics(
-        self, *, extra: Optional[Dict[str, Any]] = None, timeout: Optional[float] = None
+        self, *, extra: Optional[Dict[str, Any]] = None, timeout: Optional[float] = None, **kwargs
     ) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
         """
         Get the kinematics information associated with the arm.

src/viam/components/gripper/__init__.py

@@ -1,3 +1,4 @@
+from viam.proto.common import KinematicsFileFormat
 from viam.resource.registry import Registry, ResourceRegistration
 
 from .client import GripperClient
@@ -6,6 +7,7 @@
 
 __all__ = [
     "Gripper",
+    "KinematicsFileFormat",
 ]
 
 Registry.register_api(ResourceRegistration(Gripper, GripperRPCService, lambda name, channel: GripperClient(name, channel)))

src/viam/components/gripper/client.py

@@ -1,8 +1,8 @@
-from typing import Any, Dict, List, Mapping, Optional
+from typing import Any, Dict, List, Mapping, Optional, Tuple
 
 from grpclib.client import Channel
 
-from viam.proto.common import DoCommandRequest, DoCommandResponse, Geometry
+from viam.proto.common import DoCommandRequest, DoCommandResponse, Geometry, GetKinematicsRequest, GetKinematicsResponse
 from viam.proto.component.gripper import (
     GrabRequest,
     GrabResponse,
@@ -17,6 +17,7 @@
 
 from .gripper import Gripper
 
+from . import KinematicsFileFormat
 
 class GripperClient(Gripper, ReconfigurableResourceRPCClientBase):
     """
@@ -83,3 +84,15 @@ async def do_command(
     async def get_geometries(self, *, extra: Optional[Dict[str, Any]] = None, timeout: Optional[float] = None, **kwargs) -> List[Geometry]:
         md = kwargs.get("metadata", self.Metadata())
         return await get_geometries(self.client, self.name, extra, timeout, md)
+
+    async def get_kinematics(
+        self,
+        *,
+        extra: Optional[Dict[str, Any]] = None,
+        timeout: Optional[float] = None,
+        **kwargs,
+    ) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
+        md = kwargs.get("metadata", self.Metadata()).proto
+        request = GetKinematicsRequest(name=self.name, extra=dict_to_struct(extra))
+        response: GetKinematicsResponse = await self.client.GetKinematics(request, timeout=timeout, metadata=md)
+        return (response.format, response.kinematics_data)

src/viam/components/gripper/gripper.py

@@ -1,9 +1,11 @@
 import abc
-from typing import Any, Dict, Final, Optional
+from typing import Any, Dict, Final, Optional, Tuple
 
 from viam.components.component_base import ComponentBase
 from viam.resource.types import API, RESOURCE_NAMESPACE_RDK, RESOURCE_TYPE_COMPONENT
 
+from . import KinematicsFileFormat
+
 
 class Gripper(ComponentBase):
     """
@@ -112,3 +114,37 @@ async def is_moving(self) -> bool:
         For more information, see `Gripper component <https://docs.viam.com/dev/reference/apis/components/gripper/#is_moving>`_.
         """
         ...
+
+    @abc.abstractmethod
+    async def get_kinematics(
+        self,
+        *,
+        extra: Optional[Dict[str, Any]] = None,
+        timeout: Optional[float] = None,
+        **kwargs,
+    ) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
+        """
+        Get the kinematics information associated with the gripper.
+
+        ::
+
+            my_gripper = Gripper.from_robot(robot=machine, name="my_gripper")
+
+            # Get the kinematics information associated with the gripper.
+            kinematics = await my_gripper.get_kinematics()
+
+            # Get the format of the kinematics file.
+            k_file = kinematics[0]
+
+            # Get the byte contents of the file.
+            k_bytes = kinematics[1]
+
+        Returns:
+            Tuple[KinematicsFileFormat.ValueType, bytes]: A tuple containing two values; the first [0] value represents the format of the
+            file, either in URDF format (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_URDF``) or
+            Viam's kinematic parameter format (spatial vector algebra) (``KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA``),
+            and the second [1] value represents the byte contents of the file.
+
+        For more information, see `Gripper component <https://docs.viam.com/dev/reference/apis/components/gripper/#getkinematics>`_.
+        """
+        ...

src/viam/components/gripper/service.py

@@ -1,6 +1,13 @@
 from grpclib.server import Stream
 
-from viam.proto.common import DoCommandRequest, DoCommandResponse, GetGeometriesRequest, GetGeometriesResponse
+from viam.proto.common import (
+    DoCommandRequest,
+    DoCommandResponse,
+    GetGeometriesRequest,
+    GetGeometriesResponse,
+    GetKinematicsRequest,
+    GetKinematicsResponse
+)
 from viam.proto.component.gripper import (
     GrabRequest,
     GrabResponse,
@@ -74,8 +81,17 @@ async def DoCommand(self, stream: Stream[DoCommandRequest, DoCommandResponse]) -
     async def GetGeometries(self, stream: Stream[GetGeometriesRequest, GetGeometriesResponse]) -> None:
         request = await stream.recv_message()
         assert request is not None
-        arm = self.get_resource(request.name)
+        gripper = self.get_resource(request.name)
         timeout = stream.deadline.time_remaining() if stream.deadline else None
-        geometries = await arm.get_geometries(extra=struct_to_dict(request.extra), timeout=timeout)
+        geometries = await gripper.get_geometries(extra=struct_to_dict(request.extra), timeout=timeout)
         response = GetGeometriesResponse(geometries=geometries)
         await stream.send_message(response)
+
+    async def GetKinematics(self, stream: Stream[GetKinematicsRequest, GetKinematicsResponse]) -> None:
+        request = await stream.recv_message()
+        assert request is not None
+        gripper = self.get_resource(request.name)
+        timeout = stream.deadline.time_remaining() if stream.deadline else None
+        format, kinematics_data = await gripper.get_kinematics(extra=struct_to_dict(request.extra), timeout=timeout)
+        response = GetKinematicsResponse(format=format, kinematics_data=kinematics_data)
+        await stream.send_message(response)