Merge pull request #66 from robofit/devel

Devel

arcor2_kinali/VERSION

@@ -1 +1 @@
-0.6.0
+0.7.1

arcor2_kinali/data/interaction.py

@@ -1,6 +1,8 @@
 from dataclasses import dataclass
-from arcor2.data.common import StrEnum
 from typing import List
+
+from arcor2.data.common import StrEnum
+
 from dataclasses_jsonschema import JsonSchemaMixin
 
 

arcor2_kinali/data/robot.py

@@ -1,9 +1,9 @@
-from typing import List
 from dataclasses import dataclass
+from typing import List
 
-from dataclasses_jsonschema import JsonSchemaMixin
+from arcor2.data.common import Joint, Orientation, Pose, Position, StrEnum
 
-from arcor2.data.common import Pose, Joint, StrEnum, Position, Orientation
+from dataclasses_jsonschema import JsonSchemaMixin
 
 
 class MoveTypeEnum(StrEnum):

arcor2_kinali/data/search.py

@@ -1,10 +1,10 @@
 from dataclasses import dataclass
 from typing import List, Optional
 
-from dataclasses_jsonschema import JsonSchemaMixin
-
 from arcor2.data.common import Pose, StrEnum
 
+from dataclasses_jsonschema import JsonSchemaMixin
+
 
 @dataclass
 class ScoredItem(JsonSchemaMixin):

arcor2_kinali/object_types/rest_robot.py

@@ -1,14 +1,14 @@
-from typing import Iterator, FrozenSet, Optional, List
+from typing import FrozenSet, Iterator, List, Optional
 
-from arcor2.object_types import Robot
-from arcor2.data.common import Pose, ActionMetadata, Joint, ProjectRobotJoints
-from arcor2.data.object_type import MeshFocusAction, Models
-from arcor2_kinali.services.robot import RestRobotService, MoveTypeEnum
 from arcor2.action import action
+from arcor2.data.common import ActionMetadata, Joint, Pose, ProjectRobotJoints
+from arcor2.data.object_type import MeshFocusAction, Models
 from arcor2.exceptions import Arcor2Exception
-
+from arcor2.object_types import Robot
 from arcor2.parameter_plugins.relative_pose import RelativePose
 
+from arcor2_kinali.services.robot import MoveTypeEnum, RestRobotService
+
 # TODO how to copy docstrings of methods from service?
 
 
@@ -45,6 +45,12 @@ def stop(self) -> None:
     def robot_joints(self) -> List[Joint]:
         return self.robot_api.robot_joints(self.id)
 
