From 5fe5fc3d0f3b5c5d744099b5a18f7e00564e5565 Mon Sep 17 00:00:00 2001 From: Nikhil Date: Tue, 8 Oct 2024 16:28:42 +0200 Subject: [PATCH] Moveit generic (#192) --- .github/workflows/test_build.yml | 35 ++ docs/dictionary.txt | 4 +- docs/libraries.rst | 145 ++++++++ examples/example_moveit2/README.md | 31 ++ examples/example_moveit2/example_moveit2.osc | 26 ++ libs/scenario_execution_moveit2/MANIFEST.in | 1 + libs/scenario_execution_moveit2/README.md | 8 + libs/scenario_execution_moveit2/package.xml | 24 ++ .../resource/scenario_execution_moveit2 | 0 .../scenario_execution_moveit2/__init__.py | 21 ++ .../actions/__init__.py | 15 + .../actions/move_to_joint_pose.py | 93 +++++ .../actions/move_to_pose.py | 107 ++++++ .../get_osc_library.py | 19 + .../lib_osc/moveit2.osc | 49 +++ libs/scenario_execution_moveit2/setup.cfg | 4 + libs/scenario_execution_moveit2/setup.py | 48 +++ .../gazebo/arm_sim_scenario/CMakeLists.txt | 21 ++ simulation/gazebo/arm_sim_scenario/README.md | 1 + .../gazebo/arm_sim_scenario/config/arm.rviz | 335 ++++++++++++++++++ .../config/control.urdf.xacro | 95 +++++ .../arm_sim_scenario/config/panda.urdf.xacro | 332 +++++++++++++++++ .../launch/arm_description_launch.py | 89 +++++ .../launch/controller_manager_launch.py | 105 ++++++ .../launch/ignition_launch.py | 93 +++++ .../arm_sim_scenario/launch/moveit_launch.py | 100 ++++++ .../launch/sim_moveit_scenario_launch.py | 106 ++++++ .../gazebo/arm_sim_scenario/package.xml | 32 ++ 28 files changed, 1938 insertions(+), 1 deletion(-) create mode 100644 examples/example_moveit2/README.md create mode 100644 examples/example_moveit2/example_moveit2.osc create mode 100644 libs/scenario_execution_moveit2/MANIFEST.in create mode 100644 libs/scenario_execution_moveit2/README.md create mode 100644 libs/scenario_execution_moveit2/package.xml create mode 100644 libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py create mode 100644 libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc create mode 100644 libs/scenario_execution_moveit2/setup.cfg create mode 100644 libs/scenario_execution_moveit2/setup.py create mode 100644 simulation/gazebo/arm_sim_scenario/CMakeLists.txt create mode 100644 simulation/gazebo/arm_sim_scenario/README.md create mode 100644 simulation/gazebo/arm_sim_scenario/config/arm.rviz create mode 100644 simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/package.xml diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index 7875e579..6f613217 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -358,6 +358,39 @@ jobs: with: name: test-example-external-method-result path: test_example_external_method/test.xml + test-example-moveit2: + needs: [build] + runs-on: intellabs-01 + container: + image: ghcr.io/intellabs/scenario-execution:${{ github.event.pull_request.base.ref }} + credentials: + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + steps: + - name: Restore cache + uses: actions/cache@0c45773b623bea8c8e75f6c82b208c3cf94ea4f9 # v4.0.2 + with: + key: ${{ runner.os }}-build-${{ github.run_number }} + path: . + - name: Test Example Moveit2 + shell: bash + run: | + source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash + source install/setup.bash + Xvfb :1 -screen 0 800x600x16 & + export DISPLAY=:1.0 + export -n CYCLONEDDS_URI + export ROS_DOMAIN_ID=2 + export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} + sed -i 's/60s/600s/g' examples/example_moveit2/example_moveit2.osc + # shellcheck disable=SC1083 + scenario_batch_execution -i examples/example_moveit2/ -o test_example_moveit2 -- ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} + - name: Upload result + uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 + if: always() + with: + name: test-example-moveit2-result + path: test_example_moveit2/test.xml test-scenario-execution-gazebo: needs: [build] runs-on: intellabs-01 @@ -515,6 +548,7 @@ jobs: - test-example-nav2 - test-example-simulation - test-example-multirobot + - test-example-moveit2 - test-example-external-method - test-scenario-execution-gazebo - test-scenario-execution-nav2 @@ -555,6 +589,7 @@ jobs: downloaded-artifacts/test-example-simulation-result/test.xml downloaded-artifacts/test-example-multirobot-result/test.xml downloaded-artifacts/test-example-external-method-result/test.xml + downloaded-artifacts/test-example-moveit2-result/test.xml downloaded-artifacts/test-scenario-execution-gazebo/test.xml downloaded-artifacts/test-scenario-execution-nav2/test.xml downloaded-artifacts/test-scenario-execution-pybullet/test.xml diff --git a/docs/dictionary.txt b/docs/dictionary.txt index f46eb76c..76adebcb 100644 --- a/docs/dictionary.txt +++ b/docs/dictionary.txt @@ -33,4 +33,6 @@ png svg Kubernetes yaml -absolutized \ No newline at end of file +absolutized +moveit +replan \ No newline at end of file diff --git a/docs/libraries.rst b/docs/libraries.rst index 70b7f0d7..0086f531 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -26,6 +26,8 @@ Beside ``osc.standard`` provided by OpenSCENARIO 2 (which we divide into ``osc.s - ROS Library (provided with :repo_link:`scenario_execution_ros`) * - ``osc.x11`` - X11 Library (provided with :repo_link:`libs/scenario_execution_x11`) + * - ``osc.moveit2`` + - ROS Moveit2 manipulation stack Library (provided with :repo_link:`libs/scenario_execution_moveit2`) Additional features can be implemented by defining your own library. @@ -1454,3 +1456,146 @@ Capture the screen content within a video. - ``float`` - ``25.0`` - Frame-rate of the resulting video + +Moveit2 +------- + +The library contains actions to interact with the `Moveit2 `__ manipulation stack. Import it with ``import osc.moveit2``. It is provided by the package :repo_link:`libs/scenario_execution_moveit2`. + +Actors +^^^^^^ + +``arm`` +""""""" +An articulated arm actor inheriting from the more general ``robot`` actor + +.. list-table:: + :widths: 15 15 5 65 + :header-rows: 1 + :class: tight-table + + * - Parameter + - Type + - Default + - Description + * - ``namespace`` + - ``string`` + - `` ' ' `` + - Namespace for the arm + * - ``arm_joints`` + - ``list of string`` + - + - List of joint names for the arm joints + * - ``gripper_joints`` + - ``list of string`` + - + - List of joint names for the gripper joints + * - ``arm_group`` + - ``bool`` + - ``false`` + - Name of the move group controlling the arm joints + * - ``gripper_group`` + - ``string`` + - + - Name of the move group controlling the gripper joints + * - ``end_effector`` + - ``string`` + - + - Name of the end effector component (e.g., hand or tool) + * - ``base_link`` + - ``string`` + - + - Name of the robot's base link for reference in kinematics + +``arm.move_to_joint_pose()`` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Use MoveIt2 to move the arm joints to specified joint positions, utilizing `MoveGroup action `__ from the move_group node by specifying target joint values. + +.. list-table:: + :widths: 15 15 5 65 + :header-rows: 1 + :class: tight-table + + * - Parameter + - Type + - Default + - Description + * - ``goal_pose`` + - ``list of float`` + - + - List joint positions to move to + * - ``move_group`` + - ``move_group_type`` + - + - Move group type. Allowed [arm, gripper] (e.g. ``[move_group_type!arm, move_group_type!gripper]``) + * - ``plan_only`` + - ``bool`` + - ``false`` + - If true, the plan is calculated but not executed. The calculated plan can be visualized in rviz. + * - ``replan`` + - ``bool`` + - ``true`` + - If true, replan if plan becomes invalidated during execution + * - ``tolerance`` + - ``float`` + - ``0.001`` + - The acceptable range of variation around both the start and goal positions. + * - ``max_velocity_scaling_factor`` + - ``float`` + - ``0.1`` + - Scaling factors for optionally reducing the maximum joint velocities + * - ``namespace_override`` + - ``string`` + - ``false`` + - if set, it's used as namespace (instead of the associated actor's name) + * - ``action_topic`` + - ``string`` + - ``move_action`` + - Action name + * - ``success_on_acceptance`` + - ``bool`` + - ``false`` + - Succeed on goal acceptance + +``arm.move_to_pose`` +^^^^^^^^^^^^^^^^^^^^ + +Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup action `__ from the move_group node by specifying the desired end-effector position and orientation. + + * - Parameter + - Type + - Default + - Description + * - ``goal_pose`` + - ``pose_3d`` + - + - end effector pose to move to + * - ``plan_only`` + - ``bool`` + - ``false`` + - If true, the plan is calculated but not executed. The calculated plan can be visualized in rviz. + * - ``replan`` + - ``bool`` + - ``true`` + - If true, replan if plan becomes invalidated during execution + * - ``tolerance`` + - ``float`` + - ``0.001`` + - The acceptable range of variation around both the start and goal positions. + * - ``max_velocity_scaling_factor`` + - ``float`` + - ``0.1`` + - Scaling factors for optionally reducing the maximum joint velocities + * - ``namespace_override`` + - ``string`` + - ``false`` + - if set, it's used as namespace (instead of the associated actor's name) + * - ``action_topic`` + - ``string`` + - ``move_action`` + - Action name + * - ``success_on_acceptance`` + - ``bool`` + - ``false`` + - Succeed on goal acceptance \ No newline at end of file diff --git a/examples/example_moveit2/README.md b/examples/example_moveit2/README.md new file mode 100644 index 00000000..63677fa2 --- /dev/null +++ b/examples/example_moveit2/README.md @@ -0,0 +1,31 @@ +# Example Manipulation + +To run the Example [moveit2](https://moveit.picknik.ai/main/index.html) Scenario. + +```bash +colcon build --packages-up-to arm_sim_scenario +``` + +Source the workspace: + +```bash +source install/setup.bash +``` + +Now, run the following command to launch the scenario: + +a. Full Simulation + +```bash +ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit2/example_moveit2.osc +``` + +b.Visualization Only + +```bash +ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit2/example_moveit2.osc ros2_control_hardware_type:=mock_components use_rviz:=true +``` + +The arm initially moves to a specified joint position. Next, the gripper opens. Once the gripper is open, the arm moves to the designated end-effector position. Finally, the gripper closes. + +For a more detailed understanding of the code structure and scenario implementation please refer to the [tutorial documentation](https://intellabs.github.io/scenario_execution/tutorials.html). diff --git a/examples/example_moveit2/example_moveit2.osc b/examples/example_moveit2/example_moveit2.osc new file mode 100644 index 00000000..9b6daca6 --- /dev/null +++ b/examples/example_moveit2/example_moveit2.osc @@ -0,0 +1,26 @@ +import osc.helpers +import osc.ros +import osc.moveit2 + +scenario example_moveit2: + timeout(60s) + manipulator: arm = arm(arm_joints: ['panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7'], + gripper_joints: ['panda_finger_joint1','panda_finger_joint2'], + arm_group: 'panda_arm', + gripper_group: 'hand', + end_effector: 'panda_hand', + base_link: 'panda_link0') + do serial: + joint_pose: manipulator.move_to_joint_pose( + goal_pose: [-2.82, 1.01, -2.40, -1.46, 0.57, 2.47, 0.0], + move_group: move_group_type!arm) + open_gripper: manipulator.move_to_joint_pose( + goal_pose: [0.04, 0.04], + move_group: move_group_type!gripper) + move_to_pose: manipulator.move_to_pose( + goal_pose: pose_3d(position_3d(x: 0.25, y: 0.0, z: 1.0))) + close_gripper: manipulator.move_to_joint_pose( + goal_pose: [0.04, 0.04], + move_group: move_group_type!gripper) + wait elapsed(1s) + emit end \ No newline at end of file diff --git a/libs/scenario_execution_moveit2/MANIFEST.in b/libs/scenario_execution_moveit2/MANIFEST.in new file mode 100644 index 00000000..d4c598e3 --- /dev/null +++ b/libs/scenario_execution_moveit2/MANIFEST.in @@ -0,0 +1 @@ +include scenario_execution_moveit2/lib_osc/*.osc diff --git a/libs/scenario_execution_moveit2/README.md b/libs/scenario_execution_moveit2/README.md new file mode 100644 index 00000000..c7a0b4f2 --- /dev/null +++ b/libs/scenario_execution_moveit2/README.md @@ -0,0 +1,8 @@ +# Scenario Execution Library for moveIt2 + +The `scenario_execution_moveit2` package provides actions to interact with the [moveit2](https://moveit.picknik.ai/main/index.html) manipulation stack. + +It provides the following scenario execution library: + +- `moveit2.osc`: Actions specific to moveit2 manipulation stack + diff --git a/libs/scenario_execution_moveit2/package.xml b/libs/scenario_execution_moveit2/package.xml new file mode 100644 index 00000000..9fb25b9d --- /dev/null +++ b/libs/scenario_execution_moveit2/package.xml @@ -0,0 +1,24 @@ + + + + scenario_execution_moveit2 + 1.2.0 + Scenario Execution library for moveIt2 + Intel Labs + Intel Labs + Apache-2.0 + + scenario_execution_ros + + rclpy + moveit_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 b/libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 new file mode 100644 index 00000000..e69de29b diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py new file mode 100644 index 00000000..ec44632b --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py @@ -0,0 +1,21 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from . import actions + +__all__ = [ + 'actions' +] diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py new file mode 100644 index 00000000..3ba13780 --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py @@ -0,0 +1,15 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py new file mode 100644 index 00000000..da4e8382 --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py @@ -0,0 +1,93 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from rclpy.duration import Duration +from scenario_execution_ros.actions.ros_action_call import RosActionCall, ActionCallActionState +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import MotionPlanRequest, Constraints, JointConstraint, PlanningOptions + + +class MoveToJointPose(RosActionCall): + """ + Class to move to a joint pose + """ + + def __init__(self, associated_actor, action_topic: str, namespace_override: str, success_on_acceptance: bool) -> None: + self.namespace = associated_actor["namespace"] + if namespace_override: + self.namespace = namespace_override + self.goal_pose = None + self.move_group = None + self.group = None + self.join_names = None + super().__init__(self.namespace + '/' + action_topic, "moveit_msgs.action.MoveGroup", success_on_acceptance=success_on_acceptance) + + def execute(self, associated_actor, goal_pose: list, move_group: str, plan_only: bool, tolerance: float, replan: bool, max_velocity_scaling_factor: float) -> None: # pylint: disable=arguments-differ,arguments-renamed + self.goal_pose = goal_pose + + if not isinstance(move_group, tuple) or not isinstance(move_group[0], str): + raise ValueError("move group type expected to be enum.") + self.move_group = move_group[0] + if self.move_group == "arm": + self.group = associated_actor['arm_group'] + self.join_names = associated_actor['arm_joints'] + elif self.move_group == "gripper": + self.group = associated_actor['gripper_group'] + self.join_names = associated_actor['gripper_joints'] + else: + raise ValueError(f"element_type {move_group[0]} unknown.") + self.plan_only = plan_only + self.tolerance = tolerance + self.replan = replan + self.max_velocity_scaling_factor = max_velocity_scaling_factor + super().execute("") + + def get_goal_msg(self): + motion_plan_request = MotionPlanRequest() + motion_plan_request.group_name = self.group + motion_plan_request.max_velocity_scaling_factor = self.max_velocity_scaling_factor + constraints = Constraints() + for joint_name, position in zip(self.join_names, self.goal_pose): + joint_constraint = JointConstraint() + joint_constraint.joint_name = joint_name + joint_constraint.position = float(position) + joint_constraint.tolerance_above = self.tolerance + joint_constraint.tolerance_below = self.tolerance + joint_constraint.weight = 1.0 # Set weight (importance) of this constraint + constraints.joint_constraints.append(joint_constraint) + + motion_plan_request.goal_constraints.append(constraints) + planning_options = PlanningOptions() + planning_options.plan_only = self.plan_only # Only plan, do not execute (set to False if you want to execute) + planning_options.replan = self.replan # Set to True if you want to allow re-planning in case of failure + goal_msg = MoveGroup.Goal() + goal_msg.request = motion_plan_request # Add the motion planning request + goal_msg.planning_options = planning_options # Add the planning options + return goal_msg + + def get_feedback_message(self, current_state): + feedback_message = super().get_feedback_message(current_state) + + if self.current_state == ActionCallActionState.IDLE: + feedback_message = "Waiting for manipulation" + elif self.current_state == ActionCallActionState.ACTION_CALLED: + if self.received_feedback: + feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}.' + else: + feedback_message = f"Executing manipulation to ({self.goal_pose})." + elif current_state == ActionCallActionState.DONE: + feedback_message = f"Goal reached." + return feedback_message diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py new file mode 100644 index 00000000..4cce7ae0 --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py @@ -0,0 +1,107 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from rclpy.duration import Duration +from scenario_execution_ros.actions.common import get_pose_stamped +from scenario_execution_ros.actions.ros_action_call import RosActionCall, ActionCallActionState +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import MotionPlanRequest, PlanningOptions, Constraints, PositionConstraint, OrientationConstraint, BoundingVolume +from shape_msgs.msg import SolidPrimitive + + +class MoveToPose(RosActionCall): + """ + Class to move to a pose + """ + + def __init__(self, associated_actor, action_topic: str, namespace_override: str, success_on_acceptance: bool) -> None: + self.namespace = associated_actor["namespace"] + if namespace_override: + self.namespace = namespace_override + self.goal_pose = None + super().__init__(self.namespace + '/' + action_topic, "moveit_msgs.action.MoveGroup", success_on_acceptance=success_on_acceptance) + + def execute(self, associated_actor, goal_pose: list, plan_only: bool, tolerance: float, replan: bool, max_velocity_scaling_factor: float) -> None: # pylint: disable=arguments-differ,arguments-renamed + self.goal_pose = goal_pose + self.group = associated_actor['arm_group'] + self.base_link = associated_actor['base_link'] + self.end_effector = associated_actor['end_effector'] + self.plan_only = plan_only + self.tolerance = tolerance + self.replan = replan + self.max_velocity_scaling_factor = max_velocity_scaling_factor + super().execute("") + + def get_goal_msg(self): + motion_plan_request = MotionPlanRequest() + motion_plan_request.group_name = self.group + motion_plan_request.max_velocity_scaling_factor = self.max_velocity_scaling_factor + + target_pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose) + + # Create PositionConstraint + position_constraint = PositionConstraint() + position_constraint.header.frame_id = self.base_link # Reference frame + position_constraint.link_name = self.end_effector # Link for which this constraint applies ( End-Effector Name) + + # Define the region around the target position (e.g., a small bounding box or sphere around the target) + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + bounding_volume = BoundingVolume() + bounding_volume.primitives.append(primitive) + bounding_volume.primitive_poses.append(target_pose.pose) + + # Assign the bounding volume to the position constraint + position_constraint.constraint_region = bounding_volume + position_constraint.weight = 1.0 + + # Create OrientationConstraint + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = self.base_link # Reference frame + orientation_constraint.link_name = self.end_effector # Link for which this constraint applies ( End-Effector Name) + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = self.tolerance # Tolerances for orientation + orientation_constraint.absolute_y_axis_tolerance = self.tolerance + orientation_constraint.absolute_z_axis_tolerance = self.tolerance + orientation_constraint.weight = 1.0 + + # Create the Constraints object and add both position and orientation constraints + goal_constraints = Constraints() + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + motion_plan_request.goal_constraints.append(goal_constraints) + + planning_options = PlanningOptions() + planning_options.plan_only = self.plan_only # Only plan, do not execute (if False, action will execute also) + planning_options.replan = self.replan # if True, re-planning in case of failure + goal_msg = MoveGroup.Goal() + goal_msg.request = motion_plan_request # Add the motion planning request + goal_msg.planning_options = planning_options # Add the planning options + return goal_msg + + def get_feedback_message(self, current_state): + feedback_message = super().get_feedback_message(current_state) + + if self.current_state == ActionCallActionState.IDLE: + feedback_message = "Waiting for manipulation" + elif self.current_state == ActionCallActionState.ACTION_CALLED: + if self.received_feedback: + feedback_message = f'Estimated time of arrival: {Duration.from_msg(self.received_feedback.estimated_time_remaining).nanoseconds / 1e9:0.0f}.' + else: + feedback_message = f"Executing manipulation to ({self.goal_pose})." + elif current_state == ActionCallActionState.DONE: + feedback_message = f"Goal reached." + return feedback_message diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py new file mode 100644 index 00000000..02d4a0b4 --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py @@ -0,0 +1,19 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + + +def get_osc_library(): + return 'scenario_execution_moveit2', 'moveit2.osc' diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc new file mode 100644 index 00000000..457bde61 --- /dev/null +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -0,0 +1,49 @@ +import osc.standard.base +import osc.robotics + +enum move_group_type: [ + arm, # Use the arm group to execute moveit action + gripper # Use the gripper group to execute moveit action +] + +########### +# Actor +########### + +actor arm inherits robot: + # An articulated arm actor inheriting from the more general robot actor + namespace: string = '' # Namespace for the arm + arm_joints: list of string # List of joint names for the arm joints + gripper_joints: list of string # List of joint names for the gripper joints + arm_group: string # Name of the move group controlling the arm joints + gripper_group: string # Name of the move group controlling the gripper joints + end_effector: string # Name of the end effector component (e.g., hand or tool) + base_link: string # Name of the robot's base link for reference in kinematics + + +########### +# Actions +########### + +action arm.move_to_joint_pose: + # Use Moveit2 to move the arm joints to specified joint positions + goal_pose: list of float # list joint positions to move to + move_group: move_group_type # move group type (arm, gripper) + plan_only: bool = false # If true, the plan is calculated but not executed. The calculated plan can be visualized in RViz. + replan: bool = true # if true, replan if plan becomes invalidated during execution + tolerance: float = 0.001 # the acceptable range of variation around both the start and goal positions. + max_velocity_scaling_factor: float = 0.1 # Scaling factors for optionally reducing the maximum joint velocities + namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) + action_topic: string = 'move_action' # Name of action + success_on_acceptance: bool = false # succeed on goal acceptance + +action arm.move_to_pose: + # Use moveit2 to move the end-effector to specified position + goal_pose: pose_3d # end effector pose to move to + plan_only: bool = false # If true, the plan is calculated but not executed. The calculated plan can then be visualized in RViz. + replan: bool = true # if true, replan if plan becomes invalidated during execution + tolerance: float = 0.001 # the acceptable range of variation around both the start and goal positions. + max_velocity_scaling_factor: float = 0.1 # Scaling factors for optionally reducing the maximum joint velocities + namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) + action_topic: string = 'move_action' # Name of action + success_on_acceptance: bool = false # succeed on goal acceptance \ No newline at end of file diff --git a/libs/scenario_execution_moveit2/setup.cfg b/libs/scenario_execution_moveit2/setup.cfg new file mode 100644 index 00000000..e70b8baf --- /dev/null +++ b/libs/scenario_execution_moveit2/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/scenario_execution_moveit2 +[install] +install_scripts=$base/lib/scenario_execution_moveit2 diff --git a/libs/scenario_execution_moveit2/setup.py b/libs/scenario_execution_moveit2/setup.py new file mode 100644 index 00000000..97a70a72 --- /dev/null +++ b/libs/scenario_execution_moveit2/setup.py @@ -0,0 +1,48 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from setuptools import find_namespace_packages, setup + +PACKAGE_NAME = 'scenario_execution_moveit2' + +setup( + name=PACKAGE_NAME, + version='1.2.0', + packages=find_namespace_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Intel Labs', + maintainer_email='scenario-execution@intel.com', + description='Scenario Execution library for moveit2', + license='Apache License 2.0', + tests_require=['pytest'], + include_package_data=True, + entry_points={ + 'scenario_execution.actions': [ + 'arm.move_to_joint_pose = scenario_execution_moveit2.actions.move_to_joint_pose:MoveToJointPose', + 'arm.move_to_pose = scenario_execution_moveit2.actions.move_to_pose:MoveToPose', + ], + 'scenario_execution.osc_libraries': [ + 'moveit2 = ' + 'scenario_execution_moveit2.get_osc_library:get_osc_library', + ] + }, +) diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt new file mode 100644 index 00000000..51876156 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(arm_sim_scenario) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/simulation/gazebo/arm_sim_scenario/README.md b/simulation/gazebo/arm_sim_scenario/README.md new file mode 100644 index 00000000..b3beb83b --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/README.md @@ -0,0 +1 @@ +# MoveIt2 Arm Simulation Scenario Execution \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/config/arm.rviz b/simulation/gazebo/arm_sim_scenario/config/arm.rviz new file mode 100644 index 00000000..112f4c65 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/arm.rviz @@ -0,0 +1,335 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 244 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: scenario_execution_rviz/Scenario View + Name: Scenario View + snapshot_topic: /scenario_execution/snapshots +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + ground_plane_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + random: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + ground_plane_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + random: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.119211196899414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.05966663733124733 + Y: 0.006320085376501083 + Z: 0.08645729720592499 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4253983497619629 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.985392689704895 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 902 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c200000324fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004600000182000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004700fffffffb0000001a005300630065006e006100720069006f0020005600690065007701000001cd0000019d0000007100ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000ab00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003d20000032400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Scenario View: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1433 + X: 167 + Y: 148 diff --git a/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro new file mode 100644 index 00000000..3e954d4e --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro @@ -0,0 +1,95 @@ + + + + + + mock_components/GenericSystem + + + + ign_ros2_control/IgnitionSystem + + + + + + + ${initial_positions['panda_joint1']} + + + + + + + ${initial_positions['panda_joint2']} + + + + + + + ${initial_positions['panda_joint3']} + + + + + + + ${initial_positions['panda_joint4']} + + + + + + + ${initial_positions['panda_joint5']} + + + + + + + ${initial_positions['panda_joint6']} + + + + + + + ${initial_positions['panda_joint7']} + + + + + + + 0.01 + + + + + + panda_finger_joint1 + 1 + + + 0.01 + + + + + + + + + + robot_description + robot_state_publisher + $(find moveit_resources_panda_moveit_config)/config/ros2_controllers.yaml + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro new file mode 100644 index 00000000..16923ac8 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro @@ -0,0 +1,332 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py new file mode 100644 index 00000000..afbb2003 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -0,0 +1,89 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, PathJoinSubstitution +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + +ARGUMENTS = [ + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('virtual_joint_child_name', default_value='panda_link0', + description='arm base_link name'), + DeclareLaunchArgument('virtual_joint_parent_frame', default_value='world', + description='virtual_joint_parent_frame name to which arm is attached to'), + DeclareLaunchArgument('urdf_pkg', default_value='arm_sim_scenario', + description='Package where URDF/Xacro file is located (file should be inside the config dir of pkg/config/robot_name.urdf.xacro)'), + DeclareLaunchArgument('urdf', default_value='panda.urdf.xacro', + description='Name of URDF/Xacro file') +] + + +def generate_launch_description(): + + pkg_urdf = FindPackageShare(LaunchConfiguration('urdf_pkg')) + xacro_file = PathJoinSubstitution([pkg_urdf, + 'config', + LaunchConfiguration('urdf')]) + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + virtual_joint_child_name = LaunchConfiguration('virtual_joint_child_name') + virtual_joint_parent_frame = LaunchConfiguration('virtual_joint_parent_frame') + use_sim_time = LaunchConfiguration('use_sim_time') + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[ + {'use_sim_time': use_sim_time}, + {'robot_description': ParameterValue(Command([ + 'xacro', ' ', xacro_file, ' ', + 'ros2_control_hardware_type:=', ros2_control_hardware_type]), value_type=str)}, + ], + ) + + # Static TF + static_tf_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", virtual_joint_parent_frame, virtual_joint_child_name], + ) + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_sim_time': use_sim_time}], + output={'both': 'log'}, + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(robot_state_publisher) + ld.add_action(joint_state_publisher) + ld.add_action(static_tf_node) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py new file mode 100644 index 00000000..637c92f8 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -0,0 +1,105 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch.conditions import LaunchConfigurationNotEquals +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +ARGUMENTS = [ + DeclareLaunchArgument('arm_group_controller', default_value='panda_arm_controller', + description='arm_group_controller name'), + DeclareLaunchArgument('gripper_group_controller', default_value='panda_hand_controller', + description='gripper_group_controller name'), + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('ros2_control_config_pkg', default_value='moveit_resources_panda_moveit_config', + description='ROS2 control config pkg (file should be inside the config dir of pkg/config/ros2_controllers.yam)') +] + + +def generate_launch_description(): + + pkg_controller_config = FindPackageShare(LaunchConfiguration('ros2_control_config_pkg')) + arm_group_controller = LaunchConfiguration('arm_group_controller') + gripper_group_controller = LaunchConfiguration('gripper_group_controller') + + ros2_controllers_path = PathJoinSubstitution([ + pkg_controller_config, + "config", + "ros2_controllers.yaml"] + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + condition=LaunchConfigurationNotEquals( + "ros2_control_hardware_type", "ignition" + ), + ) + + joint_state_broadcaster = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + "joint_state_broadcaster", + ], + output="screen", + ) + + arm_controller_spawner = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + arm_group_controller, + ], + output="screen", + ) + + gripper_controller_spawner = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + gripper_group_controller + ], + output="screen", + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ros2_control_node) + ld.add_action(joint_state_broadcaster) + ld.add_action(arm_controller_spawner) + ld.add_action(gripper_controller_spawner) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py new file mode 100644 index 00000000..1e2ab03c --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -0,0 +1,93 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, Shutdown, ExecuteProcess +from launch.substitutions import LaunchConfiguration, EnvironmentVariable +from launch_ros.actions import Node + +# Define Launch Arguments +ARGUMENTS = [ + DeclareLaunchArgument( + 'world', + default_value="empty.sdf", + description='Simulation World File' + ) +] + + +def generate_launch_description(): + + world = LaunchConfiguration('world') + + # Set environment variables + ign_gazebo_resource_path = SetEnvironmentVariable( + name='IGN_GAZEBO_RESOURCE_PATH', + value=[ + EnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', default_value=os.path.dirname( + get_package_share_directory('moveit_resources_panda_description'))) + ] + ) + + env = { + 'GZ_SIM_SYSTEM_PLUGIN_PATH': ':'.join([ + os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), + os.environ.get('LD_LIBRARY_PATH', '') + ]), + } + + # Ignition Gazebo Launch + ignition_gazebo = ExecuteProcess( + cmd=['ign', 'gazebo', world, '-r', '-v', '4'], + output='screen', + additional_env=env, + on_exit=Shutdown(), + sigterm_timeout='5', + sigkill_timeout='10', + log_cmd=True, + emulate_tty=True + ) + + # Spawn Robot Node + spawn_robot_node = Node( + package="ros_gz_sim", + executable="create", + output="screen", + arguments=[ + "-topic", "robot_description", + "-name", "arm", + "-allow-renaming", "true", + ], + ) + + # Clock Bridge Node + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output="screen", + ) + + # Create and Return Launch Description + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ign_gazebo_resource_path) + ld.add_action(ignition_gazebo) + ld.add_action(clock_bridge) + ld.add_action(spawn_robot_node) + + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py new file mode 100644 index 00000000..dc438505 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -0,0 +1,100 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from pathlib import Path +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + +ARGUMENTS = [ + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'config', + 'arm.rviz', + ]) + ) +] + + +def generate_launch_description(): + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + use_sim_time = LaunchConfiguration('use_sim_time') + rviz_config = LaunchConfiguration('rviz_config') + use_rviz = LaunchConfiguration('use_rviz') + + moveit_config_builder = MoveItConfigsBuilder("moveit_resources_panda") + + moveit_config_builder._MoveItConfigsBuilder__urdf_package = pkg_arm_sim_scenario # pylint: disable=W0212 + moveit_config_builder._MoveItConfigsBuilder__urdf_file_path = Path("config/panda.urdf.xacro") # pylint: disable=W0212 + + moveit_config = ( + moveit_config_builder + .robot_description( + mappings={ + "ros2_control_hardware_type": ros2_control_hardware_type + }, + ) + .robot_description_semantic(file_path="config/panda.srdf") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] + ) + .planning_scene_monitor(publish_robot_description=True) + .to_moveit_configs() + ) + + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[{'use_sim_time': use_sim_time, }, + moveit_config.to_dict()], + arguments=["--ros-args", "--log-level", "info"], + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ], + condition=IfCondition(use_rviz), + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(move_group_node) + ld.add_action(rviz_node) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py new file mode 100644 index 00000000..a84238e3 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -0,0 +1,106 @@ +# Copyright (C) 2024 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, +# software distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions +# and limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, LaunchConfigurationEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +ARGUMENTS = [ + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('ros2_control_hardware_type', + default_value='ignition', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file', + ), + DeclareLaunchArgument('scenario', + default_value='', + description='Scenario file to execute', + ), + DeclareLaunchArgument('scenario_execution', default_value='true', + choices=['true', 'false'], + description='Wether to execute scenario execution'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'],) +] + + +def generate_launch_description(): + + arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') + scenario_execution_dir = get_package_share_directory('scenario_execution_ros') + use_sim_time = LaunchConfiguration('use_sim_time') + use_rviz = LaunchConfiguration('use_rviz') + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + scenario = LaunchConfiguration('scenario') + scenario_execution = LaunchConfiguration('scenario_execution') + + moveit_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), + launch_arguments={ + 'use_rviz': use_rviz, + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + 'use_sim_time': use_sim_time, + }.items(), + ) + + arm_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'arm_description_launch.py'])]), + launch_arguments={ + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + 'use_sim_time': use_sim_time, + }.items() + ) + + controller_manager = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'controller_manager_launch.py'])]), + launch_arguments={ + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + }.items() + ) + + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='arg_ros2_control_hardware_type', + expected_value='ignition' + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + }.items() + ) + + scenario_exec = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), + condition=IfCondition(scenario_execution), + launch_arguments=[ + ('scenario', scenario), + ] + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(moveit_bringup) + ld.add_action(arm_description) + ld.add_action(controller_manager) + ld.add_action(ignition) + ld.add_action(scenario_exec) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml new file mode 100644 index 00000000..95182f06 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -0,0 +1,32 @@ + + + + arm_sim_scenario + 1.2.0 + MoveIt2 Arm Simulation Scenario Execution + Intel Labs + Intel Labs + Apache-2.0 + + ament_cmake + + moveit + moveit_resources + moveit_planners_chomp + scenario_execution + scenario_execution_moveit2 + controller_manager + gripper_controllers + joint_trajectory_controller + ros2_control + ompl + ros_gz_bridge + ros_gz_sim + + ament_lint_auto + ament_lint_common + + + ament_cmake + +