wandelbots-isaacsim-api 2.2.2__py3-none-any.whl

wandelbots_isaacsim_api/models/ws_pose.py

@@ -0,0 +1,92 @@
+# coding: utf-8
+
+"""
+    Wandelbots Omniservice
+
+    A microservice-based framework for managing Omniverse functionalities
+
+    The version of the OpenAPI document: 2.2.2
+    Generated by OpenAPI Generator (https://openapi-generator.tech)
+
+    Do not edit the class manually.
+"""  # noqa: E501
+
+
+from __future__ import annotations
+import pprint
+import re  # noqa: F401
+import json
+
+from pydantic import BaseModel, ConfigDict, Field, StrictFloat, StrictInt
+from typing import Any, ClassVar, Dict, List, Optional, Union
+from typing_extensions import Annotated
+from typing import Optional, Set
+from typing_extensions import Self
+
+class WSPose(BaseModel):
+    """
+    WSPose
+    """ # noqa: E501
+    pose: Optional[Annotated[List[Union[StrictFloat, StrictInt]], Field(min_length=6, max_length=6)]] = Field(default=None, description="Pose in rotation vector format (WS)")
+    __properties: ClassVar[List[str]] = ["pose"]
+
+    model_config = ConfigDict(
+        populate_by_name=True,
+        validate_assignment=True,
+        protected_namespaces=(),
+    )
+
+
+    def to_str(self) -> str:
+        """Returns the string representation of the model using alias"""
+        return pprint.pformat(self.model_dump(by_alias=True, exclude_none=True))
+
+    def to_json(self) -> str:
+        """
+        Returns the JSON representation of the model using alias
+        
+        Do not use pydantic v2 .model_dump_json(by_alias=True, exclude_unset=True) here!
+        It is unable to resolve nested types generated by openapi-generator.
+        """
+        return json.dumps(self.to_dict())
+
+    @classmethod
+    def from_json(cls, json_str: str) -> Optional[Self]:
+        """Create an instance of WSPose from a JSON string"""
+        return cls.from_dict(json.loads(json_str))
+
+    def to_dict(self) -> Dict[str, Any]:
+        """Return the dictionary representation of the model using alias.
+
+        This has the following differences from calling pydantic's
+        `self.model_dump(by_alias=True)`:
+
+        * `None` is only added to the output dict for nullable fields that
+          were set at model initialization. Other fields with value `None`
+          are ignored.
+        """
+        excluded_fields: Set[str] = set([
+        ])
+
+        _dict = self.model_dump(
+            by_alias=True,
+            exclude=excluded_fields,
+            exclude_none=True,
+        )
+        return _dict
+
+    @classmethod
+    def from_dict(cls, obj: Optional[Dict[str, Any]]) -> Optional[Self]:
+        """Create an instance of WSPose from a dict"""
+        if obj is None:
+            return None
+
+        if not isinstance(obj, dict):
+            return cls.model_validate(obj)
+
+        _obj = cls.model_validate({
+            "pose": obj.get("pose")
+        })
+        return _obj
+
+

wandelbots_isaacsim_api/rest.py