+    def move_to_pose(self, end_effector_id: str, target_pose: Pose, speed: float) -> None:
+        self.robot_api.move_to_pose(self.id, end_effector_id, target_pose, speed)
+
+    def move_to_joints(self, target_joints: List[Joint], speed: float) -> None:
+        self.robot_api.move_to_joints(self.id, target_joints, speed)
+
     @action
     def move(self, end_effector_id: str, pose: Pose, move_type: MoveTypeEnum, speed: float = 0.5) -> None:
         """

arcor2_kinali/services/__init__.py

@@ -1,5 +1,5 @@
+from arcor2_kinali.services import systems
 from arcor2_kinali.services.robot import RestRobotService
 from arcor2_kinali.services.search import SearchService
-from arcor2_kinali.services import systems
 
 __all__ = ["RestRobotService", "SearchService", "systems"]

arcor2_kinali/services/barcode.py

@@ -1,10 +1,10 @@
-from typing import FrozenSet, List
 import os
+from typing import FrozenSet, List
 
-from arcor2.services import Service
-from arcor2.data.common import ActionMetadata
-from arcor2.action import action
 from arcor2 import rest
+from arcor2.action import action
+from arcor2.data.common import ActionMetadata
+from arcor2.services.service import Service
 
 from arcor2_kinali.services import systems
 

arcor2_kinali/services/interaction.py

@@ -1,10 +1,10 @@
-from typing import FrozenSet, List
 import os
+from typing import FrozenSet, List
 
-from arcor2.services import Service
-from arcor2.data.common import ActionMetadata
-from arcor2.action import action
 from arcor2 import rest
+from arcor2.action import action
+from arcor2.data.common import ActionMetadata
+from arcor2.services.service import Service
 
 from arcor2_kinali.data.interaction import DialogValue, NotificationLevelEnum, NotificationValue
 

arcor2_kinali/services/robot.py

@@ -1,16 +1,16 @@
-from typing import FrozenSet, List, TYPE_CHECKING, TypeVar, Callable
 import os
+from typing import Callable, FrozenSet, List, TYPE_CHECKING, TypeVar
 
-from arcor2.services import RobotService
-from arcor2.data.common import Pose, ActionMetadata, ProjectRobotJoints, Joint
-from arcor2.action import action
 from arcor2 import rest
+from arcor2.action import action
+from arcor2.data.common import ActionMetadata, Joint, Pose, ProjectRobotJoints
+from arcor2.data.object_type import MeshFocusAction, Model3dType, Models
 from arcor2.object_types import Generic
-from arcor2.data.object_type import Model3dType, MeshFocusAction
 from arcor2.parameter_plugins.relative_pose import RelativePose
+from arcor2.services.robot_service import RobotService
 
+from arcor2_kinali.data.robot import MoveRelativeJointsParameters, MoveRelativeParameters, MoveTypeEnum
 from arcor2_kinali.services import systems
-from arcor2_kinali.data.robot import MoveTypeEnum, MoveRelativeJointsParameters, MoveRelativeParameters
 
 
 URL = os.getenv("REST_ROBOT_SERVICE_URL", "http://127.0.0.1:13000")
@@ -25,11 +25,6 @@ def lru_cache(maxsize: int = 128, typed: bool = False) -> Callable[[F], F]:
     from functools import lru_cache
 
 
-def collision_id(obj: Generic) -> str:
-    assert obj.collision_model is not None
-    return obj.collision_model.type().value + "-" + obj.id
-
-
 class RestRobotService(RobotService):
     """
     REST interface to the robot service.
@@ -47,32 +42,73 @@ def cleanup(self) -> None:
     def get_configuration_ids() -> FrozenSet[str]:
         return systems.systems(URL)
 
-    def add_collision(self, obj: Generic) -> None:
-        if not obj.collision_model or obj.collision_model.type() == Model3dType.NONE:
-            return
-        params = obj.collision_model.to_dict()
+    def add_collision_model(self, model: Models, pose: Pose) -> None:
+        """
+        Adds arbitrary collision model to the collision scene.
+        :param model: Box, Sphere, Cylinder, Mesh
+        :param pose: Pose of the collision object.
+        :return:
+
+        Example usage:
+
+        >>> from arcor2.data.object_type import Box
+        >>> from arcor2.data.common import Pose, Position. Orientation
+        >>> box = Box("UniqueBoxId", 0.1, 0.1, 0.1)
+        >>> robot_service.add_collision_model(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1)))
+
+        """
+
+        model_id = model.id
+        params = model.to_dict()
         del params["id"]
-        params[obj.collision_model.__class__.__name__.lower() + "Id"] = collision_id(obj)
+        params[model.__class__.__name__.lower() + "Id"] = model_id
 
         # TODO temporary hack
-        if obj.collision_model.type() == Model3dType.MESH:
+        if model.type() == Model3dType.MESH:
             params["mesh_scale_x"] = 1.0
             params["mesh_scale_y"] = 1.0
             params["mesh_scale_z"] = 1.0
             params["transform_id"] = "world"
 
