RSDK-3788 - add getkinematics functionality in arm (#335)

examples/module/src/arm/__init__.py

@@ -0,0 +1,9 @@
+"""
+This file registers the MyArm model with the Python SDK.
+"""
+
+from viam.components.arm import Arm
+from viam.resource.registry import Registry, ResourceCreatorRegistration
+from .my_arm import MyArm
+
+Registry.register_resource_creator(Arm.SUBTYPE, MyArm.MODEL, ResourceCreatorRegistration(MyArm.new))

examples/module/src/arm/my_arm.py

@@ -0,0 +1,96 @@
+import asyncio
+import os
+from typing import Any, ClassVar, Dict, Mapping, Optional, Self, Tuple
+from viam.components.arm import Arm, JointPositions, KinematicsFileFormat, Pose
+from viam.operations import run_with_operation
+from viam.proto.app.robot import ComponentConfig
+from viam.proto.common import ResourceName
+from viam.resource.base import ResourceBase
+from viam.resource.types import Model, ModelFamily
+
+
+class MyArm(Arm):
+    # Subclass the Viam Arm component and implement the required functions
+    MODEL: ClassVar[Model] = Model(ModelFamily("acme", "demo"), "myarm")
+
+    def __init__(self, name: str):
+        # Starting position
+        self.position = Pose(
+            x=0,
+            y=0,
+            z=0,
+            o_x=0,
+            o_y=0,
+            o_z=1,
+            theta=0,
+        )
+
+        # Starting joint positions
+        self.joint_positions = JointPositions(values=[0, 0, 0, 0, 0, 0])
+        self.is_stopped = True
+        super().__init__(name)
+
+    @classmethod
+    def new(cls, config: ComponentConfig, dependencies: Mapping[ResourceName, ResourceBase]) -> Self:
+        arm = cls(config.name)
+        return arm
+
+    async def get_end_position(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> Pose:
+        return self.position
+
+    @run_with_operation
+    async def move_to_position(
+        self,
+        pose: Pose,
+        extra: Optional[Dict[str, Any]] = None,
+        **kwargs,
+    ):
+        operation = self.get_operation(kwargs)
+
+        self.is_stopped = False
+
+        # Simulate the length of time it takes for the arm to move to its new position
+        for x in range(10):
+            await asyncio.sleep(1)
+
+            # Check if the operation is cancelled and, if it is, stop the arm's motion
+            if await operation.is_cancelled():
+                await self.stop()
+                break
+
+        self.position = pose
+        self.is_stopped = True
+
+    async def get_joint_positions(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> JointPositions:
+        return self.joint_positions
+
+    @run_with_operation
+    async def move_to_joint_positions(self, positions: JointPositions, extra: Optional[Dict[str, Any]] = None, **kwargs):
+        operation = self.get_operation(kwargs)
+
+        self.is_stopped = False
+
+        # Simulate the length of time it takes for the arm to move to its new joint position
+        for x in range(10):
+            await asyncio.sleep(1)
+
+            # Check if the operation is cancelled and, if it is, stop the arm's motion
+            if await operation.is_cancelled():
+                await self.stop()
+                break
+
+        self.joint_positions = positions
+        self.is_stopped = True
+
+    async def stop(self, extra: Optional[Dict[str, Any]] = None, **kwargs):
+        self.is_stopped = True
+
+    async def is_moving(self) -> bool:
+        return not self.is_stopped
+
+    async def get_kinematics(self, **kwargs) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
+        dirname = os.path.dirname(__file__)
+        filepath = os.path.join(dirname, "./xarm6_kinematics.json")
+        with open(filepath, mode="rb") as f:
+            file_data = f.read()
+        return (KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA, file_data)

examples/module/src/arm/xarm6_kinematics.json

@@ -0,0 +1,217 @@
+{
+    "name": "xArm6",
+    "links": [
+        {
+            "id": "base",
+            "parent": "world",
+            "translation": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            }
+        },
+        {
+            "id": "base_top",
+            "parent": "waist",
+            "translation": {
+                "x": 0,
+                "y": 0,
+                "z": 267
+            },
+            "geometry": {
+                "r": 50,
+                "l": 320,
+                "translation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 160
+                }
+            }
+        },
+        {
+            "id": "upper_arm",
+            "parent": "shoulder",
+            "translation": {
+                "x": 53.5,
+                "y": 0,
+                "z": 284.5
+            },
+            "geometry": {
+                "x": 110,
+                "y": 190,
+                "z": 370,
+                "translation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 135
+                }
+            }
+        },
+        {
+            "id": "upper_forearm",
+            "parent": "elbow",
+            "translation": {
+                "x": 77.5,
+                "y": 0,
+                "z": -172.5
+            },
+            "geometry": {
+                "x": 100,
+                "y": 190,
+                "z": 250,
+                "translation": {
+                    "x": 49.49,
+                    "y": 0,
+                    "z": -49.49
+                },
+                "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                        "x": 0.707106,
+                        "y": 0,
+                        "z": -0.707106,
+                        "th": 0
+                    }
+                }
+            }
+        },
+        {
+            "id": "lower_forearm",
+            "parent": "forearm_rot",
+            "translation": {
+                "x": 0,
+                "y": 0,
+                "z": -170
+            },
+            "geometry": {
+                "r": 45,
+                "l": 285,
+                "translation": {
+                    "x": 0,
+                    "y": -27.5,
+                    "z": -104.8
+                },
+                "orientation": {
+                    "type": "ov_degrees",
+                    "value": {
+                        "th": -90,
+                        "x": 0,
+                        "y": 0.2537568,
+                        "z": 0.9672615
+                    }
+                }
+            }
+        },
+        {
+            "id": "wrist_link",
+            "parent": "wrist",
+            "translation": {
+                "x": 76,
+                "y": 0,
+                "z": -97
+            },
+            "geometry": {
+                "x": 150,
+                "y": 100,
+                "z": 135,
+                "translation": {
+                    "x": 75,
+                    "y": 10,
+                    "z": -67.5
+                }
+            }
+        },
+        {
+            "id": "gripper_mount",
+            "parent": "gripper_rot",
+            "translation": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "orientation": {
+                "type": "ov_degrees",
+                "value": {
+                    "x": 0,
+                    "y": 0,
+                    "z": -1,
+                    "th": 0
+                }
+            }
+        }
+    ],
+    "joints": [
+        {
+            "id": "waist",
+            "type": "revolute",
+            "parent": "base",
+            "axis": {
+                "x": 0,
+                "y": 0,
+                "z": 1
+            },
+            "max": 359,
+            "min": -359
+        },
+        {
+            "id": "shoulder",
+            "type": "revolute",
+            "parent": "base_top",
+            "axis": {
+                "x": 0,
+                "y": 1,
+                "z": 0
+            },
+            "max": 120,
+            "min": -118
+        },
+        {
+            "id": "elbow",
+            "type": "revolute",
+            "parent": "upper_arm",
+            "axis": {
+                "x": 0,
+                "y": 1,
+                "z": 0
+            },
+            "max": 10,
+            "min": -225
+        },
+        {
+            "id": "forearm_rot",
+            "type": "revolute",
+            "parent": "upper_forearm",
+            "axis": {
+                "x": 0,
+                "y": 0,
+                "z": -1
+            },
+            "max": 359,
+            "min": -359
+        },
+        {
+            "id": "wrist",
+            "type": "revolute",
+            "parent": "lower_forearm",
+            "axis": {
+                "x": 0,
+                "y": 1,
+                "z": 0
+            },
+            "max": 179,
+            "min": -97
+        },
+        {
+            "id": "gripper_rot",
+            "type": "revolute",
+            "parent": "wrist_link",
+            "axis": {
+                "x": 0,
+                "y": 0,
+                "z": -1
+            },
+            "max": 359,
+            "min": -359
+        }
+    ]
+}