@@ -0,0 +1,212 @@
+# coding: utf-8
+
+"""
+    Wandelbots Omniservice
+
+    A microservice-based framework for managing Omniverse functionalities
+
+    The version of the OpenAPI document: 2.2.2
+    Generated by OpenAPI Generator (https://openapi-generator.tech)
+
+    Do not edit the class manually.
+"""  # noqa: E501
+
+
+import io
+import json
+import re
+import ssl
+from typing import Optional, Union
+
+import aiohttp
+import aiohttp_retry
+
+from wandelbots_isaacsim_api.exceptions import ApiException, ApiValueError
+
+RESTResponseType = aiohttp.ClientResponse
+
+ALLOW_RETRY_METHODS = frozenset({'DELETE', 'GET', 'HEAD', 'OPTIONS', 'PUT', 'TRACE'})
+
+class RESTResponse(io.IOBase):
+
+    def __init__(self, resp) -> None:
+        self.response = resp
+        self.status = resp.status
+        self.reason = resp.reason
+        self.data = None
+
+    async def read(self):
+        if self.data is None:
+            self.data = await self.response.read()
+        return self.data
+
+    def getheaders(self):
+        """Returns a CIMultiDictProxy of the response headers."""
+        return self.response.headers
+
+    def getheader(self, name, default=None):
+        """Returns a given response header."""
+        return self.response.headers.get(name, default)
+
+
+class RESTClientObject:
+
+    def __init__(self, configuration) -> None:
+
+        # maxsize is number of requests to host that are allowed in parallel
+        self.maxsize = configuration.connection_pool_maxsize
+
+        self.ssl_context = ssl.create_default_context(
+            cafile=configuration.ssl_ca_cert
+        )
+        if configuration.cert_file:
+            self.ssl_context.load_cert_chain(
+                configuration.cert_file, keyfile=configuration.key_file
+            )
+
+        if not configuration.verify_ssl:
+            self.ssl_context.check_hostname = False
+            self.ssl_context.verify_mode = ssl.CERT_NONE
+
+        self.proxy = configuration.proxy
+        self.proxy_headers = configuration.proxy_headers
+
+        self.retries = configuration.retries
+
+        self.pool_manager: Optional[aiohttp.ClientSession] = None
+        self.retry_client: Optional[aiohttp_retry.RetryClient] = None
+
+    async def close(self) -> None:
+        if self.pool_manager:
+            await self.pool_manager.close()
+        if self.retry_client is not None:
+            await self.retry_client.close()
+
+    async def request(
+        self,
+        method,
+        url,
+        headers=None,
+        body=None,
+        post_params=None,
+        _request_timeout=None
+    ):
+        """Execute request
+
+        :param method: http request method
+        :param url: http request url
+        :param headers: http request headers
+        :param body: request json body, for `application/json`
+        :param post_params: request post parameters,
+                            `application/x-www-form-urlencoded`
+                            and `multipart/form-data`
+        :param _request_timeout: timeout setting for this request. If one
+                                 number provided, it will be total request
+                                 timeout. It can also be a pair (tuple) of
+                                 (connection, read) timeouts.
+        """
+        method = method.upper()
+        assert method in [
+            'GET',
+            'HEAD',
+            'DELETE',
+            'POST',
+            'PUT',
+            'PATCH',
+            'OPTIONS'
+        ]
+
+        if post_params and body:
+            raise ApiValueError(
+                "body parameter cannot be used with post_params parameter."
+            )
+
+        post_params = post_params or {}
+        headers = headers or {}
+        # url already contains the URL query string
+        timeout = _request_timeout or 5 * 60
+
+        if 'Content-Type' not in headers:
+            headers['Content-Type'] = 'application/json'
+
+        args = {
+            "method": method,
+            "url": url,
+            "timeout": timeout,
+            "headers": headers
+        }
+
+        if self.proxy:
+            args["proxy"] = self.proxy
+        if self.proxy_headers:
+            args["proxy_headers"] = self.proxy_headers
+
+        # For `POST`, `PUT`, `PATCH`, `OPTIONS`, `DELETE`
+        if method in ['POST', 'PUT', 'PATCH', 'OPTIONS', 'DELETE']:
+            if re.search('json', headers['Content-Type'], re.IGNORECASE):
+                if body is not None:
+                    body = json.dumps(body)
+                args["data"] = body
+            elif headers['Content-Type'] == 'application/x-www-form-urlencoded':
+                args["data"] = aiohttp.FormData(post_params)
+            elif headers['Content-Type'] == 'multipart/form-data':
+                # must del headers['Content-Type'], or the correct
+                # Content-Type which generated by aiohttp
+                del headers['Content-Type']
+                data = aiohttp.FormData()
+                for param in post_params:
+                    k, v = param
+                    if isinstance(v, tuple) and len(v) == 3:
+                        data.add_field(
+                            k,
+                            value=v[1],
+                            filename=v[0],
+                            content_type=v[2]
+                        )
+                    else:
+                        # Ensures that dict objects are serialized
+                        if isinstance(v, dict):
+                            v = json.dumps(v)
+                        elif isinstance(v, int):
+                            v = str(v)
+                        data.add_field(k, v)
+                args["data"] = data
+
+            # Pass a `bytes` or `str` parameter directly in the body to support
+            # other content types than Json when `body` argument is provided
+            # in serialized form
+            elif isinstance(body, str) or isinstance(body, bytes):
+                args["data"] = body
+            else:
+                # Cannot generate the request from given parameters
+                msg = """Cannot prepare a request message for provided
+                         arguments. Please check that your arguments match
+                         declared content type."""
+                raise ApiException(status=0, reason=msg)
+
+        pool_manager: Union[aiohttp.ClientSession, aiohttp_retry.RetryClient]
+
+        # https pool manager
+        if self.pool_manager is None:
+            self.pool_manager = aiohttp.ClientSession(
+                connector=aiohttp.TCPConnector(limit=self.maxsize, ssl=self.ssl_context),
+                trust_env=True,
+            )
+        pool_manager = self.pool_manager
+
+        if self.retries is not None and method in ALLOW_RETRY_METHODS:
+            if self.retry_client is None:
+                self.retry_client = aiohttp_retry.RetryClient(
+                    client_session=self.pool_manager,
+                    retry_options=aiohttp_retry.ExponentialRetry(
+                        attempts=self.retries,
+                        factor=2.0,
+                        start_timeout=0.1,
+                        max_timeout=120.0
+                    )
+                )
+            pool_manager = self.retry_client
+
+        r = await pool_manager.request(**args)
+
+        return RESTResponse(r)

