Moveit humble

.github/workflows/test_build.yml

@@ -86,17 +86,6 @@ jobs:
           rosdep install --rosdistro=humble --from-paths . --ignore-src -r -y
           apt install -y ros-humble-gazebo-ros2-control
           pip3 install -r requirements.txt
-      - name: Build IGN_ROS_CONTROL # build from source for now because base package has errors.
-        shell: bash
-        run: |
-          mkdir -p /ros_ign/src
-          cd /ros_ign/src
-          git clone --branch humble https://github.com/ros-controls/gz_ros2_control.git
-          rosdep install -r --from-paths . --ignore-src --rosdistro humble -y
-          cd /ros_ign
-          source /opt/ros/humble/setup.bash
-          colcon build --continue-on-error
-          source install/setup.bash
       - name: Build
         shell: bash
         run: |
@@ -144,7 +133,6 @@ jobs:
       - name: Test Example Nav2
         shell: bash
         run: |
-          source /ros_ign/install/setup.bash
           source /opt/ros/humble/setup.bash
           source install/setup.bash
           Xvfb :1 -screen 0 800x600x16 &
@@ -158,7 +146,6 @@ jobs:
       - name: Test Example Simulation
         shell: bash
         run: |
-          source /ros_ign/install/setup.bash
           source /opt/ros/humble/setup.bash
           source install/setup.bash
           Xvfb :1 -screen 0 800x600x16 &
@@ -172,7 +159,6 @@ jobs:
       - name: Test Example Multirobot
         shell: bash
         run: |
-          source /ros_ign/install/setup.bash
           source /opt/ros/humble/setup.bash
           source install/setup.bash
           Xvfb :1 -screen 0 800x600x16 &

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "dependencies/py_trees"]
 	path = dependencies/py_trees
 	url = https://github.com/splintered-reality/py_trees.git
+[submodule "dependencies/pymoveit2"]
+	path = dependencies/pymoveit2
+	url = https://github.com/AndrejOrsula/pymoveit2.git

deb_requirements.txt

@@ -2,4 +2,5 @@ ros-humble-turtlebot4-simulator
 ros-humble-py-trees-ros-interfaces
 python3-autopep8
 clang-format
-pylint
+pylint
+ros-humble-moveit

examples/example_moveit_simulation/README.md

@@ -0,0 +1,23 @@
+# Example Simulation Navigation
+
+To run the Example Simulation Moveit with scenario, first build the `example_moveit_simulation` package:
+
+```bash
+colcon build --packages-up-to example_moveit_simulation
+```
+
+Source the workspace:
+
+```bash
+source install/setup.bash
+```
+
+Now, run the following command to launch the scenario:
+
+```bash
+ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc
+```
+
+A turtlebot is initialised with nav2 which drives to a goal and back. During the ride an obstacle is spawned in front of the turtlebot which will then drive around the object.
+
+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).

examples/example_moveit_simulation/example_moveit_simulation/__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

examples/example_moveit_simulation/models/box.sdf

@@ -0,0 +1,37 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+  <model name="my_model">
+    <pose>0 0 0.5 0 0 0</pose>
+    <link name="link">
+      <inertial>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.083</ixx>   
+          <ixy>0.0</ixy>         
+          <ixz>0.0</ixz>        
+          <iyy>0.083</iyy>     
+          <iyz>0.0</iyz>         
+          <izz>0.083</izz>       
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <diffuse>0.1 0.1 0.1 1</diffuse>
+          <specular>0.4 0.4 0.4 1</specular>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

examples/example_moveit_simulation/package.xml

@@ -0,0 +1,26 @@
+<?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>example_moveit_simulation</name>
+  <version>1.1.0</version>
+  <description>Scenario Execution Example for Moveit Simulation</description>
+  <author email="scenario-execution@intel.com">Intel Labs</author>
+  <maintainer email="scenario-execution@intel.com">Intel Labs</maintainer>
+  <license file="../../LICENSE">Apache-2.0</license>
+
+  <depend>scenario_execution</depend>
+  <depend>scenario_execution_gazebo</depend>
+  <depend>arm_sim_scenario</depend>
+  <depend>scenario_execution_moveit</depend>
+
+  <exec_depend>rclpy</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>

examples/example_moveit_simulation/scenarios/example_moveit_simulation.osc

