|
| 1 | +import asyncio |
| 2 | +import os |
| 3 | +from typing import Any, ClassVar, Dict, Mapping, Optional, Self, Tuple |
| 4 | +from viam.components.arm import Arm, JointPositions, KinematicsFileFormat, Pose |
| 5 | +from viam.operations import run_with_operation |
| 6 | +from viam.proto.app.robot import ComponentConfig |
| 7 | +from viam.proto.common import ResourceName |
| 8 | +from viam.resource.base import ResourceBase |
| 9 | +from viam.resource.types import Model, ModelFamily |
| 10 | + |
| 11 | + |
| 12 | +class MyArm(Arm): |
| 13 | + # Subclass the Viam Arm component and implement the required functions |
| 14 | + MODEL: ClassVar[Model] = Model(ModelFamily("acme", "demo"), "myarm") |
| 15 | + |
| 16 | + def __init__(self, name: str): |
| 17 | + # Starting position |
| 18 | + self.position = Pose( |
| 19 | + x=0, |
| 20 | + y=0, |
| 21 | + z=0, |
| 22 | + o_x=0, |
| 23 | + o_y=0, |
| 24 | + o_z=1, |
| 25 | + theta=0, |
| 26 | + ) |
| 27 | + |
| 28 | + # Starting joint positions |
| 29 | + self.joint_positions = JointPositions(values=[0, 0, 0, 0, 0, 0]) |
| 30 | + self.is_stopped = True |
| 31 | + super().__init__(name) |
| 32 | + |
| 33 | + @classmethod |
| 34 | + def new(cls, config: ComponentConfig, dependencies: Mapping[ResourceName, ResourceBase]) -> Self: |
| 35 | + arm = cls(config.name) |
| 36 | + return arm |
| 37 | + |
| 38 | + async def get_end_position(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> Pose: |
| 39 | + return self.position |
| 40 | + |
| 41 | + @run_with_operation |
| 42 | + async def move_to_position( |
| 43 | + self, |
| 44 | + pose: Pose, |
| 45 | + extra: Optional[Dict[str, Any]] = None, |
| 46 | + **kwargs, |
| 47 | + ): |
| 48 | + operation = self.get_operation(kwargs) |
| 49 | + |
| 50 | + self.is_stopped = False |
| 51 | + |
| 52 | + # Simulate the length of time it takes for the arm to move to its new position |
| 53 | + for x in range(10): |
| 54 | + await asyncio.sleep(1) |
| 55 | + |
| 56 | + # Check if the operation is cancelled and, if it is, stop the arm's motion |
| 57 | + if await operation.is_cancelled(): |
| 58 | + await self.stop() |
| 59 | + break |
| 60 | + |
| 61 | + self.position = pose |
| 62 | + self.is_stopped = True |
| 63 | + |
| 64 | + async def get_joint_positions(self, extra: Optional[Dict[str, Any]] = None, **kwargs) -> JointPositions: |
| 65 | + return self.joint_positions |
| 66 | + |
| 67 | + @run_with_operation |
| 68 | + async def move_to_joint_positions(self, positions: JointPositions, extra: Optional[Dict[str, Any]] = None, **kwargs): |
| 69 | + operation = self.get_operation(kwargs) |
| 70 | + |
| 71 | + self.is_stopped = False |
| 72 | + |
| 73 | + # Simulate the length of time it takes for the arm to move to its new joint position |
| 74 | + for x in range(10): |
| 75 | + await asyncio.sleep(1) |
| 76 | + |
| 77 | + # Check if the operation is cancelled and, if it is, stop the arm's motion |
| 78 | + if await operation.is_cancelled(): |
| 79 | + await self.stop() |
| 80 | + break |
| 81 | + |
| 82 | + self.joint_positions = positions |
| 83 | + self.is_stopped = True |
| 84 | + |
| 85 | + async def stop(self, extra: Optional[Dict[str, Any]] = None, **kwargs): |
| 86 | + self.is_stopped = True |
| 87 | + |
| 88 | + async def is_moving(self) -> bool: |
| 89 | + return not self.is_stopped |
| 90 | + |
| 91 | + async def get_kinematics(self, **kwargs) -> Tuple[KinematicsFileFormat.ValueType, bytes]: |
| 92 | + dirname = os.path.dirname(__file__) |
| 93 | + filepath = os.path.join(dirname, "./xarm6_kinematics.json") |
| 94 | + with open(filepath, mode="rb") as f: |
| 95 | + file_data = f.read() |
| 96 | + return (KinematicsFileFormat.KINEMATICS_FILE_FORMAT_SVA, file_data) |
0 commit comments