Moveit humble

.gitmodules

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

examples/example_moveit_simulation/README.md

@@ -0,0 +1,23 @@
+# Example Simulation MoveIt
+
+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,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>example_moveit_simulation</name>
+  <version>1.1.0</version>
+  <description>Scenario Execution Example for Moveit Simulation</description>
+  <author email="[email protected]">Intel Labs</author>
+  <maintainer email="[email protected]">Intel Labs</maintainer>
+  <license file="../../LICENSE">Apache-2.0</license>
+
+
+  <depend>arm_sim_scenario</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,34 @@
+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')
+            wx200.move_to_joint_pose(joint_pose: '[0.0, 0.122173, 0.296706, 1.0472, 0.0]')
+            wx200.move_gripper(gripper:'open')
+            wx200.move_to_joint_pose(joint_pose: '[0.0, 0.296706, 0.296706, 1.22173, 0.0]')
+            wx200.move_gripper(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(joint_pose: '[0.5235, 0.0, 0.0, 0.0, 0.0]')
+            wx200.move_gripper(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='[email protected]',
+    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>

libs/scenario_execution_moveit/MANIFEST.in

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

libs/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](https://moveit.picknik.ai/main/index.html) library.
+

libs/scenario_execution_moveit/package.xml

@@ -0,0 +1,25 @@
+<?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="[email protected]">Intel Labs</author>
+  <maintainer email="[email protected]">Intel Labs</maintainer>
+  <license file="../LICENSE">Apache-2.0</license>
+
+  <depend>scenario_execution_ros</depend>
+  <exec_depend>joint_to_pose</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>pymoveit2</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>

libs/scenario_execution_moveit/scenario_execution_moveit/__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'
+]

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