Moveit generic

docs/dictionary.txt

@@ -33,4 +33,6 @@ png
 svg
 Kubernetes
 yaml
-absolutized
+absolutized
+moveit
+replan

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://docs.ros.org/en/noetic/api/moveit_msgs/html/action/MoveGroup.html>`__ 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 string``
+     -
+     - 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://docs.ros.org/en/noetic/api/moveit_msgs/html/action/MoveGroup.html>`__ 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 ``[x, y, z, quatx, quaty, quatz, w]``
+   * - ``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

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).

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

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