Moveit generic

.gitmodules

@@ -1,3 +1,7 @@
 [submodule "dependencies/py_trees_ros"]
 	path = dependencies/py_trees_ros
 	url = https://github.com/splintered-reality/py_trees_ros.git
+[submodule "dependencies/moveit_resources"]
+	path = dependencies/moveit_resources
+	url = https://github.com/moveit/moveit_resources
+	branch = humble

examples/example_moveit/README.md

@@ -0,0 +1,36 @@
+# Example Moveit
+
+To run the Example Moveit 2 Scenario, first build the `arm_sim_scenario` package:
+
+Update submodule
+
+```bash
+git submodule update --init
+```
+
+Install dependencies
+
+```bash
+rosdep install  --from-paths simulation/gazebo/arm_sim_scenario --ignore-src
+```
+
+build packages
+
+```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:
+
+```bash
+ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.osc
+```
+
+
+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/example_moveit.osc

@@ -0,0 +1,22 @@
+import osc.helpers
+import osc.ros
+import osc.moveit
+
+scenario example_moveit:
+    timeout(60s)
+    manipulator: arm with:
+        keep(it.arm_joints == ['panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7'])
+        keep(it.gripper_joints == ['panda_finger_joint1'])
+        keep(it.arm_group == 'panda_arm')
+        keep(it.gripper_group == 'hand')
+    do serial:
+        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
+        )
+        wait elapsed(3s)
+        manipulator.move_to_joint_pose(
+            goal_pose: ['0.035'],
+            move_group: move_group_type!gripper
+        )
+        wait elapsed(5s)

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,8 @@
+# Scenario Execution Library for moveit
+
+The `scenario_execution_moveit` package provides actions to interact with the [moveit](https://moveit.picknik.ai/main/idex.html) manipulation stack.
+
+It provides the following scenario execution library:
+
+- `moveit.osc`: Actions specific to moveit manipulation stack
+

libs/scenario_execution_moveit/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_moveit</name>
+  <version>1.2.0</version>
+  <description>Scenario Execution library 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>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>

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

libs/scenario_execution_moveit/scenario_execution_moveit/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 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_str in zip(self.join_names, self.goal_pose):
+            joint_constraint = JointConstraint()
+            joint_constraint.joint_name = joint_name
+            joint_constraint.position = float(position_str)
+            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

libs/scenario_execution_moveit/scenario_execution_moveit/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_moveit', 'moveit.osc'

libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc

@@ -0,0 +1,26 @@
+import osc.standard.base
+import osc.robotics
+
+enum move_group_type: [
+  arm,
+  gripper
+]
+
+actor arm inherits robot:
+    namespace: string = ''
+    arm_joints: list of string
+    gripper_joints: list of string
+    arm_group: string
+    gripper_group: string
+
+action arm.move_to_joint_pose:
+    # Nav2 to navigate to goal pose.
+    goal_pose: list of string                     # end effector pose to move to
+    move_group: move_group_type
+    plan_only: bool = false
+    replan: bool = false
+    tolerance: float = 0.1
+    max_velocity_scaling_factor: float = 0.1
+    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

libs/scenario_execution_moveit/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/scenario_execution_moveit
+[install]
+install_scripts=$base/lib/scenario_execution_moveit

libs/scenario_execution_moveit/setup.py

@@ -0,0 +1,47 @@
+# 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_moveit'
+
+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='[email protected]',
+    description='Scenario Execution library for moveit',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    include_package_data=True,
+    entry_points={
+        'scenario_execution.actions': [
+            'arm.move_to_joint_pose = scenario_execution_moveit.actions.move_to_joint_pose:MoveToJointPose',
+        ],
+        'scenario_execution.osc_libraries': [
+            'moveit = '
+            'scenario_execution_moveit.get_osc_library:get_osc_library',
+        ]
+    },
+)

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
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

simulation/gazebo/arm_sim_scenario/README.md

@@ -0,0 +1 @@
+# Moveit Arm Simulation Scenario Execution