From db4989b4eab1f32920973749b2c267c018419d7d Mon Sep 17 00:00:00 2001 From: ZdenekM Date: Thu, 12 Dec 2024 08:37:23 +0100 Subject: [PATCH] feat(arcor2_ur): handling of collision objects --- build-support/install_ur_dependencies.sh | 6 +- compose-files/ur-demo/docker-compose.yml | 17 +- mypy.ini | 6 + src/docker/arcor2_ur/BUILD | 2 +- src/docker/arcor2_ur_ot/BUILD | 2 +- src/python/arcor2_arserver/tests/testutils.py | 8 +- src/python/arcor2_ur/CHANGELOG.md | 7 + src/python/arcor2_ur/README.md | 4 +- src/python/arcor2_ur/VERSION | 2 +- src/python/arcor2_ur/exceptions.py | 4 + src/python/arcor2_ur/object_types/ur5e.py | 6 +- src/python/arcor2_ur/scripts/ur.py | 450 ++++++++++++++++-- src/python/arcor2_ur/tests/conftest.py | 3 +- .../arcor2_ur/tests/test_interaction.py | 30 +- 14 files changed, 489 insertions(+), 58 deletions(-) diff --git a/build-support/install_ur_dependencies.sh b/build-support/install_ur_dependencies.sh index e53f5ce8..e02c92eb 100755 --- a/build-support/install_ur_dependencies.sh +++ b/build-support/install_ur_dependencies.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash apt-get install -y -q --no-install-recommends \ - ros-jazzy-ros-base=0.11.0-1noble.20240922.110418 \ - ros-jazzy-ur=2.4.12-1noble.20241015.122847 \ - ros-jazzy-moveit-py=2.10.0-1noble.20241008.001636 + ros-jazzy-ros-base=0.11.0-1noble.20241018.114811 \ + ros-jazzy-ur=2.4.13-1noble.20241118.195218 \ + ros-jazzy-moveit-py=2.10.0-1noble.20241108.205840 diff --git a/compose-files/ur-demo/docker-compose.yml b/compose-files/ur-demo/docker-compose.yml index e089b350..03cf89d2 100644 --- a/compose-files/ur-demo/docker-compose.yml +++ b/compose-files/ur-demo/docker-compose.yml @@ -1,6 +1,6 @@ services: ur-demo-robot-api: - image: arcor2/arcor2_ur:1.4.1 + image: arcor2/arcor2_ur:1.5.0 container_name: ur-demo-robot-api ports: - "5012:5012" @@ -26,8 +26,6 @@ services: condition: service_healthy ur-demo-execution: condition: service_started - ur-demo-scene: - condition: service_healthy ur-demo-calibration: condition: service_healthy ports: @@ -38,7 +36,7 @@ services: environment: - ARCOR2_PROJECT_SERVICE_URL=http://ur-demo-project:10000 - ARCOR2_ASSET_SERVICE_URL=http://ur-demo-asset:10040 - - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013 + - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012 - ARCOR2_EXECUTION_URL=ws://ur-demo-execution:6790 - ARCOR2_BUILD_URL=http://ur-demo-build:5008 - ARCOR2_CALIBRATION_URL=http://ur-demo-calibration:5014 @@ -63,7 +61,7 @@ services: networks: - ur-demo-network environment: - - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-scene:5013 + - ARCOR2_SCENE_SERVICE_URL=http://ur-demo-robot-api:5012 - ARCOR2_PROJECT_PATH=/root/project volumes: - ur-demo-execution:/root/project @@ -97,15 +95,6 @@ services: - ARCOR2_CALIBRATION_MOCK=false volumes: - ./calibration.yaml:/root/calibration.yaml - - ur-demo-scene: - image: arcor2/arcor2_scene:1.1.0 - container_name: ur-demo-scene - networks: - - ur-demo-network - ports: - - "5013:5013" - ur-demo-asset: image: registry.gitlab.com/kinalisoft/test-it-off/asset:2.0.2 container_name: "ur-demo-asset" diff --git a/mypy.ini b/mypy.ini index 0b7b0baa..20416dd7 100644 --- a/mypy.ini +++ b/mypy.ini @@ -141,4 +141,10 @@ ignore_missing_imports = True ignore_missing_imports = True [mypy-std_msgs.*] +ignore_missing_imports = True + +[mypy-moveit_msgs.*] +ignore_missing_imports = True + +[mypy-shape_msgs.*] ignore_missing_imports = True \ No newline at end of file diff --git a/src/docker/arcor2_ur/BUILD b/src/docker/arcor2_ur/BUILD index f2fa5c56..9428788d 100644 --- a/src/docker/arcor2_ur/BUILD +++ b/src/docker/arcor2_ur/BUILD @@ -1,2 +1,2 @@ shell_source(name="start.sh", source="start.sh") -docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.4.1"]) +docker_image(name="arcor2_ur", repository="arcor2/arcor2_ur", dependencies=[":start.sh", "build-support:install_ur_dependencies.sh"], image_tags=["1.5.0"]) diff --git a/src/docker/arcor2_ur_ot/BUILD b/src/docker/arcor2_ur_ot/BUILD index b4b8718d..f518ee84 100644 --- a/src/docker/arcor2_ur_ot/BUILD +++ b/src/docker/arcor2_ur_ot/BUILD @@ -1 +1 @@ -docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.4.1"]) +docker_image(name="arcor2_ur_ot", repository="arcor2/arcor2_ur_ot", image_tags=["1.5.0"]) diff --git a/src/python/arcor2_arserver/tests/testutils.py b/src/python/arcor2_arserver/tests/testutils.py index a29ed4b1..ef63bca8 100644 --- a/src/python/arcor2_arserver/tests/testutils.py +++ b/src/python/arcor2_arserver/tests/testutils.py @@ -32,8 +32,12 @@ def log_proc_output(out: tuple[bytes, bytes]) -> None: def finish_processes(processes) -> None: for proc in processes: - proc.terminate() - proc.wait() + if proc.poll() is None: + proc.terminate() + try: + proc.wait(timeout=5) + except sp.TimeoutExpired: + proc.kill() log_proc_output(proc.communicate()) diff --git a/src/python/arcor2_ur/CHANGELOG.md b/src/python/arcor2_ur/CHANGELOG.md index aab79433..3c770c6d 100644 --- a/src/python/arcor2_ur/CHANGELOG.md +++ b/src/python/arcor2_ur/CHANGELOG.md @@ -2,6 +2,13 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +## [1.5.0] - 2024-12-12 + +### Changed + +- Checking for robot's state disabled as it was not working reliable. +- Added Scene functions - ability to manage collision objects. Please note that so far, only boxes are considered. Other types can be added, but will be ignored. + ## [1.4.1] - 2024-11-21 ### Fixed diff --git a/src/python/arcor2_ur/README.md b/src/python/arcor2_ur/README.md index 924ede0f..d6edf085 100644 --- a/src/python/arcor2_ur/README.md +++ b/src/python/arcor2_ur/README.md @@ -2,7 +2,9 @@ The service communicates over ROS 2 and MoveItPy with Universal Robots and provides simple REST API to control the robot and allow integration into ARCOR2. -The service is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)). +It is tested with Ubuntu 24.04, ROS 2 Jazzy and the UR5e robot - however, it should be possible to use it with any robot supported by the [ROS 2 driver](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/main). It expects that the `ur_control.launch.py` is already running (which is handled in the [docker image](https://github.com/robofit/arcor2/blob/master/src/docker/arcor2_ur/start.sh)). + +This service also offers API for managing collision objects. The API is compatible with `arcor2_scene`, meaning that the standard client `arcor2.clients.scene_service` can be used, and it is also fully compatible with ARServer. Typically, in ARCOR2 architecture, this would be done by a separate service. This solution was chosen for simplicity. In the future, there might be something like `arcor2_scene_ros2`, which would enable managing collision objects for multiple robots (which could be just a proxy for forwarding collision object requests to individual ROS-based robots, if they will use separate ROS domains). ## Setup diff --git a/src/python/arcor2_ur/VERSION b/src/python/arcor2_ur/VERSION index 13175fdc..3e1ad720 100644 --- a/src/python/arcor2_ur/VERSION +++ b/src/python/arcor2_ur/VERSION @@ -1 +1 @@ -1.4.1 \ No newline at end of file +1.5.0 \ No newline at end of file diff --git a/src/python/arcor2_ur/exceptions.py b/src/python/arcor2_ur/exceptions.py index d42551d3..0e6d7443 100644 --- a/src/python/arcor2_ur/exceptions.py +++ b/src/python/arcor2_ur/exceptions.py @@ -10,6 +10,10 @@ class UrGeneral(UrException): description = General.description +class UrCollisions(UrException): + description = "Something regarding collision objects went wrong" + + class NotFound(UrException): description = "Occurs when something is not found" diff --git a/src/python/arcor2_ur/object_types/ur5e.py b/src/python/arcor2_ur/object_types/ur5e.py index ff635025..9f25cda3 100644 --- a/src/python/arcor2_ur/object_types/ur5e.py +++ b/src/python/arcor2_ur/object_types/ur5e.py @@ -75,12 +75,13 @@ def get_end_effector_pose(self, end_effector_id: str) -> Pose: def move_to_pose( self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True ) -> None: - self.move(target_pose, speed * 100) + self.move(target_pose, speed * 100, safe) def move( self, pose: Pose, speed: float = 50.0, + safe: bool = True, payload: float = 0.0, *, an: None | str = None, @@ -89,6 +90,7 @@ def move( :param pose: Target pose. :param speed: Relative speed. + :param safe: Avoid collisions. :param payload: Object weight. :return: @@ -102,7 +104,7 @@ def move( rest.Method.PUT, f"{self.settings.url}/eef/pose", body=pose, - params={"velocity": speed, "payload": payload}, + params={"velocity": speed, "payload": payload, "safe": safe}, ) def suck( diff --git a/src/python/arcor2_ur/scripts/ur.py b/src/python/arcor2_ur/scripts/ur.py index 36b3b432..807fc6e4 100644 --- a/src/python/arcor2_ur/scripts/ur.py +++ b/src/python/arcor2_ur/scripts/ur.py @@ -7,7 +7,9 @@ import time from dataclasses import dataclass, field from functools import wraps +from typing import NamedTuple +import humps import rclpy # pants: no-infer-dep from ament_index_python.packages import get_package_share_directory # pants: no-infer-dep from flask import Response, jsonify, request @@ -15,10 +17,12 @@ from geometry_msgs.msg import PoseStamped # pants: no-infer-dep from moveit.planning import MoveItPy, PlanningComponent # pants: no-infer-dep from moveit_configs_utils import MoveItConfigsBuilder # pants: no-infer-dep +from moveit_msgs.msg import CollisionObject # pants: no-infer-dep from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # pants: no-infer-dep from rclpy.node import Node # pants: no-infer-dep from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy # pants: no-infer-dep from sensor_msgs.msg import JointState # pants: no-infer-dep +from shape_msgs.msg import SolidPrimitive # pants: no-infer-dep from std_msgs.msg import Bool, String # pants: no-infer-dep from std_srvs.srv import Trigger # pants: no-infer-dep from tf2_geometry_msgs import do_transform_pose # pants: no-infer-dep @@ -30,13 +34,14 @@ from arcor2 import env from arcor2 import transformations as tr +from arcor2.data import common, object_type from arcor2.data.common import Joint, Pose from arcor2.data.robot import InverseKinematicsRequest from arcor2.flask import RespT, create_app, run_app from arcor2.helpers import port_from_url from arcor2.logging import get_logger from arcor2_ur import get_data, topics, version -from arcor2_ur.exceptions import StartError, UrGeneral, WebApiError +from arcor2_ur.exceptions import NotFound, StartError, UrGeneral, WebApiError from arcor2_ur.object_types.ur5e import Vacuum from arcor2_ur.vgc10 import VGC10 @@ -54,6 +59,24 @@ SERVICE_NAME = f"UR Web API ({UR_TYPE})" +class CollisionObjectTuple(NamedTuple): + model: object_type.Models + pose: common.Pose + + +def pose_to_ros_pose(ps: Pose) -> RosPose: + rp = RosPose() + rp.position.x = ps.position.x + rp.position.y = ps.position.y + rp.position.z = ps.position.z + rp.orientation.x = ps.orientation.x + rp.orientation.y = ps.orientation.y + rp.orientation.z = ps.orientation.z + rp.orientation.w = ps.orientation.w + + return rp + + def plan_and_execute( robot, planning_component, @@ -296,7 +319,8 @@ def __post_init__(self) -> None: class Globs: debug = False state: State | None = None - # lock: threading.Lock = threading.Lock() + collision_objects: dict[str, CollisionObjectTuple] = field(default_factory=dict) + scene_started = False # flag for "Scene service" globs: Globs = Globs() @@ -323,6 +347,34 @@ class Globs: ).to_dict() +def apply_collision_objects(scene) -> None: + assert globs.state + + scene.remove_all_collision_objects() + + for obj_id, obj in globs.collision_objects.items(): + if not isinstance(obj.model, object_type.Box): + continue + + collision_object = CollisionObject() + collision_object.header.frame_id = BASE_LINK + collision_object.id = obj_id + + box_pose = pose_to_ros_pose(tr.make_pose_rel(globs.state.pose, obj.pose)) + + box = SolidPrimitive() + box.type = SolidPrimitive.BOX + box.dimensions = (obj.model.size_x, obj.model.size_y, obj.model.size_z) + + collision_object.primitives.append(box) + collision_object.primitive_poses.append(box_pose) + collision_object.operation = CollisionObject.ADD + + scene.apply_collision_object(collision_object) + + scene.current_state.update() + + def started() -> bool: return globs.state is not None @@ -337,6 +389,12 @@ def wrapped(*args, **kwargs): return wrapped +@app.route("/system/start", methods=["PUT"]) # for compatibility with Scene service +def put_start_scene() -> RespT: + globs.scene_started = True + return Response(status=200) + + @app.route("/state/start", methods=["PUT"]) def put_start() -> RespT: """Start the robot. @@ -362,7 +420,7 @@ def put_start() -> RespT: """ if started(): - raise StartError("Already started.") + raise UrGeneral("Already started.") if not isinstance(request.json, dict): raise UrGeneral("Body should be a JSON dict containing Pose.") @@ -380,10 +438,12 @@ def put_start() -> RespT: try: node.brake_release() - node.wait_for_robot_mode({RobotMode.RUNNING, RobotMode.IDLE}) + # TODO this is somehow broken, not sure why + # node.wait_for_robot_mode({RobotMode.RUNNING, RobotMode.IDLE}) node.load_program() node.play() - node.wait_for_program_running() + # TODO this is somehow broken, not sure why + # node.wait_for_program_running() except Exception: node.destroy_node() @@ -405,6 +465,15 @@ def put_start() -> RespT: tool=vgc10, ) + with globs.state.moveitpy.get_planning_scene_monitor().read_write() as scene: + apply_collision_objects(scene) + + return Response(status=204) + + +@app.route("/system/stop", methods=["PUT"]) # for compatibility with Scene service +def put_stop_scene() -> RespT: + globs.scene_started = False return Response(status=204) @@ -428,6 +497,9 @@ def put_stop() -> RespT: $ref: WebApiError """ + if not started(): + raise UrGeneral("Not started!") + assert globs.state globs.state.node.power_off() @@ -437,6 +509,11 @@ def put_stop() -> RespT: return Response(status=204) +@app.route("/system/running", methods=["GET"]) # for compatibility with Scene service +def get_stated_scene() -> RespT: + return jsonify(globs.scene_started) + + @app.route("/state/started", methods=["GET"]) def get_started() -> RespT: """Get the current state. @@ -463,6 +540,308 @@ def get_started() -> RespT: return jsonify(started()) +@app.route("/collisions/box", methods=["PUT"]) +def put_box() -> RespT: + """Add or update collision box. + --- + put: + tags: + - Collisions + description: Add or update collision box. + parameters: + - name: boxId + in: query + description: unique box collision ID + required: true + schema: + type: string + - name: sizeX + in: query + schema: + type: number + format: float + - name: sizeY + in: query + schema: + type: number + format: float + - name: sizeZ + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + args = request.args.to_dict() + box = object_type.Box(args["boxId"], float(args["sizeX"]), float(args["sizeY"]), float(args["sizeZ"])) + globs.collision_objects[box.id] = CollisionObjectTuple(box, common.Pose.from_dict(humps.decamelize(request.json))) + + if started(): + assert globs.state + with globs.state.moveitpy.get_planning_scene_monitor().read_write() as scene: + apply_collision_objects(scene) + + return Response(status=204) + + +@app.route("/collisions/sphere", methods=["PUT"]) +def put_sphere() -> RespT: + """Add or update collision sphere. + --- + put: + tags: + - Collisions + description: Add or update collision sphere. + parameters: + - name: sphereId + in: query + description: unique sphere collision ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + args = humps.decamelize(request.args.to_dict()) + sphere = object_type.Sphere(args["sphere_id"], float(args["radius"])) + globs.collision_objects[sphere.id] = CollisionObjectTuple( + sphere, common.Pose.from_dict(humps.decamelize(request.json)) + ) + + logger.warning("Sphere collision object added but will be ignored as only boxes are supported at the moment.") + + return Response(status=204) + + +@app.route("/collisions/cylinder", methods=["PUT"]) +def put_cylinder() -> RespT: + """Add or update collision cylinder. + --- + put: + tags: + - Collisions + description: Add or update collision cylinder. + parameters: + - name: cylinderId + in: query + description: unique cylinder collision ID + required: true + schema: + type: string + - name: radius + in: query + schema: + type: number + format: float + - name: height + in: query + schema: + type: number + format: float + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 200: + description: Ok + content: + application/json: + schema: + type: string + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + args = humps.decamelize(request.args.to_dict()) + cylinder = object_type.Cylinder(args["cylinder_id"], float(args["radius"]), float(args["height"])) + globs.collision_objects[cylinder.id] = CollisionObjectTuple( + cylinder, common.Pose.from_dict(humps.decamelize(request.json)) + ) + + logger.warning("Cylinder collision object added but will be ignored as only boxes are supported at the moment.") + + return Response(status=204) + + +@app.route("/collisions/mesh", methods=["PUT"]) +def put_mesh() -> RespT: + """Add or update collision mesh. + --- + put: + tags: + - Collisions + description: Add or update collision mesh. + parameters: + - name: meshId + in: query + description: unique mesh collision ID + required: true + schema: + type: string + - name: meshFileId + in: query + schema: + type: string + - name: meshScaleX + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleY + in: query + schema: + type: number + format: float + default: 1.0 + - name: meshScaleZ + in: query + schema: + type: number + format: float + default: 1.0 + requestBody: + content: + application/json: + schema: + $ref: Pose + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **SceneGeneral**." + content: + application/json: + schema: + $ref: WebApiError + """ + + if not isinstance(request.json, dict): + raise UrGeneral("Body should be a JSON dict containing Pose.") + + args = humps.decamelize(request.args.to_dict()) + mesh = object_type.Mesh(args["mesh_id"], args["mesh_file_id"]) + globs.collision_objects[mesh.id] = CollisionObjectTuple(mesh, common.Pose.from_dict(humps.decamelize(request.json))) + + logger.warning("Mesh collision object added but will be ignored as only boxes are supported at the moment.") + + return Response(status=204) + + +@app.route("/collisions/", methods=["DELETE"]) +def delete_collision(id: str) -> RespT: + """Deletes collision object. + --- + delete: + tags: + - Collisions + summary: Deletes collision object. + parameters: + - name: id + in: path + description: unique ID + required: true + schema: + type: string + responses: + 204: + description: Ok + 500: + description: "Error types: **General**, **NotFound**." + content: + application/json: + schema: + $ref: WebApiError + """ + + try: + del globs.collision_objects[id] + except KeyError: + raise NotFound("Collision not found") + + if started(): + assert globs.state + with globs.state.moveitpy.get_planning_scene_monitor().read_write() as scene: + apply_collision_objects(scene) + + return Response(status=200) + + +@app.route("/collisions", methods=["GET"]) +def get_collisions() -> RespT: + """Gets collision ids. + --- + get: + tags: + - Collisions + summary: Gets collision ids. + responses: + 200: + description: Success + content: + application/json: + schema: + type: array + items: + type: string + 500: + description: "Error types: **General**." + content: + application/json: + schema: + $ref: WebApiError + """ + + return jsonify(list(globs.collision_objects.keys())) + + @app.route("/joints", methods=["GET"]) @requires_started def get_joints() -> RespT: @@ -542,35 +921,34 @@ def put_ik() -> RespT: logger.debug(f"Got IK request: {ikr}") with globs.state.moveitpy.get_planning_scene_monitor().read_write() as scene: - if not ikr.avoid_collisions: + if ikr.avoid_collisions: + apply_collision_objects(scene) + else: scene.remove_all_collision_objects() scene.current_state.update(force=True) - scene.current_state.joint_positions = {j.name: j.value for j in ikr.start_joints} - - pose_goal = RosPose() - pose_goal.position.x = ikr.pose.position.x - pose_goal.position.y = ikr.pose.position.y - pose_goal.position.z = ikr.pose.position.z - pose_goal.orientation.x = ikr.pose.orientation.x - pose_goal.orientation.y = ikr.pose.orientation.y - pose_goal.orientation.z = ikr.pose.orientation.z - pose_goal.orientation.w = ikr.pose.orientation.w - - # Set the robot state and check collisions - if not scene.current_state.set_from_ik(PLANNING_GROUP_NAME, pose_goal, TOOL_LINK, timeout=3): - raise UrGeneral("Can't get IK!") - - scene.current_state.update() # required to update transforms - - if ( - scene.is_state_colliding( - robot_state=scene.current_state, joint_model_group_name=PLANNING_GROUP_NAME, verbose=True - ) - and ikr.avoid_collisions - ): - raise UrGeneral("State is in collision.") # TODO IK exception... + pose_goal = pose_to_ros_pose(ikr.pose) + + try: + # Set the robot state and check collisions + if not scene.current_state.set_from_ik(PLANNING_GROUP_NAME, pose_goal, TOOL_LINK, timeout=3): + raise UrGeneral("Can't get IK!") + + scene.current_state.update() # required to update transforms + + if ( + scene.is_state_colliding( + robot_state=scene.current_state, joint_model_group_name=PLANNING_GROUP_NAME, verbose=True + ) + and ikr.avoid_collisions + ): + raise UrGeneral("State is in collision.") # TODO IK exception... + except UrGeneral: + if not ikr.avoid_collisions: + # restore collision objects + apply_collision_objects(scene) + raise assert len(scene.current_state.joint_positions) == len(globs.state.joints) @@ -652,6 +1030,11 @@ def put_eef_pose() -> RespT: minimum: 0 maximum: 5 default: 0 + - in: query + name: safe + schema: + type: boolean + default: true requestBody: content: application/json: @@ -676,6 +1059,7 @@ def put_eef_pose() -> RespT: pose = Pose.from_dict(request.json) velocity = float(request.args.get("velocity", default=50.0)) / 100.0 payload = float(request.args.get("payload", default=0.0)) + safe = request.args.get("safe", default="true") == "true" pose = tr.make_pose_rel(globs.state.pose, pose) @@ -691,6 +1075,11 @@ def put_eef_pose() -> RespT: pose_goal.pose.position.z = pose.position.z with globs.state.moveitpy.get_planning_scene_monitor().read_write() as scene: + if safe: + apply_collision_objects(scene) + else: + scene.remove_all_collision_objects() + scene.current_state.update(force=True) scene.current_state.joint_positions = {j.name: j.value for j in globs.state.joints} scene.current_state.update() @@ -702,7 +1091,6 @@ def put_eef_pose() -> RespT: globs.state.node.set_payload(payload) plan_and_execute(globs.state.moveitpy, globs.state.ur_manipulator, logger) - return Response(status=204) diff --git a/src/python/arcor2_ur/tests/conftest.py b/src/python/arcor2_ur/tests/conftest.py index 4f810e4c..71cafc34 100644 --- a/src/python/arcor2_ur/tests/conftest.py +++ b/src/python/arcor2_ur/tests/conftest.py @@ -34,7 +34,7 @@ def start_processes(request) -> Iterator[Urls]: pypath = ":".join(sys.path) my_env["PYTHONPATH"] = pypath - kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT} + kwargs = {"env": my_env, "stdout": sp.PIPE, "stderr": sp.STDOUT, "start_new_session": True} processes.append( sp.Popen( # type: ignore @@ -61,6 +61,7 @@ def start_processes(request) -> Iterator[Urls]: my_env["ARCOR2_UR_INTERACT_WITH_DASHBOARD"] = "false" my_env["ARCOR2_UR_TYPE"] = ur_type my_env["PEX_EXTRA_SYS_PATH"] = "/opt/ros/jazzy/lib/python3.12/site-packages" + my_env["ARCOR2_REST_API_DEBUG"] = "true" robot_proc = sp.Popen(["python", "src.python.arcor2_ur.scripts/ur.pex"], **kwargs) # type: ignore diff --git a/src/python/arcor2_ur/tests/test_interaction.py b/src/python/arcor2_ur/tests/test_interaction.py index 5adac381..feae40b7 100644 --- a/src/python/arcor2_ur/tests/test_interaction.py +++ b/src/python/arcor2_ur/tests/test_interaction.py @@ -1,12 +1,21 @@ import pytest -from arcor2.data.common import Pose +from arcor2.clients import scene_service +from arcor2.data.common import Orientation, Pose, Position +from arcor2.data.object_type import Box +from arcor2.exceptions import Arcor2Exception from arcor2_ur.object_types.ur5e import Ur5e, UrSettings from arcor2_ur.tests.conftest import Urls @pytest.mark.timeout(60) def test_basics(start_processes: Urls) -> None: + scene_service.URL = start_processes.robot_url + box = Box("UniqueBoxId", 0.1, 0.1, 0.1) + scene_service.upsert_collision(box, Pose(Position(1, 0, 0), Orientation(0, 0, 0, 1))) + scene_service.start() + assert scene_service.started() + ot = Ur5e("", "", Pose(), UrSettings(start_processes.robot_url)) assert len(ot.robot_joints()) == 6 @@ -17,4 +26,23 @@ def test_basics(start_processes: Urls) -> None: ot.suck() ot.release() + assert scene_service.collision_ids() == {box.id} + + scene_service.upsert_collision(box, pos) + pos.position.z += 0.01 + with pytest.raises(Arcor2Exception): # attempt to move into a collision object + ot.move_to_pose("", pos, 0.5) + + # now without collision checking + ot.move_to_pose("", pos, 0.5, safe=False) + + pos.position.z -= 0.01 + with pytest.raises(Arcor2Exception): # start state in collision + ot.move_to_pose("", pos, 0.5) + + scene_service.delete_all_collisions() + assert not scene_service.collision_ids() + + ot.move_to_pose("", pos, 0.5) + ot.cleanup()