Moveit generic (#192)

IntelLabs · Oct 8, 2024 · 5fe5fc3 · 5fe5fc3
1 parent be9ec06
commit 5fe5fc3
Show file tree

Hide file tree

Showing 28 changed files with 1,938 additions and 1 deletion.
diff --git 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
@@ -33,4 +33,6 @@ png
 svg
 Kubernetes
 yaml
-absolutized
+absolutized
+moveit
+replan
diff --git 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 <https://moveit.picknik.ai/main/index.html>`__ 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 <https://github.com/moveit/moveit_msgs/blob/master/action/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 <https://github.com/moveit/moveit_msgs/blob/master/action/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
diff --git a/examples/example_moveit2/README.md 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
@@ -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
diff --git a/libs/scenario_execution_moveit2/MANIFEST.in 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
@@ -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
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>scenario_execution_moveit2</name>
+  <version>1.2.0</version>
+  <description>Scenario Execution library for moveIt2</description>
+  <author email="[email protected]">Intel Labs</author>
+  <maintainer email="[email protected]">Intel Labs</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>scenario_execution_ros</depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 b/libs/scenario_execution_moveit2/resource/scenario_execution_moveit2
diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py 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
@@ -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