-        rest.put(f"{URL}/collisions/{obj.collision_model.type().value}", obj.pose, params)
+        rest.put(f"{URL}/collisions/{model.type().value}", pose, params)
+
+    def remove_collision_model_id(self, model_id: str) -> None:
+
+        rest.delete(f"{URL}/collisions/{model_id}")
+
+    def add_collision(self, obj: Generic) -> None:
+        """
+        Adds object's collision model to the collision scene.
+        :param obj: Scene object.
+        :return:
+        """
+
+        if not obj.collision_model or obj.collision_model.type() == Model3dType.NONE:
+            return
+
+        self.add_collision_model(obj.collision_model, obj.pose)
 
     def remove_collision(self, obj: Generic) -> None:
         if not obj.collision_model or obj.collision_model.type() == Model3dType.NONE:
             return
-        rest.delete(f"{URL}/collisions/{collision_id(obj)}")
+
+        self.remove_collision_model_id(obj.collision_model.id)
 
     def clear_collisions(self) -> None:
 
         for coll_id in rest.get_list_primitive(f"{URL}/collisions", str):
             rest.delete(f"{URL}/collisions/{coll_id}")
 
+    def move_to_pose(self, robot_id: str, end_effector_id: str, target_pose: Pose, speed: float) -> None:
+
+        rest.put(f"{URL}/robots/{robot_id}/endeffectors/{end_effector_id}/move", target_pose,
+                 {"moveType": MoveTypeEnum.AVOID_COLLISIONS.value, "speed": speed})
+
+    def move_to_joints(self, robot_id: str, target_joints: List[Joint], speed: float) -> None:
+
+        rest.put(f"{URL}/robots/{robot_id}/joints", target_joints,
+                 {"moveType": MoveTypeEnum.AVOID_COLLISIONS.value, "speed": speed})
+
     @lru_cache()
     def get_robot_ids(self) -> FrozenSet[str]:
         return frozenset(rest.get_data(f"{URL}/robots"))

arcor2_kinali/services/search.py

@@ -1,15 +1,15 @@
-from typing import FrozenSet, List
 import os
+from typing import FrozenSet, List
 
 from PIL.Image import Image  # type: ignore
 
-from arcor2.services import Service
-from arcor2.data.common import Pose, ActionMetadata
-from arcor2.action import action
 from arcor2 import rest
+from arcor2.action import action
+from arcor2.data.common import ActionMetadata, Pose
+from arcor2.services.service import Service
 
+from arcor2_kinali.data.search import GripperSetup, SearchEngineParameters, SearchOutput
 from arcor2_kinali.services import systems
-from arcor2_kinali.data.search import SearchOutput, GripperSetup, SearchEngineParameters
 
 
 URL = os.getenv("SEARCH_SERVICE_URL", "http://127.0.0.1:12000")

arcor2_kinali/services/statistic.py

@@ -1,10 +1,11 @@
-from typing import FrozenSet, List
 import os
+from typing import FrozenSet, List
 
-from arcor2.services import Service
-from arcor2.data.common import ActionMetadata
-from arcor2.action import action
 from arcor2 import rest
+from arcor2.action import action
+from arcor2.data.common import ActionMetadata
+from arcor2.services.service import Service
+
 
 from arcor2_kinali.data.statistic import StatisticValue
 

arcor2_kinali/services/systems.py

@@ -1,8 +1,8 @@
-from typing import Optional, FrozenSet
+from typing import FrozenSet, Optional
 
 from arcor2 import rest
-from arcor2.services import Service
 from arcor2.exceptions import Arcor2Exception
+from arcor2.services.service import Service
 
 
 class SystemsException(Arcor2Exception):

setup.cfg

@@ -1,2 +1,9 @@
 [flake8]
 max-line-length = 120
+application-import-names = arcor2_kinali
+
+[metadata]
+description-file = README.md
+
+[mypy]
+python_version = 3.8

setup.py

@@ -18,7 +18,7 @@
     install_requires=[
         'dataclasses',
         'dataclasses-jsonschema[fast-validation]',
-        'arcor2==0.6.*',
+        'arcor2==0.7.1',
         'Pillow'
     ],
     zip_safe=False,