examples/module/src/main.py

@@ -1,8 +1,10 @@
 import asyncio
 
 from viam.module.module import Module
+from viam.components.arm import Arm
 
 from .gizmo import Gizmo, MyGizmo
+from .arm import MyArm
 from .summation import MySummationService, SummationService
 
 
@@ -14,6 +16,7 @@ async def main():
     module = Module.from_args()
     module.add_model_from_registry(Gizmo.SUBTYPE, MyGizmo.MODEL)
     module.add_model_from_registry(SummationService.SUBTYPE, MySummationService.MODEL)
+    module.add_model_from_registry(Arm.SUBTYPE, MyArm.MODEL)
     await module.start()
 
 

examples/server/v1/components.py

@@ -40,6 +40,7 @@
     BoardStatus,
     DigitalInterruptStatus,
     GeoPoint,
+    KinematicsFileFormat,
     Orientation,
     Pose,
     PoseInFrame,
@@ -63,6 +64,7 @@ def __init__(self, name: str):
         )
         self.joint_positions = JointPositions(values=[0, 0, 0, 0, 0, 0])
         self.is_stopped = True
+        self.kinematics = (KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA, b"\x00\x01\x02")
         super().__init__(name)
 
     async def get_end_position(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> Pose:
@@ -90,6 +92,9 @@ async def stop(self, extra: Optional[Dict[str, Any]] = None, **kwargs):
     async def is_moving(self):
         return not self.is_stopped
 
+    async def get_kinematics(self, **kwargs) -> Tuple[KinematicsFileFormat.ValueType, bytes]:
+        return self.kinematics
+
 
 class ExampleAudioInput(AudioInput):
     def __init__(self, name: str):

src/viam/components/arm/__init__.py

@@ -1,6 +1,6 @@
 import asyncio
 
-from viam.proto.common import Pose
+from viam.proto.common import KinematicsFileFormat, Pose
 from viam.proto.component.arm import JointPositions
 from viam.proto.component.arm import Status as ArmStatus
 from viam.proto.robot import Status
@@ -14,16 +14,13 @@
 __all__ = [
     "Arm",
     "JointPositions",
+    "KinematicsFileFormat",
     "Pose",
 ]
 
 
 async def create_status(component: Arm) -> Status:
-    (
-        end_position,
-        joint_positions,
-        is_moving,
-    ) = await asyncio.gather(
+    (end_position, joint_positions, is_moving,) = await asyncio.gather(
         component.get_end_position(),
         component.get_joint_positions(),
         component.is_moving(),