wandelbots_isaacsim_api/trajectory/__init__.py

@@ -0,0 +1,17 @@
+from .trajectory_viewer import (
+    TrajectoryViewer,
+    TrajectoryPlanResultConfiguration,
+)
+from .utils import (
+    draw_pose_trajectory,
+    draw_joint_trajectory,
+    TrajectoryDrawOptimization,
+)
+
+__all__ = [
+    "TrajectoryViewer",
+    "TrajectoryPlanResultConfiguration",
+    "draw_pose_trajectory",
+    "draw_joint_trajectory",
+    "TrajectoryDrawOptimization",
+]

wandelbots_isaacsim_api/trajectory/trajectory_viewer.py

@@ -0,0 +1,111 @@
+from typing import Sequence
+from nova import Nova
+from attr import dataclass
+import wandelbots_isaacsim_api as isaac_sim_api
+import wandelbots_api_client as nova_api
+from nova.viewers import Viewer, manager as viewer_manager
+from nova.actions import Action
+import nova.core.motion_group
+import os
+from . import utils as trajectory_utils
+from nova.core.exceptions import PlanTrajectoryFailed
+
+
+@dataclass
+class TrajectoryPlanResultConfiguration:
+    parent_prim_path: str = "/World"
+    name: str = "PlannedTrajectory"
+    options: isaac_sim_api.models.TrajectoryOptions = (
+        isaac_sim_api.models.TrajectoryOptions(color=[255, 255, 255], width=20),
+    )
+    optimization: trajectory_utils.TrajectoryDrawOptimization = (
+        trajectory_utils.TrajectoryDrawOptimization()
+    )
+
+
+class TrajectoryViewer(Viewer):
+    """Viewer for visualizing trajectories in Omniverse."""
+
+    def __init__(
+        self,
+        omniverse_host: str = None,
+        trajectory_success=TrajectoryPlanResultConfiguration(name="PlannedTrajectory"),
+        trajectory_failure=TrajectoryPlanResultConfiguration(
+            name="PlannedTrajectoryFailed",
+            options=isaac_sim_api.models.TrajectoryOptions(color=[255, 0, 0], width=20),
+        ),
+    ) -> None:
+        """Initialize viewer.
+
+        Args:
+            omniverse_host (str, optional): Custom host to omniverse. If None OMNIVERSE_HOST env variable will be used. Defaults to None.
+            trajectory_success (_type_, optional): Settings of success callback.
+            trajectory_failure (_type_, optional): Settings of failure callback.
+        """
+
+        viewer_manager.register_viewer(self)
+        self._omniverse_host = omniverse_host
+
+        self.trajectory_success = trajectory_success
+        self.trajectory_failure = trajectory_failure
+
+    def configure(self, nova: Nova) -> None:
+        """Configure the viewer with the Nova instance."""
+        self.nova = nova
+
+    def cleanup(self) -> None:
+        """Clean up the viewer resources."""
+        pass
+
+    async def log_planning_success(
+        self,
+        actions: Sequence[Action],
+        trajectory: nova_api.models.JointTrajectory,
+        tcp: str,
+        motion_group: nova.core.motion_group.MotionGroup,
+    ) -> None:
+        host = self._omniverse_host or os.getenv("OMNIVERSE_HOST")
+        async with isaac_sim_api.ApiClient(
+            isaac_sim_api.Configuration(host=host)
+        ) as isaac_sim_api_client:
+            await trajectory_utils.draw_joint_trajectory(
+                api_client=isaac_sim_api_client,
+                nova_api_client=self.nova._api_client._api_client,
+                joint_trajectory=trajectory,
+                cell=motion_group._cell,
+                motion_group=motion_group.motion_group_id,
+                tcp=tcp,
+                parent_prim_path=self.trajectory_success.parent_prim_path,
+                name=self.trajectory_success.name,
+                options=self.trajectory_success.options,
+                optimiation=self.trajectory_success.optimization,
+                overwrite_existing=True,
+            )
+
+    async def log_planning_failure(
+        self,
+        actions: Sequence[Action],
+        error: Exception,
+        tcp: str,
+        motion_group: nova.core.motion_group.MotionGroup,
+    ) -> None:
+        if not isinstance(error, PlanTrajectoryFailed):
+            return
+        host = self._omniverse_host or os.getenv("OMNIVERSE_HOST")
+
+        async with isaac_sim_api.ApiClient(
+            isaac_sim_api.Configuration(host=host)
+        ) as omniservice_api_client:
+            await trajectory_utils.draw_joint_trajectory(
+                api_client=omniservice_api_client,
+                nova_api_client=self.nova._api_client._api_client,
+                joint_trajectory=error.error.joint_trajectory,
+                cell=motion_group._cell,
+                motion_group=motion_group.motion_group_id,
+                tcp=tcp,
+                parent_prim_path=self.trajectory_failure.parent_prim_path,
+                name=self.trajectory_failure.name,
+                options=self.trajectory_failure.options,
+                optimiation=self.trajectory_failure.optimization,
+                overwrite_existing=True,
+            )

