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_moveit2/README.md

@@ -0,0 +1,43 @@
+# Example Moveit2
+
+To run the Example [moveit2](https://moveit.picknik.ai/main/index.html)  Scenario.
+
+## 1. Update Submodules
+
+```bash
+git submodule update --init
+```
+
+## 2. Install Dependencies
+
+```bash
+rosdep install --from-paths . --ignore-src
+```
+
+## 3. Build Packages
+
+```bash
+colcon build --packages-up-to arm_sim_scenario
+```
+
+## 4. Source the Workspace
+
+```bash
+source install/setup.bash
+```
+
+## 5. Launch the Simulation
+
+### 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
+```
+
+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_moveit2/example_moveit2.osc

@@ -0,0 +1,27 @@
+import osc.helpers
+import osc.ros
+import osc.moveit2
+
+scenario example_moveit2:
+    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','panda_finger_joint2'])
+        keep(it.arm_group == 'panda_arm')
+        keep(it.gripper_group == 'hand')
+        keep(it.end_effector == 'panda_hand')
+        keep(it.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'],
+                move_group: move_group_type!gripper)
+        move_to_pose: manipulator.move_to_pose(
+                goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'])
+        close_gripper: manipulator.move_to_joint_pose(
+                goal_pose: ['0.01'],
+                move_group: move_group_type!gripper)
+        wait elapsed(1s)
+        emit end

libs/scenario_execution_moveit2/MANIFEST.in

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

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
+

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 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_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'
+]

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

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_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_moveit2/scenario_execution_moveit2/actions/move_to_pose.py

@@ -0,0 +1,115 @@
+# 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, PlanningOptions, Constraints, PositionConstraint, OrientationConstraint, BoundingVolume
+from geometry_msgs.msg import PoseStamped
+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 = PoseStamped()
+        target_pose.header.frame_id = self.base_link  # reference_frame
+        target_pose.pose.position.x = float(self.goal_pose[0])
+        target_pose.pose.position.y = float(self.goal_pose[1])
+        target_pose.pose.position.z = float(self.goal_pose[2])
+        target_pose.pose.orientation.x = float(self.goal_pose[3])
+        target_pose.pose.orientation.y = float(self.goal_pose[4])
+        target_pose.pose.orientation.z = float(self.goal_pose[5])
+        target_pose.pose.orientation.w = float(self.goal_pose[6])
+
+        # 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

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'