@@ -0,0 +1,41 @@
+import osc.moveit
+import osc.gazebo
+
+scenario example_moveit:
+    wx200: robotic_arm with:
+        keep(it.namespace == 'wx200') 
+        keep(it.joint_names == ["waist","shoulder","elbow","wrist_angle","wrist_rotate","left_finger","right_finger"])
+        keep(it.base_link_name == 'base_link') 
+        keep(it.end_effector_name == 'gripper_link') 
+        keep(it.gripper_joint_names == ["right_finger","left_finger"]) 
+        keep(it.move_group_arm == 'interbotix_arm') 
+        keep(it.move_group_gripper == 'interbotix_gripper')
+        keep(it.open_gripper_position == '[-0.037, +0.037]')
+        keep(it.close_gripper_position == '[-0.015, +0.015]')
+    box: osc_actor
+    do parallel:
+        serial:
+            box.spawn(spawn_pose: pose_3d(
+                            position: position_3d(x: 0.25m, y: 0.0m, z: 0.02m),
+                            orientation: orientation_3d(yaw: 0.0rad)),
+                        model: 'example_moveit_simulation://models/box.sdf',world_name: 'empty')
+        serial:
+            wx200.move_to_joint_pose() with:
+                keep(it.joint_pose == '[0.0, 0.122173, 0.296706, 1.0472, 0.0]')
+            wx200.move_gripper() with:
+                keep(it.gripper == 'open')
+            wx200.move_to_joint_pose() with:
+                keep(it.joint_pose == '[0.0, 0.296706, 0.296706, 1.22173, 0.0]')
+            wx200.move_gripper() with:
+                keep(it.gripper == 'close')
+            wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m)))
+            wx200.move_to_joint_pose() with:
+                keep(it.joint_pose == '[0.5235, 0.0, 0.0, 0.0, 0.0]')
+            wx200.move_gripper() with:
+                keep(it.gripper == 'open')
+            wx200.move_to_pose(pose_3d(position: position_3d(x: 0.1m, y: 0.0m, z: 0.4m)))
+            wait elapsed(3s)
+            emit end
+        time_out: serial:
+            wait elapsed(60s)
+            emit fail

examples/example_moveit_simulation/setup.py

@@ -0,0 +1,43 @@
+# 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 glob import glob
+import os
+from setuptools import setup
+
+PACKAGE_NAME = 'example_moveit_simulation'
+
+setup(
+    name=PACKAGE_NAME,
+    version='1.1.0',
+    packages=[PACKAGE_NAME],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + PACKAGE_NAME]),
+        ('share/' + PACKAGE_NAME, ['package.xml']),
+        (os.path.join('share', PACKAGE_NAME, 'scenarios'), glob('scenarios/*.osc')),
+        (os.path.join('share', PACKAGE_NAME, 'models'), glob('models/*.sdf')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Intel Labs',
+    maintainer_email='scenario-execution@intel.com',
+    description='Scenario Execution Example for Moveit Simulation',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+    },
+)

examples/example_simulation/models/box_moveit.sdf

@@ -0,0 +1,37 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+  <model name="my_model">
+    <pose>0 0 0.5 0 0 0</pose>
+    <link name="link">
+      <inertial>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.083</ixx>   
+          <ixy>0.0</ixy>         
+          <ixz>0.0</ixz>        
+          <iyy>0.083</iyy>     
+          <iyz>0.0</iyz>         
+          <izz>0.083</izz>       
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+          <diffuse>0.1 0.1 0.1 1</diffuse>
+          <specular>0.4 0.4 0.4 1</specular>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

scenario_execution_moveit/MANIFEST.in

@@ -0,0 +1 @@
+include scenario_execution_moveit/lib_osc/*.osc

scenario_execution_moveit/README.md

@@ -0,0 +1,4 @@
+# Scenario Execution MOVEIT
+
+The `scenario_execution_moveit` package provides actions to control robotic manipulator using moveit library.
+

scenario_execution_moveit/package.xml

@@ -0,0 +1,30 @@
+<?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_moveit</name>
+  <version>1.1.0</version>
+  <description>Scenario Execution for MOVEIT</description>
+  <author email="scenario-execution@intel.com">Intel Labs</author>
+  <maintainer email="scenario-execution@intel.com">Intel Labs</maintainer>
+  <license file="../LICENSE">Apache-2.0</license>
+
+  <depend>scenario_execution</depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>py_trees</exec_depend>
+  <exec_depend>py_trees_ros</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>nav2_simple_commander</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
+  <exec_depend>scenario_status</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>

scenario_execution_moveit/scenario_execution_moveit/__init__.py

@@ -0,0 +1,23 @@
+# 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
+
+""" Main entry for scenario execution """
+
+from . import actions
+
+__all__ = [
+    'actions',
+]

scenario_execution_moveit/scenario_execution_moveit/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