wandelbots_isaacsim_api/trajectory/utils.py

@@ -0,0 +1,154 @@
+from attr import dataclass
+import wandelbots_isaacsim_api as isaac_sim_api
+import wandelbots_api_client as nova_api
+
+
+@dataclass
+class TrajectoryDrawOptimization:
+    min_time_delta_seconds: float = 0.5
+    min_pose_distance_millimeters: float = 10.0
+
+
+async def draw_joint_trajectory(
+    api_client: isaac_sim_api.ApiClient,
+    nova_api_client: nova_api.ApiClient,
+    joint_trajectory: nova_api.models.JointTrajectory,
+    cell: str,
+    motion_group: str,
+    tcp="Flange",
+    parent_prim_path="/World",
+    name="JointTrajectory",
+    options: isaac_sim_api.models.TrajectoryOptions = isaac_sim_api.models.TrajectoryOptions(
+        color=[255, 255, 255], width=20
+    ),
+    optimiation: TrajectoryDrawOptimization = TrajectoryDrawOptimization(),
+    overwrite_existing=True,
+) -> None:
+    kinematic_api = nova_api.MotionGroupKinematicApi(nova_api_client)
+
+    joint_trajectory = _resacle_trajectory_timeline(
+        joint_trajectory, min_time_delta_seconds=optimiation.min_time_delta_seconds
+    )
+
+    poses = [
+        await kinematic_api.calculate_forward_kinematic(
+            cell=cell,
+            motion_group=motion_group,
+            tcp_pose_request=nova_api.models.TcpPoseRequest(
+                motion_group=motion_group,
+                tcp=tcp,
+                joint_position=joint_state,
+            ),
+        )
+        for joint_state in joint_trajectory.joint_positions
+    ]
+    await draw_pose_trajectory(
+        api_client,
+        poses=poses,
+        parent_prim_path=parent_prim_path,
+        name=name,
+        options=options,
+        overwrite_existing=overwrite_existing,
+        min_pose_distance_millimeters=optimiation.min_pose_distance_millimeters,
+    )
+
+
+async def draw_pose_trajectory(
+    api_client: isaac_sim_api.ApiClient,
+    poses: list[nova_api.models.Pose],
+    name="Trajectory",
+    parent_prim_path="/World",
+    options: isaac_sim_api.models.TrajectoryOptions = isaac_sim_api.models.TrajectoryOptions(
+        color=[255, 255, 255], width=20
+    ),
+    overwrite_existing=True,
+    min_pose_distance_millimeters=10.0,
+) -> None:
+    trajectory_api = isaac_sim_api.TrajectoryApi(api_client)
+    update_trajectory = False
+
+    poses = _reduce_pose_overlap(poses, min_pose_distance_millimeters)
+
+    poses = [
+        [
+            pose.position.x,
+            pose.position.y,
+            pose.position.z,
+            pose.orientation.x,
+            pose.orientation.y,
+            pose.orientation.z,
+        ]
+        for pose in poses
+    ]
+
+    if overwrite_existing:
+        update_trajectory = f"{parent_prim_path}/{name}" in [
+            f"{parent_prim_path}/{trajectory.name}"
+            for trajectory in await trajectory_api.list_trajectories()
+        ]
+
+    if update_trajectory:
+        await trajectory_api.update_trajectory(
+            name=name,
+            patch_trajectory_data=isaac_sim_api.models.PatchTrajectoryData(
+                poses=poses,
+                options=options,
+            ),
+        )
+    else:
+        await trajectory_api.create_trajectory(
+            isaac_sim_api.models.TrajectoryData(
+                name=name,
+                parent_prim_path=parent_prim_path,
+                poses=poses,
+                options=options,
+            )
+        )
+
+
+def _resacle_trajectory_timeline(
+    trajectory: nova_api.models.JointTrajectory, min_time_delta_seconds=0.5
+) -> nova_api.models.JointTrajectory:
+    time_rescaled_trajectory = nova_api.models.JointTrajectory(
+        joint_positions=[], times=[], locations=[]
+    )
+    last_time = 0.0
+    last_index = -1
+    for time_idx, time in enumerate(trajectory.times):
+        # Do not skip starting point
+        if time < last_time + min_time_delta_seconds and time_idx != 0:
+            continue
+        time_rescaled_trajectory.joint_positions.append(
+            trajectory.joint_positions[time_idx]
+        )
+        time_rescaled_trajectory.times.append(time)
+        time_rescaled_trajectory.locations.append(trajectory.locations[time_idx])
+        last_time += min_time_delta_seconds
+
+    # do not forget to add the last point if it was not added
+    # its the most noticeable point if removed
+    if last_index != len(trajectory.times):
+        time_rescaled_trajectory.joint_positions.append(trajectory.joint_positions[-1])
+        time_rescaled_trajectory.times.append(trajectory.times[-1])
+        time_rescaled_trajectory.locations.append(trajectory.locations[-1])
+    return time_rescaled_trajectory
+
+
+def _reduce_pose_overlap(
+    poses: list[nova_api.models.Pose], min_pose_distance_millimeters=10.0
+) -> list[nova_api.models.Pose]:
+    if len(poses) < 2:
+        return poses
+    last_point = poses[0]
+
+    reduced_poses = [last_point]
+    for pose in poses[1:]:
+        distance = (
+            (pose.position.x - last_point.position.x) ** 2
+            + (pose.position.y - last_point.position.y) ** 2
+            + (pose.position.z - last_point.position.z) ** 2
+        ) ** 0.5
+        if distance >= min_pose_distance_millimeters:
+            reduced_poses.append(pose)
+            last_point = pose
+    return reduced_poses