From e7abf6e0c1d1df43a14c57a56a61bc831063993d Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 17 Sep 2024 16:34:33 +0200 Subject: [PATCH 01/40] initial --- .gitmodules | 4 + dependencies/moveit_resources | 1 + examples/example_moveit/README.md | 36 +++++++ examples/example_moveit/example_moveit.osc | 22 +++++ libs/scenario_execution_moveit/MANIFEST.in | 1 + libs/scenario_execution_moveit/README.md | 8 ++ libs/scenario_execution_moveit/package.xml | 24 +++++ .../resource/scenario_execution_moveit | 0 .../scenario_execution_moveit/__init__.py | 21 +++++ .../actions/__init__.py | 15 +++ .../actions/move_to_joint_pose.py | 93 +++++++++++++++++++ .../get_osc_library.py | 19 ++++ .../lib_osc/moveit.osc | 26 ++++++ libs/scenario_execution_moveit/setup.cfg | 4 + libs/scenario_execution_moveit/setup.py | 47 ++++++++++ .../gazebo/arm_sim_scenario/CMakeLists.txt | 21 +++++ simulation/gazebo/arm_sim_scenario/README.md | 1 + .../launch/sim_moveit_scenario_launch.py | 61 ++++++++++++ .../gazebo/arm_sim_scenario/package.xml | 30 ++++++ 19 files changed, 434 insertions(+) create mode 160000 dependencies/moveit_resources create mode 100644 examples/example_moveit/README.md create mode 100644 examples/example_moveit/example_moveit.osc create mode 100644 libs/scenario_execution_moveit/MANIFEST.in create mode 100644 libs/scenario_execution_moveit/README.md create mode 100644 libs/scenario_execution_moveit/package.xml create mode 100644 libs/scenario_execution_moveit/resource/scenario_execution_moveit create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc create mode 100644 libs/scenario_execution_moveit/setup.cfg create mode 100644 libs/scenario_execution_moveit/setup.py create mode 100644 simulation/gazebo/arm_sim_scenario/CMakeLists.txt create mode 100644 simulation/gazebo/arm_sim_scenario/README.md create mode 100644 simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/package.xml diff --git a/.gitmodules b/.gitmodules index f5f1d42b..b07dabb8 100644 --- a/.gitmodules +++ b/.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 diff --git a/dependencies/moveit_resources b/dependencies/moveit_resources new file mode 160000 index 00000000..45ea3551 --- /dev/null +++ b/dependencies/moveit_resources @@ -0,0 +1 @@ +Subproject commit 45ea3551aa74fd5dc40cc39ef53391425cda0af9 diff --git a/examples/example_moveit/README.md b/examples/example_moveit/README.md new file mode 100644 index 00000000..134ec749 --- /dev/null +++ b/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). diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc new file mode 100644 index 00000000..e07f8986 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/libs/scenario_execution_moveit/MANIFEST.in b/libs/scenario_execution_moveit/MANIFEST.in new file mode 100644 index 00000000..97c5e0ff --- /dev/null +++ b/libs/scenario_execution_moveit/MANIFEST.in @@ -0,0 +1 @@ +include scenario_execution_moveit/lib_osc/*.osc diff --git a/libs/scenario_execution_moveit/README.md b/libs/scenario_execution_moveit/README.md new file mode 100644 index 00000000..43c45c91 --- /dev/null +++ b/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 + diff --git a/libs/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit/package.xml new file mode 100644 index 00000000..240eba6e --- /dev/null +++ b/libs/scenario_execution_moveit/package.xml @@ -0,0 +1,24 @@ + + + + scenario_execution_moveit + 1.2.0 + Scenario Execution library for moveit + Intel Labs + Intel Labs + Apache-2.0 + + scenario_execution_ros + + rclpy + moveit + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/libs/scenario_execution_moveit/resource/scenario_execution_moveit b/libs/scenario_execution_moveit/resource/scenario_execution_moveit new file mode 100644 index 00000000..e69de29b diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py b/libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py new file mode 100644 index 00000000..ec44632b --- /dev/null +++ b/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' +] diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py new file mode 100644 index 00000000..3ba13780 --- /dev/null +++ b/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 diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py new file mode 100644 index 00000000..1e5a44bb --- /dev/null +++ b/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 diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py b/libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py new file mode 100644 index 00000000..1236259d --- /dev/null +++ b/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' diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc new file mode 100644 index 00000000..0ff92e56 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/libs/scenario_execution_moveit/setup.cfg b/libs/scenario_execution_moveit/setup.cfg new file mode 100644 index 00000000..e910f915 --- /dev/null +++ b/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 diff --git a/libs/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit/setup.py new file mode 100644 index 00000000..66ad6b83 --- /dev/null +++ b/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='scenario-execution@intel.com', + 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', + ] + }, +) diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt new file mode 100644 index 00000000..782bc423 --- /dev/null +++ b/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() diff --git a/simulation/gazebo/arm_sim_scenario/README.md b/simulation/gazebo/arm_sim_scenario/README.md new file mode 100644 index 00000000..eb8ce369 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/README.md @@ -0,0 +1 @@ +# Moveit Arm Simulation Scenario Execution \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py new file mode 100644 index 00000000..cdbc37aa --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -0,0 +1,61 @@ +# 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 + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description(): + + panda_moveit_config_dir = get_package_share_directory('moveit_resources_panda_moveit_config') + scenario_execution_dir = get_package_share_directory('scenario_execution_ros') + + scenario = LaunchConfiguration('scenario') + scenario_execution = LaunchConfiguration('scenario_execution') + arg_scenario = DeclareLaunchArgument('scenario', + description='Scenario file to execute') + arg_scenario_execution = DeclareLaunchArgument( + 'scenario_execution', default_value='True', + description='Wether to execute scenario execution') + + + scenario_exec = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), + condition=IfCondition(scenario_execution), + launch_arguments=[ + ('scenario', scenario), + ] + ) + + panda_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([panda_moveit_config_dir, 'launch', 'demo.launch.py'])]), + ) + + ld = LaunchDescription([ + arg_scenario, + arg_scenario_execution, + ]) + + ld.add_action(panda_launch) + ld.add_action(scenario_exec) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml new file mode 100644 index 00000000..a2af3c19 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -0,0 +1,30 @@ + + + + arm_sim_scenario + 1.2.0 + Moveit Arm Simulation Scenario Execution + Intel Labs + Intel Labs + Apache-2.0 + + ament_cmake + + + moveit + scenario_execution + moveit_resources + controller_manager + ompl + moveit_planners_chomp + gripper_controllers + joint_trajectory_controller + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 2d4b0ce28c4651b11a77e78013046e4e330897aa Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 17 Sep 2024 17:03:09 +0200 Subject: [PATCH 02/40] format --- .../arm_sim_scenario/launch/sim_moveit_scenario_launch.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index cdbc37aa..b1fa551e 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -14,8 +14,6 @@ # # SPDX-License-Identifier: Apache-2.0 -import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -38,7 +36,6 @@ def generate_launch_description(): 'scenario_execution', default_value='True', description='Wether to execute scenario execution') - scenario_exec = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), condition=IfCondition(scenario_execution), From 9f8bd851682ed69b6aa21b470e3b3c65b5d7584f Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 17 Sep 2024 17:14:14 +0200 Subject: [PATCH 03/40] fix package depend --- libs/scenario_execution_moveit/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit/package.xml index 240eba6e..c81e81a6 100644 --- a/libs/scenario_execution_moveit/package.xml +++ b/libs/scenario_execution_moveit/package.xml @@ -11,7 +11,7 @@ scenario_execution_ros rclpy - moveit + moveit_msgs ament_copyright ament_flake8 From 4bcd27947339b29f35820fe608d2cfaf7f630c56 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 18 Sep 2024 15:17:02 +0200 Subject: [PATCH 04/40] move to pose action --- examples/example_moveit/example_moveit.osc | 6 + .../actions/move_to_joint_pose.py | 2 +- .../actions/move_to_pose.py | 118 ++++++++++++++++++ .../lib_osc/moveit.osc | 16 ++- libs/scenario_execution_moveit/setup.py | 1 + 5 files changed, 140 insertions(+), 3 deletions(-) create mode 100644 libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index e07f8986..5569b16c 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -9,6 +9,8 @@ scenario example_moveit: keep(it.gripper_joints == ['panda_finger_joint1']) 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: manipulator.move_to_joint_pose( goal_pose: ['-2.82', '+1.01', '-2.40', '-1.46', '0.57', '2.47', '0.0'], @@ -19,4 +21,8 @@ scenario example_moveit: goal_pose: ['0.035'], move_group: move_group_type!gripper ) + wait elapsed(3s) + manipulator.move_to_pose( + goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] + ) wait elapsed(5s) \ No newline at end of file diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py index 1e5a44bb..2a4f8572 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py @@ -22,7 +22,7 @@ class MoveToJointPose(RosActionCall): """ - Class to move to a pose + Class to move to a joint pose """ def __init__(self, associated_actor, action_topic: str, namespace_override: str, success_on_acceptance: bool) -> None: diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py new file mode 100644 index 00000000..6952ae67 --- /dev/null +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -0,0 +1,118 @@ +# 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, PlanningOptions +from moveit_msgs.msg import 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 + + # Optionally, set tolerances for weight and target position constraint + position_constraint.weight = 1.0 # Importance of this constraint + + # 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 # Importance of this constraint + + # 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 diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index 0ff92e56..1cf8bb7d 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -12,10 +12,11 @@ actor arm inherits robot: gripper_joints: list of string arm_group: string gripper_group: string + end_effector: string + base_link: string action arm.move_to_joint_pose: - # Nav2 to navigate to goal pose. - goal_pose: list of string # end effector pose to move to + goal_pose: list of string # joint pose to move to move_group: move_group_type plan_only: bool = false replan: bool = false @@ -23,4 +24,15 @@ action arm.move_to_joint_pose: 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 + + +action arm.move_to_pose: + goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] + 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 \ No newline at end of file diff --git a/libs/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit/setup.py index 66ad6b83..9dba1af8 100644 --- a/libs/scenario_execution_moveit/setup.py +++ b/libs/scenario_execution_moveit/setup.py @@ -38,6 +38,7 @@ entry_points={ 'scenario_execution.actions': [ 'arm.move_to_joint_pose = scenario_execution_moveit.actions.move_to_joint_pose:MoveToJointPose', + 'arm.move_to_pose = scenario_execution_moveit.actions.move_to_pose:MoveToPose', ], 'scenario_execution.osc_libraries': [ 'moveit = ' From acab519a2d9ef1c173473494e30000c68d3c44cc Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 18 Sep 2024 15:27:12 +0200 Subject: [PATCH 05/40] import fix --- .../scenario_execution_moveit/actions/move_to_pose.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index 6952ae67..faba3416 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -17,8 +17,7 @@ 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, PlanningOptions -from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume +from moveit_msgs.msg import MotionPlanRequest, PlanningOptions, Constraints, PositionConstraint, OrientationConstraint, BoundingVolume from geometry_msgs.msg import PoseStamped from shape_msgs.msg import SolidPrimitive From f5b2daf4bc33729edd36adc9c1650222f66cf4cf Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 18 Sep 2024 19:46:10 +0200 Subject: [PATCH 06/40] format --- examples/example_moveit/README.md | 4 ++-- examples/example_moveit/example_moveit.osc | 5 ++--- libs/scenario_execution_moveit/README.md | 2 +- simulation/gazebo/arm_sim_scenario/package.xml | 1 + 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/example_moveit/README.md b/examples/example_moveit/README.md index 134ec749..79119223 100644 --- a/examples/example_moveit/README.md +++ b/examples/example_moveit/README.md @@ -1,6 +1,6 @@ # Example Moveit -To run the Example Moveit 2 Scenario, first build the `arm_sim_scenario` package: +To run the Example [moveit2](https://moveit.picknik.ai/main/index.html) Scenario. Update submodule @@ -11,7 +11,7 @@ git submodule update --init Install dependencies ```bash -rosdep install --from-paths simulation/gazebo/arm_sim_scenario --ignore-src +rosdep install --from-paths . --ignore-src ``` build packages diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 5569b16c..6c119d46 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -16,13 +16,12 @@ scenario example_moveit: 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(3s) manipulator.move_to_pose( goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] ) - wait elapsed(5s) \ No newline at end of file + wait elapsed(5s) + emit end \ No newline at end of file diff --git a/libs/scenario_execution_moveit/README.md b/libs/scenario_execution_moveit/README.md index 43c45c91..a0d349a9 100644 --- a/libs/scenario_execution_moveit/README.md +++ b/libs/scenario_execution_moveit/README.md @@ -1,6 +1,6 @@ # 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. +The `scenario_execution_moveit` package provides actions to interact with the [moveit](https://moveit.picknik.ai/main/index.html) manipulation stack. It provides the following scenario execution library: diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index a2af3c19..0a8af533 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -13,6 +13,7 @@ moveit scenario_execution + scenario_execution_moveit moveit_resources controller_manager ompl From f124b933c38bb74e0612dcdc86ca41890aefc810 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 19 Sep 2024 14:40:31 +0200 Subject: [PATCH 07/40] launch files --- examples/example_moveit/example_moveit.osc | 27 ++- .../gazebo/arm_sim_scenario/CMakeLists.txt | 2 +- .../gazebo/arm_sim_scenario/config/arm.rviz | 226 ++++++++++++++++++ .../launch/arm_description_launch.py | 104 ++++++++ .../launch/controller_manager_launch.py | 84 +++++++ .../launch/ignition_launch.py | 60 +++++ .../arm_sim_scenario/launch/moveit_launch.py | 61 +++++ .../launch/sim_moveit_scenario_launch.py | 67 ++++-- 8 files changed, 601 insertions(+), 30 deletions(-) create mode 100644 simulation/gazebo/arm_sim_scenario/config/arm.rviz create mode 100644 simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 6c119d46..cd1b535b 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -12,16 +12,19 @@ scenario example_moveit: keep(it.end_effector == 'panda_hand') keep(it.base_link == 'panda_link0') 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 - ) - manipulator.move_to_joint_pose( - goal_pose: ['0.035'], - move_group: move_group_type!gripper - ) - manipulator.move_to_pose( - goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] - ) - wait elapsed(5s) + joint_pose: 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 + ) + open_gripper: serial: + manipulator.move_to_joint_pose( + goal_pose: ['0.035'], + move_group: move_group_type!gripper + ) + move_to_pose: serial: + manipulator.move_to_pose( + goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] + ) + wait elapsed(1s) emit end \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index 782bc423..51876156 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY launch + DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/arm_sim_scenario/config/arm.rviz b/simulation/gazebo/arm_sim_scenario/config/arm.rviz new file mode 100644 index 00000000..6b5476fd --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/config/arm.rviz @@ -0,0 +1,226 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 234 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: scenario_execution_rviz/Scenario Control + Name: Scenario Control + - Class: scenario_execution_rviz/Scenario View + Name: Scenario View + snapshot_topic: /scenario_execution/snapshots +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.4266018867492676 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.16563944518566132 + Y: -0.11561775952577591 + Z: 0.2661708891391754 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.31039851903915405 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.0954009294509888 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 839 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 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 + Scenario Control: + collapsed: false + Scenario View: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1564 + X: 173 + Y: 144 diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py new file mode 100644 index 00000000..3df0b4c1 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -0,0 +1,104 @@ +# Copyright (C) 2024 Intel Corporation +# Copyright 2021 Clearpath Robotics, Inc. +# +# 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. +# +# @author Roni Kreinin (rkreinin@clearpathrobotics.com) + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, PathJoinSubstitution +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue + +ARGUMENTS = [ + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('use_sim_time', default_value='false', + choices=['true', 'false'], + description='use_sim_time'), + DeclareLaunchArgument('virtual_joint_child_name', default_value='panda_link0', + description='arm base_link name'), + DeclareLaunchArgument('virtual_joint_parent_frame', default_value='world', + description='virtual_joint_parent_frame name to which arm is attached to'), + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'config', + 'arm.rviz', + ]), + description='file path to the config file RViz should load.',), +] + + +def generate_launch_description(): + pkg_moveit_resources_panda_moveit_config = get_package_share_directory('moveit_resources_panda_moveit_config') + xacro_file = PathJoinSubstitution([pkg_moveit_resources_panda_moveit_config, + 'config', + 'panda.urdf.xacro']) + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + virtual_joint_child_name = LaunchConfiguration('virtual_joint_child_name') + virtual_joint_parent_frame = LaunchConfiguration('virtual_joint_parent_frame') + rviz_config = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') + use_rviz = LaunchConfiguration('use_rviz') + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[ + {'use_sim_time': LaunchConfiguration('use_sim_time')}, + {'robot_description': ParameterValue(Command([ + 'xacro', ' ', xacro_file, ' ', + 'ros2_control_hardware_type:=', ros2_control_hardware_type]), value_type=str)}, + ], + ) + + # Static TF + static_tf_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", virtual_joint_parent_frame, virtual_joint_child_name], + ) + + rviz_node = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=[ + '-d', rviz_config, + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + output={'both': 'log'}, + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(robot_state_publisher) + ld.add_action(static_tf_node) + ld.add_action(rviz_node) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py new file mode 100644 index 00000000..f3da4d71 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -0,0 +1,84 @@ +# Copyright (C) 2024 Intel Corporation +# Copyright 2021 Clearpath Robotics, Inc. +# +# 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. +# +# @author Roni Kreinin (rkreinin@clearpathrobotics.com) + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions.launch_configuration import LaunchConfiguration +from launch_ros.actions import Node +import os + +ARGUMENTS = [ + DeclareLaunchArgument('arm_group_controller', default_value='panda_arm_controller', + description='arm_group_controller name'), + DeclareLaunchArgument('gripper_group_controller', default_value='panda_hand_controller', + description='gripper_group_controller name'), +] + + +def generate_launch_description(): + + pkg_moveit_resources_panda_moveit_config = get_package_share_directory('moveit_resources_panda_moveit_config') + arm_group_controller = LaunchConfiguration('arm_group_controller') + gripper_group_controller = LaunchConfiguration('gripper_group_controller') + + ros2_controllers_path = os.path.join( + pkg_moveit_resources_panda_moveit_config, + "config", + "ros2_controllers.yaml", + ) + + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[arm_group_controller, "-c", "/controller_manager"], + ) + + gripper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[gripper_group_controller, "-c", "/controller_manager"], + ) + + # Define LaunchDescription variable + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ros2_control_node) + ld.add_action(joint_state_broadcaster_spawner) + ld.add_action(arm_controller_spawner) + ld.add_action(gripper_controller_spawner) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py new file mode 100644 index 00000000..ce53754a --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -0,0 +1,60 @@ +# 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 + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + arguments = [ + + DeclareLaunchArgument('world', default_value=os.path.join(get_package_share_directory( + 'tb4_sim_scenario'), 'worlds', 'maze.sdf'), + description='Simulation World File'), + ] + + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), + os.environ.get('LD_LIBRARY_PATH', default='')])} + + # Ignition gazebo + ignition_gazebo = ExecuteProcess( + cmd=['ign', 'gazebo', LaunchConfiguration('world'), '-r', '-v', '4'], + output='screen', + additional_env=env, + on_exit=Shutdown(), + sigterm_timeout='5', + sigkill_timeout='10', + log_cmd=True, + emulate_tty=True + ) + + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) + + ld = LaunchDescription(arguments) + ld.add_action(ignition_gazebo) + ld.add_action(clock_bridge) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py new file mode 100644 index 00000000..eb92d9e1 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -0,0 +1,61 @@ +# 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from moveit_configs_utils import MoveItConfigsBuilder + +ARGUMENTS = [ + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), +] + + +def generate_launch_description(): + + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") # package name (moveit_resources_panda + _moveit_config) + .robot_description( + file_path="config/panda.urdf.xacro", + mappings={ + "ros2_control_hardware_type": ros2_control_hardware_type + }, + ) + .robot_description_semantic(file_path="config/panda.srdf") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] + ) + .to_moveit_configs() + ) + + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + arguments=["--ros-args", "--log-level", "info"], + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(move_group_node) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index b1fa551e..246e462a 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -18,23 +18,62 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition +from launch.conditions import IfCondition, LaunchConfigurationEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +ARGUMENTS = [ + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('ros2_control_hardware_type', + default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file', + ), + DeclareLaunchArgument('scenario', + default_value='', + description='Scenario file to execute', + ), + DeclareLaunchArgument('scenario_execution', default_value='true', + choices=['true', 'false'], + description='Wether to execute scenario execution'), +] + + def generate_launch_description(): - panda_moveit_config_dir = get_package_share_directory('moveit_resources_panda_moveit_config') + arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') scenario_execution_dir = get_package_share_directory('scenario_execution_ros') + use_rviz = LaunchConfiguration('use_rviz') + ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') scenario = LaunchConfiguration('scenario') scenario_execution = LaunchConfiguration('scenario_execution') - arg_scenario = DeclareLaunchArgument('scenario', - description='Scenario file to execute') - arg_scenario_execution = DeclareLaunchArgument( - 'scenario_execution', default_value='True', - description='Wether to execute scenario execution') + + moveit_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), + launch_arguments={ + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + }.items(), + ) + + arm_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'arm_description_launch.py'])]), + launch_arguments={ + 'use_rviz': use_rviz, + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + }.items() + ) + + controller_manager = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'controller_manager_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='arg_ros2_control_hardware_type', + expected_value='mock_components' + ), + ) scenario_exec = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), @@ -44,15 +83,9 @@ def generate_launch_description(): ] ) - panda_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([PathJoinSubstitution([panda_moveit_config_dir, 'launch', 'demo.launch.py'])]), - ) - - ld = LaunchDescription([ - arg_scenario, - arg_scenario_execution, - ]) - - ld.add_action(panda_launch) + ld = LaunchDescription(ARGUMENTS) + ld.add_action(moveit_bringup) + ld.add_action(arm_description) + ld.add_action(controller_manager) ld.add_action(scenario_exec) return ld From 6ec046747142bb9aa16febb8b0698352d426bc60 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Thu, 19 Sep 2024 15:13:37 +0200 Subject: [PATCH 08/40] cleanup --- .../launch/ignition_launch.py | 60 ------------------- 1 file changed, 60 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py deleted file mode 100644 index ce53754a..00000000 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ /dev/null @@ -1,60 +0,0 @@ -# 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 - -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - - arguments = [ - - DeclareLaunchArgument('world', default_value=os.path.join(get_package_share_directory( - 'tb4_sim_scenario'), 'worlds', 'maze.sdf'), - description='Simulation World File'), - ] - - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': - ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), - os.environ.get('LD_LIBRARY_PATH', default='')])} - - # Ignition gazebo - ignition_gazebo = ExecuteProcess( - cmd=['ign', 'gazebo', LaunchConfiguration('world'), '-r', '-v', '4'], - output='screen', - additional_env=env, - on_exit=Shutdown(), - sigterm_timeout='5', - sigkill_timeout='10', - log_cmd=True, - emulate_tty=True - ) - - clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='clock_bridge', - output='screen', - arguments=[ - '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' - ]) - - ld = LaunchDescription(arguments) - ld.add_action(ignition_gazebo) - ld.add_action(clock_bridge) - return ld From 807921d22014f79e503851bc82413b6ac35cd40e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 23 Sep 2024 16:43:41 +0200 Subject: [PATCH 09/40] fix --- examples/example_moveit/example_moveit.osc | 7 ++++++- .../scenario_execution_moveit/lib_osc/moveit.osc | 4 ++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index cd1b535b..1cb1d2ab 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -6,7 +6,7 @@ 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.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') @@ -26,5 +26,10 @@ scenario example_moveit: manipulator.move_to_pose( goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] ) + close_gripper: serial: + manipulator.move_to_joint_pose( + goal_pose: ['0.0'], + move_group: move_group_type!gripper + ) wait elapsed(1s) emit end \ No newline at end of file diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index 1cf8bb7d..8ced46a6 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -20,7 +20,7 @@ action arm.move_to_joint_pose: move_group: move_group_type plan_only: bool = false replan: bool = false - tolerance: float = 0.1 + tolerance: float = 0.001 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 @@ -31,7 +31,7 @@ action arm.move_to_pose: goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] plan_only: bool = false replan: bool = false - tolerance: float = 0.1 + tolerance: float = 0.001 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 From 920946df95baa022ebe0fd03ff2318e0ffb5d63d Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 24 Sep 2024 17:39:58 +0200 Subject: [PATCH 10/40] add simulation --- examples/example_moveit/README.md | 19 +- examples/example_moveit/example_moveit.osc | 4 +- .../actions/move_to_pose.py | 6 +- .../lib_osc/moveit.osc | 4 +- .../gazebo/arm_sim_scenario/CMakeLists.txt | 2 +- .../launch/arm_description_launch.py | 21 +- .../launch/ignition_launch.py | 133 +++++++ .../arm_sim_scenario/launch/moveit_launch.py | 21 +- .../launch/sim_moveit_scenario_launch.py | 20 +- .../arm_sim_scenario/urdf/arm.urdf.xacro | 337 ++++++++++++++++++ .../arm_sim_scenario/urdf/control.urdf.xacro | 110 ++++++ 11 files changed, 649 insertions(+), 28 deletions(-) create mode 100644 simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro create mode 100644 simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro diff --git a/examples/example_moveit/README.md b/examples/example_moveit/README.md index 79119223..aef37998 100644 --- a/examples/example_moveit/README.md +++ b/examples/example_moveit/README.md @@ -2,35 +2,42 @@ To run the Example [moveit2](https://moveit.picknik.ai/main/index.html) Scenario. -Update submodule +#### 1. Update Submodules ```bash git submodule update --init ``` -Install dependencies +#### 2. Install Dependencies ```bash -rosdep install --from-paths . --ignore-src +rosdep install --from-paths . --ignore-src ``` -build packages +#### 3. Build Packages ```bash colcon build --packages-up-to arm_sim_scenario ``` -Source the workspace: +#### 4. Source the Workspace ```bash source install/setup.bash ``` -Now, run the following command to launch the scenario: +#### 5. Launch the Simulation + +##### a. Full Simulation ```bash ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.osc ``` +##### b.Visualization Only + +```bash +ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.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). diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 1cb1d2ab..83947a6f 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -19,7 +19,7 @@ scenario example_moveit: ) open_gripper: serial: manipulator.move_to_joint_pose( - goal_pose: ['0.035'], + goal_pose: ['0.04'], move_group: move_group_type!gripper ) move_to_pose: serial: @@ -28,7 +28,7 @@ scenario example_moveit: ) close_gripper: serial: manipulator.move_to_joint_pose( - goal_pose: ['0.0'], + goal_pose: ['0.01'], move_group: move_group_type!gripper ) wait elapsed(1s) diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py index faba3416..c074414a 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py @@ -74,9 +74,7 @@ def get_goal_msg(self): # Assign the bounding volume to the position constraint position_constraint.constraint_region = bounding_volume - - # Optionally, set tolerances for weight and target position constraint - position_constraint.weight = 1.0 # Importance of this constraint + position_constraint.weight = 1.0 # Create OrientationConstraint orientation_constraint = OrientationConstraint() @@ -86,7 +84,7 @@ def get_goal_msg(self): 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 # Importance of this constraint + orientation_constraint.weight = 1.0 # Create the Constraints object and add both position and orientation constraints goal_constraints = Constraints() diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc index 8ced46a6..f69b63c6 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc +++ b/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc @@ -19,7 +19,7 @@ action arm.move_to_joint_pose: goal_pose: list of string # joint pose to move to move_group: move_group_type plan_only: bool = false - replan: bool = false + replan: bool = true tolerance: float = 0.001 max_velocity_scaling_factor: float = 0.1 namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) @@ -30,7 +30,7 @@ action arm.move_to_joint_pose: action arm.move_to_pose: goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] plan_only: bool = false - replan: bool = false + replan: bool = true tolerance: float = 0.001 max_velocity_scaling_factor: float = 0.1 namespace_override: string = '' # if set, it's used as namespace (instead of the associated actor's name) diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index 51876156..f9988310 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY launch config + DIRECTORY launch config urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index 3df0b4c1..ec019d52 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -29,7 +29,7 @@ DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', choices=['ignition', 'mock_components'], description='ROS2 control hardware interface type to use for the launch file'), - DeclareLaunchArgument('use_sim_time', default_value='false', + DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='use_sim_time'), DeclareLaunchArgument('virtual_joint_child_name', default_value='panda_link0', @@ -49,10 +49,10 @@ def generate_launch_description(): - pkg_moveit_resources_panda_moveit_config = get_package_share_directory('moveit_resources_panda_moveit_config') - xacro_file = PathJoinSubstitution([pkg_moveit_resources_panda_moveit_config, - 'config', - 'panda.urdf.xacro']) + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') + xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, + 'urdf', + 'arm.urdf.xacro']) ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') virtual_joint_child_name = LaunchConfiguration('virtual_joint_child_name') virtual_joint_parent_frame = LaunchConfiguration('virtual_joint_parent_frame') @@ -66,7 +66,7 @@ def generate_launch_description(): name="robot_state_publisher", output="both", parameters=[ - {'use_sim_time': LaunchConfiguration('use_sim_time')}, + {'use_sim_time': use_sim_time}, {'robot_description': ParameterValue(Command([ 'xacro', ' ', xacro_file, ' ', 'ros2_control_hardware_type:=', ros2_control_hardware_type]), value_type=str)}, @@ -82,6 +82,14 @@ def generate_launch_description(): arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", virtual_joint_parent_frame, virtual_joint_child_name], ) + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_sim_time': use_sim_time}], + output={'both': 'log'}, + ) + rviz_node = Node( condition=IfCondition(use_rviz), package='rviz2', @@ -99,6 +107,7 @@ def generate_launch_description(): # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(robot_state_publisher) + ld.add_action(joint_state_publisher) ld.add_action(static_tf_node) ld.add_action(rviz_node) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py new file mode 100644 index 00000000..bdf5422e --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -0,0 +1,133 @@ +# 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 + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown, RegisterEventHandler +from launch.substitutions import LaunchConfiguration +from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node + +ARGUMENTS = [ + DeclareLaunchArgument('world', default_value=os.path.join(get_package_share_directory( + 'tb4_sim_scenario'), 'worlds', 'maze.sdf'), + description='Simulation World File'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), +] + + +def generate_launch_description(): + + world = LaunchConfiguration('world') + use_sim_time = LaunchConfiguration('use_sim_time') + pkg_moveit_resources_panda_description = get_package_share_directory('moveit_resources_panda_description') + + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), + os.environ.get('LD_LIBRARY_PATH', default='')]), + 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname(pkg_moveit_resources_panda_description)} + + # Ignition gazebo + ignition_gazebo = ExecuteProcess( + cmd=['ign', 'gazebo', world, '-r', '-v', '4'], + output='screen', + additional_env=env, + on_exit=Shutdown(), + sigterm_timeout='5', + sigkill_timeout='10', + log_cmd=True, + emulate_tty=True + ) + + spawn_robot_node = Node( + package='ros_ign_gazebo', + executable='create', + arguments=['-name', 'panda', + '-x', '0.0', + '-y', '0.0', + '-z', '0.0', + '-Y', '0.0', + '-topic', 'robot_description', + "allow-renaming", "true", + "--ros-args", '--log-level', 'warn'], + output={'both': 'log'}, + ) + + clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' + ]) + + spawn_joint_state_broadcaster_node = Node( + name='joint_state_broadcaster_spawner', + package='controller_manager', + executable='spawner', + arguments=[ + '-c', + 'controller_manager', + 'joint_state_broadcaster', + ], + parameters=[{ + 'use_sim_time': use_sim_time, + }], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + gripper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + load_joint_state_broadcaster_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_robot_node, + on_exit=[spawn_joint_state_broadcaster_node] + ) + ) + + load_arm_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[arm_controller_spawner] + ) + ) + + load_gripper_controller_event = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_joint_state_broadcaster_node, + on_exit=[gripper_controller_spawner] + ) + ) + + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ignition_gazebo) + ld.add_action(clock_bridge) + ld.add_action(spawn_robot_node) + ld.add_action(load_joint_state_broadcaster_event) + ld.add_action(load_arm_controller_event) + ld.add_action(load_gripper_controller_event) + return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index eb92d9e1..087cef2f 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -14,28 +14,37 @@ # # SPDX-License-Identifier: Apache-2.0 +from pathlib import Path from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - +from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder ARGUMENTS = [ DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', choices=['ignition', 'mock_components'], description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], + description='use_sim_time'), ] def generate_launch_description(): - + pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') + use_sim_time = LaunchConfiguration('use_sim_time') + + moveit_config_builder = MoveItConfigsBuilder("moveit_resources_panda") + + moveit_config_builder._MoveItConfigsBuilder__urdf_package = pkg_arm_sim_scenario # pylint: disable=W0212 + moveit_config_builder._MoveItConfigsBuilder__urdf_file_path = Path("urdf/arm.urdf.xacro") # pylint: disable=W0212 moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") # package name (moveit_resources_panda + _moveit_config) + moveit_config_builder .robot_description( - file_path="config/panda.urdf.xacro", mappings={ "ros2_control_hardware_type": ros2_control_hardware_type }, @@ -45,6 +54,7 @@ def generate_launch_description(): .planning_pipelines( pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] ) + # .planning_scene_monitor(publish_robot_description=True) .to_moveit_configs() ) @@ -52,7 +62,8 @@ def generate_launch_description(): package="moveit_ros_move_group", executable="move_group", output="screen", - parameters=[moveit_config.to_dict()], + parameters=[{'use_sim_time': use_sim_time, }, + moveit_config.to_dict()], arguments=["--ros-args", "--log-level", "info"], ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 246e462a..77892d23 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -28,7 +28,7 @@ choices=['true', 'false'], description='launches RViz if set to `true`.'), DeclareLaunchArgument('ros2_control_hardware_type', - default_value='mock_components', + default_value='ignition', choices=['ignition', 'mock_components'], description='ROS2 control hardware interface type to use for the launch file', ), @@ -39,6 +39,8 @@ DeclareLaunchArgument('scenario_execution', default_value='true', choices=['true', 'false'], description='Wether to execute scenario execution'), + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'],) ] @@ -46,7 +48,7 @@ def generate_launch_description(): arm_sim_scenario_dir = get_package_share_directory('arm_sim_scenario') scenario_execution_dir = get_package_share_directory('scenario_execution_ros') - + use_sim_time = LaunchConfiguration('use_sim_time') use_rviz = LaunchConfiguration('use_rviz') ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') scenario = LaunchConfiguration('scenario') @@ -56,6 +58,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), launch_arguments={ 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + 'use_sim_time': use_sim_time, }.items(), ) @@ -64,6 +67,7 @@ def generate_launch_description(): launch_arguments={ 'use_rviz': use_rviz, 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + 'use_sim_time': use_sim_time, }.items() ) @@ -75,6 +79,17 @@ def generate_launch_description(): ), ) + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'ignition_launch.py'])]), + condition=LaunchConfigurationEquals( + launch_configuration_name='arg_ros2_control_hardware_type', + expected_value='ignition' + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + }.items() + ) + scenario_exec = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([scenario_execution_dir, 'launch', 'scenario_launch.py'])]), condition=IfCondition(scenario_execution), @@ -87,5 +102,6 @@ def generate_launch_description(): ld.add_action(moveit_bringup) ld.add_action(arm_description) ld.add_action(controller_manager) + ld.add_action(ignition) ld.add_action(scenario_exec) return ld diff --git a/simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro new file mode 100644 index 00000000..7fa1503c --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro @@ -0,0 +1,337 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro new file mode 100644 index 00000000..afa406b8 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro @@ -0,0 +1,110 @@ + + + + + + + + mock_components/GenericSystem + + + + ign_ros2_control/IgnitionSystem + + + + + + ${initial_positions['panda_joint1']} + + + 0.0 + + + + + + ${initial_positions['panda_joint2']} + + + 0.0 + + + + + + ${initial_positions['panda_joint3']} + + + 0.0 + + + + + + ${initial_positions['panda_joint4']} + + + 0.0 + + + + + + ${initial_positions['panda_joint5']} + + + 0.0 + + + + + + ${initial_positions['panda_joint6']} + + + 0.0 + + + + + + ${initial_positions['panda_joint7']} + + + 0.0 + + + + + + 0.01 + + + + + + panda_finger_joint1 + 1 + + + 0.01 + + + + + + + + + + robot_description + robot_state_publisher + $(find moveit_resources_panda_moveit_config)/config/ros2_controllers.yaml + + + + + + + From e3e1493746c47b06e856bd74bf6ba55ee39ac156 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 24 Sep 2024 17:55:24 +0200 Subject: [PATCH 11/40] fix lint --- examples/example_moveit/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/example_moveit/README.md b/examples/example_moveit/README.md index aef37998..a08c8d5a 100644 --- a/examples/example_moveit/README.md +++ b/examples/example_moveit/README.md @@ -2,39 +2,39 @@ To run the Example [moveit2](https://moveit.picknik.ai/main/index.html) Scenario. -#### 1. Update Submodules +## 1. Update Submodules ```bash git submodule update --init ``` -#### 2. Install Dependencies +## 2. Install Dependencies ```bash rosdep install --from-paths . --ignore-src ``` -#### 3. Build Packages +## 3. Build Packages ```bash colcon build --packages-up-to arm_sim_scenario ``` -#### 4. Source the Workspace +## 4. Source the Workspace ```bash source install/setup.bash ``` -#### 5. Launch the Simulation +## 5. Launch the Simulation -##### a. Full Simulation +### a. Full Simulation ```bash ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.osc ``` -##### b.Visualization Only +### b.Visualization Only ```bash ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.osc ros2_control_hardware_type:=mock_components use_rviz:=true From a4fbac49346f403909dde61629cfeef99e78930c Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 25 Sep 2024 12:05:17 +0200 Subject: [PATCH 12/40] cleanup --- .../launch/controller_manager_launch.py | 53 ++++++++---- .../launch/ignition_launch.py | 82 ++++--------------- .../launch/sim_moveit_scenario_launch.py | 7 +- .../gazebo/arm_sim_scenario/package.xml | 4 +- 4 files changed, 59 insertions(+), 87 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py index f3da4d71..fe408f68 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -18,9 +18,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.substitutions.launch_configuration import LaunchConfiguration from launch_ros.actions import Node +from launch.conditions import LaunchConfigurationNotEquals import os ARGUMENTS = [ @@ -28,6 +29,9 @@ description='arm_group_controller name'), DeclareLaunchArgument('gripper_group_controller', default_value='panda_hand_controller', description='gripper_group_controller name'), + DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', + choices=['ignition', 'mock_components'], + description='ROS2 control hardware interface type to use for the launch file'), ] @@ -51,34 +55,51 @@ def generate_launch_description(): ("/controller_manager/robot_description", "/robot_description"), ], output="screen", + condition=LaunchConfigurationNotEquals( + "ros2_control_hardware_type", "ignition" + ), ) - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ + joint_state_broadcaster = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", ], + output="screen", ) - arm_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[arm_group_controller, "-c", "/controller_manager"], + arm_controller_spawner = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + arm_group_controller, + ], + output="screen", ) - gripper_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[gripper_group_controller, "-c", "/controller_manager"], + gripper_controller_spawner = ExecuteProcess( + cmd=[ + "ros2", + "control", + "load_controller", + "--set-state", + "active", + gripper_group_controller + ], + output="screen", ) # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(ros2_control_node) - ld.add_action(joint_state_broadcaster_spawner) + ld.add_action(joint_state_broadcaster) ld.add_action(arm_controller_spawner) ld.add_action(gripper_controller_spawner) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index bdf5422e..b53a41ad 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -17,9 +17,8 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown, RegisterEventHandler +from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown from launch.substitutions import LaunchConfiguration -from launch.event_handlers import OnProcessExit from launch_ros.actions import Node ARGUMENTS = [ @@ -56,78 +55,29 @@ def generate_launch_description(): ) spawn_robot_node = Node( - package='ros_ign_gazebo', - executable='create', - arguments=['-name', 'panda', - '-x', '0.0', - '-y', '0.0', - '-z', '0.0', - '-Y', '0.0', - '-topic', 'robot_description', - "allow-renaming", "true", - "--ros-args", '--log-level', 'warn'], - output={'both': 'log'}, - ) - - clock_bridge = Node(package='ros_gz_bridge', executable='parameter_bridge', - name='clock_bridge', - output='screen', - arguments=[ - '/clock' + '@rosgraph_msgs/msg/Clock' + '[ignition.msgs.Clock' - ]) - - spawn_joint_state_broadcaster_node = Node( - name='joint_state_broadcaster_spawner', - package='controller_manager', - executable='spawner', + package="ros_gz_sim", + executable="create", + output="screen", arguments=[ - '-c', - 'controller_manager', - 'joint_state_broadcaster', + "-topic", + "robot_description", + "-name", + "panda", + "-allow-renaming", + "true", ], - parameters=[{ - 'use_sim_time': use_sim_time, - }], - ) - - arm_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["panda_arm_controller", "-c", "/controller_manager"], - ) - - gripper_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["panda_hand_controller", "-c", "/controller_manager"], - ) - - load_joint_state_broadcaster_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_robot_node, - on_exit=[spawn_joint_state_broadcaster_node] - ) - ) - - load_arm_controller_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_joint_state_broadcaster_node, - on_exit=[arm_controller_spawner] - ) ) - load_gripper_controller_event = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_joint_state_broadcaster_node, - on_exit=[gripper_controller_spawner] - ) + # Clock Bridge + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output="screen", ) ld = LaunchDescription(ARGUMENTS) ld.add_action(ignition_gazebo) ld.add_action(clock_bridge) ld.add_action(spawn_robot_node) - ld.add_action(load_joint_state_broadcaster_event) - ld.add_action(load_arm_controller_event) - ld.add_action(load_gripper_controller_event) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index 77892d23..c599c1bc 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -73,10 +73,9 @@ def generate_launch_description(): controller_manager = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'controller_manager_launch.py'])]), - condition=LaunchConfigurationEquals( - launch_configuration_name='arg_ros2_control_hardware_type', - expected_value='mock_components' - ), + launch_arguments={ + 'arg_ros2_control_hardware_type': ros2_control_hardware_type, + }.items() ) ignition = IncludeLaunchDescription( diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index 0a8af533..cf51d2fa 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -20,7 +20,9 @@ moveit_planners_chomp gripper_controllers joint_trajectory_controller - + ros2_control + ros_gz_bridge + ros_gz_sim ament_lint_auto ament_lint_common From 14272aed3f8587c25661bc94b87ad0cb3a3e80c1 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 25 Sep 2024 12:08:55 +0200 Subject: [PATCH 13/40] cleanuo --- .../gazebo/arm_sim_scenario/launch/controller_manager_launch.py | 2 +- simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py index fe408f68..78da4b9a 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -20,8 +20,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.substitutions.launch_configuration import LaunchConfiguration -from launch_ros.actions import Node from launch.conditions import LaunchConfigurationNotEquals +from launch_ros.actions import Node import os ARGUMENTS = [ diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index b53a41ad..807a3532 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -34,7 +34,6 @@ def generate_launch_description(): world = LaunchConfiguration('world') - use_sim_time = LaunchConfiguration('use_sim_time') pkg_moveit_resources_panda_description = get_package_share_directory('moveit_resources_panda_description') env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': From bb2f0bfccb4a10c9a77f42a9120bdac40cb9063f Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 1 Oct 2024 09:59:40 +0200 Subject: [PATCH 14/40] fix license --- .../launch/arm_description_launch.py | 13 ++++++------- .../launch/controller_manager_launch.py | 13 ++++++------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index ec019d52..e6a8b2d8 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -1,19 +1,18 @@ # Copyright (C) 2024 Intel Corporation -# Copyright 2021 Clearpath Robotics, Inc. # # 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 +# 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, +# 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. +# See the License for the specific language governing permissions +# and limitations under the License. # -# @author Roni Kreinin (rkreinin@clearpathrobotics.com) +# SPDX-License-Identifier: Apache-2.0 from ament_index_python.packages import get_package_share_directory diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py index 78da4b9a..08b1355a 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -1,19 +1,18 @@ # Copyright (C) 2024 Intel Corporation -# Copyright 2021 Clearpath Robotics, Inc. # # 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 +# 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, +# 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. +# See the License for the specific language governing permissions +# and limitations under the License. # -# @author Roni Kreinin (rkreinin@clearpathrobotics.com) +# SPDX-License-Identifier: Apache-2.0 from ament_index_python.packages import get_package_share_directory From 775b682eaa668d1b7ce2388132cc4a7e1723cdad Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 1 Oct 2024 10:05:59 +0200 Subject: [PATCH 15/40] cleanup scenario --- examples/example_moveit/example_moveit.osc | 24 ++++++++-------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit/example_moveit.osc index 83947a6f..13c7eab8 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit/example_moveit.osc @@ -12,24 +12,16 @@ scenario example_moveit: keep(it.end_effector == 'panda_hand') keep(it.base_link == 'panda_link0') do serial: - joint_pose: serial: - manipulator.move_to_joint_pose( + 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: serial: - manipulator.move_to_joint_pose( + 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: serial: - manipulator.move_to_pose( - goal_pose: ['0.25', '0.0', '1.0', '0.0', '0.0', '0.0', '1.0'] - ) - close_gripper: serial: - manipulator.move_to_joint_pose( + 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 - ) + move_group: move_group_type!gripper) wait elapsed(1s) emit end \ No newline at end of file From 23ba1a501a723a0df4b676245583b161fb162fac Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 1 Oct 2024 10:15:45 +0200 Subject: [PATCH 16/40] rename --- .../{example_moveit => example_moveit2}/README.md | 6 +++--- .../example_moveit2.osc} | 4 ++-- libs/scenario_execution_moveit/MANIFEST.in | 1 - libs/scenario_execution_moveit/README.md | 8 -------- libs/scenario_execution_moveit/setup.cfg | 4 ---- libs/scenario_execution_moveit2/MANIFEST.in | 1 + libs/scenario_execution_moveit2/README.md | 8 ++++++++ .../package.xml | 4 ++-- .../resource/scenario_execution_moveit2} | 0 .../scenario_execution_moveit2}/__init__.py | 0 .../scenario_execution_moveit2}/actions/__init__.py | 0 .../actions/move_to_joint_pose.py | 0 .../actions/move_to_pose.py | 0 .../scenario_execution_moveit2}/get_osc_library.py | 2 +- .../scenario_execution_moveit2/lib_osc/moveit2.osc} | 0 libs/scenario_execution_moveit2/setup.cfg | 4 ++++ .../setup.py | 12 ++++++------ simulation/gazebo/arm_sim_scenario/README.md | 2 +- simulation/gazebo/arm_sim_scenario/package.xml | 4 ++-- 19 files changed, 30 insertions(+), 30 deletions(-) rename examples/{example_moveit => example_moveit2}/README.md (82%) rename examples/{example_moveit/example_moveit.osc => example_moveit2/example_moveit2.osc} (96%) delete mode 100644 libs/scenario_execution_moveit/MANIFEST.in delete mode 100644 libs/scenario_execution_moveit/README.md delete mode 100644 libs/scenario_execution_moveit/setup.cfg create mode 100644 libs/scenario_execution_moveit2/MANIFEST.in create mode 100644 libs/scenario_execution_moveit2/README.md rename libs/{scenario_execution_moveit => scenario_execution_moveit2}/package.xml (87%) rename libs/{scenario_execution_moveit/resource/scenario_execution_moveit => scenario_execution_moveit2/resource/scenario_execution_moveit2} (100%) rename libs/{scenario_execution_moveit/scenario_execution_moveit => scenario_execution_moveit2/scenario_execution_moveit2}/__init__.py (100%) rename libs/{scenario_execution_moveit/scenario_execution_moveit => scenario_execution_moveit2/scenario_execution_moveit2}/actions/__init__.py (100%) rename libs/{scenario_execution_moveit/scenario_execution_moveit => scenario_execution_moveit2/scenario_execution_moveit2}/actions/move_to_joint_pose.py (100%) rename libs/{scenario_execution_moveit/scenario_execution_moveit => scenario_execution_moveit2/scenario_execution_moveit2}/actions/move_to_pose.py (100%) rename libs/{scenario_execution_moveit/scenario_execution_moveit => scenario_execution_moveit2/scenario_execution_moveit2}/get_osc_library.py (92%) rename libs/{scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc => scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc} (100%) create mode 100644 libs/scenario_execution_moveit2/setup.cfg rename libs/{scenario_execution_moveit => scenario_execution_moveit2}/setup.py (76%) diff --git a/examples/example_moveit/README.md b/examples/example_moveit2/README.md similarity index 82% rename from examples/example_moveit/README.md rename to examples/example_moveit2/README.md index a08c8d5a..740f9bdf 100644 --- a/examples/example_moveit/README.md +++ b/examples/example_moveit2/README.md @@ -1,4 +1,4 @@ -# Example Moveit +# Example Moveit2 To run the Example [moveit2](https://moveit.picknik.ai/main/index.html) Scenario. @@ -31,13 +31,13 @@ source install/setup.bash ### a. Full Simulation ```bash -ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:=examples/example_moveit/example_moveit.osc +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_moveit/example_moveit.osc ros2_control_hardware_type:=mock_components use_rviz:=true +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). diff --git a/examples/example_moveit/example_moveit.osc b/examples/example_moveit2/example_moveit2.osc similarity index 96% rename from examples/example_moveit/example_moveit.osc rename to examples/example_moveit2/example_moveit2.osc index 13c7eab8..a7488eca 100644 --- a/examples/example_moveit/example_moveit.osc +++ b/examples/example_moveit2/example_moveit2.osc @@ -1,8 +1,8 @@ import osc.helpers import osc.ros -import osc.moveit +import osc.moveit2 -scenario example_moveit: +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']) diff --git a/libs/scenario_execution_moveit/MANIFEST.in b/libs/scenario_execution_moveit/MANIFEST.in deleted file mode 100644 index 97c5e0ff..00000000 --- a/libs/scenario_execution_moveit/MANIFEST.in +++ /dev/null @@ -1 +0,0 @@ -include scenario_execution_moveit/lib_osc/*.osc diff --git a/libs/scenario_execution_moveit/README.md b/libs/scenario_execution_moveit/README.md deleted file mode 100644 index a0d349a9..00000000 --- a/libs/scenario_execution_moveit/README.md +++ /dev/null @@ -1,8 +0,0 @@ -# Scenario Execution Library for moveit - -The `scenario_execution_moveit` package provides actions to interact with the [moveit](https://moveit.picknik.ai/main/index.html) manipulation stack. - -It provides the following scenario execution library: - -- `moveit.osc`: Actions specific to moveit manipulation stack - diff --git a/libs/scenario_execution_moveit/setup.cfg b/libs/scenario_execution_moveit/setup.cfg deleted file mode 100644 index e910f915..00000000 --- a/libs/scenario_execution_moveit/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/scenario_execution_moveit -[install] -install_scripts=$base/lib/scenario_execution_moveit diff --git a/libs/scenario_execution_moveit2/MANIFEST.in b/libs/scenario_execution_moveit2/MANIFEST.in new file mode 100644 index 00000000..d4c598e3 --- /dev/null +++ b/libs/scenario_execution_moveit2/MANIFEST.in @@ -0,0 +1 @@ +include scenario_execution_moveit2/lib_osc/*.osc diff --git a/libs/scenario_execution_moveit2/README.md b/libs/scenario_execution_moveit2/README.md new file mode 100644 index 00000000..072e0c37 --- /dev/null +++ b/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 + diff --git a/libs/scenario_execution_moveit/package.xml b/libs/scenario_execution_moveit2/package.xml similarity index 87% rename from libs/scenario_execution_moveit/package.xml rename to libs/scenario_execution_moveit2/package.xml index c81e81a6..344afcf2 100644 --- a/libs/scenario_execution_moveit/package.xml +++ b/libs/scenario_execution_moveit2/package.xml @@ -1,9 +1,9 @@ - scenario_execution_moveit + scenario_execution_moveit2 1.2.0 - Scenario Execution library for moveit + Scenario Execution library for moveit2 Intel Labs Intel Labs Apache-2.0 diff --git a/libs/scenario_execution_moveit/resource/scenario_execution_moveit b/libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 similarity index 100% rename from libs/scenario_execution_moveit/resource/scenario_execution_moveit rename to libs/scenario_execution_moveit2/resource/scenario_execution_moveit2 diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/__init__.py rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/__init__.py diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/actions/__init__.py rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/__init__.py diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_joint_pose.py rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/actions/move_to_pose.py rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py similarity index 92% rename from libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py index 1236259d..02d4a0b4 100644 --- a/libs/scenario_execution_moveit/scenario_execution_moveit/get_osc_library.py +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/get_osc_library.py @@ -16,4 +16,4 @@ def get_osc_library(): - return 'scenario_execution_moveit', 'moveit.osc' + return 'scenario_execution_moveit2', 'moveit2.osc' diff --git a/libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc similarity index 100% rename from libs/scenario_execution_moveit/scenario_execution_moveit/lib_osc/moveit.osc rename to libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc diff --git a/libs/scenario_execution_moveit2/setup.cfg b/libs/scenario_execution_moveit2/setup.cfg new file mode 100644 index 00000000..e70b8baf --- /dev/null +++ b/libs/scenario_execution_moveit2/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/scenario_execution_moveit2 +[install] +install_scripts=$base/lib/scenario_execution_moveit2 diff --git a/libs/scenario_execution_moveit/setup.py b/libs/scenario_execution_moveit2/setup.py similarity index 76% rename from libs/scenario_execution_moveit/setup.py rename to libs/scenario_execution_moveit2/setup.py index 9dba1af8..97a70a72 100644 --- a/libs/scenario_execution_moveit/setup.py +++ b/libs/scenario_execution_moveit2/setup.py @@ -16,7 +16,7 @@ from setuptools import find_namespace_packages, setup -PACKAGE_NAME = 'scenario_execution_moveit' +PACKAGE_NAME = 'scenario_execution_moveit2' setup( name=PACKAGE_NAME, @@ -31,18 +31,18 @@ zip_safe=True, maintainer='Intel Labs', maintainer_email='scenario-execution@intel.com', - description='Scenario Execution library for moveit', + description='Scenario Execution library for moveit2', 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', - 'arm.move_to_pose = scenario_execution_moveit.actions.move_to_pose:MoveToPose', + 'arm.move_to_joint_pose = scenario_execution_moveit2.actions.move_to_joint_pose:MoveToJointPose', + 'arm.move_to_pose = scenario_execution_moveit2.actions.move_to_pose:MoveToPose', ], 'scenario_execution.osc_libraries': [ - 'moveit = ' - 'scenario_execution_moveit.get_osc_library:get_osc_library', + 'moveit2 = ' + 'scenario_execution_moveit2.get_osc_library:get_osc_library', ] }, ) diff --git a/simulation/gazebo/arm_sim_scenario/README.md b/simulation/gazebo/arm_sim_scenario/README.md index eb8ce369..46f4e359 100644 --- a/simulation/gazebo/arm_sim_scenario/README.md +++ b/simulation/gazebo/arm_sim_scenario/README.md @@ -1 +1 @@ -# Moveit Arm Simulation Scenario Execution \ No newline at end of file +# Moveit2 Arm Simulation Scenario Execution \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index cf51d2fa..8c57b5db 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -3,7 +3,7 @@ arm_sim_scenario 1.2.0 - Moveit Arm Simulation Scenario Execution + Moveit2 Arm Simulation Scenario Execution Intel Labs Intel Labs Apache-2.0 @@ -13,7 +13,7 @@ moveit scenario_execution - scenario_execution_moveit + scenario_execution_moveit2 moveit_resources controller_manager ompl From b063056b0d9f013a66b2b5439d983869e35da20f Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 1 Oct 2024 16:53:24 +0200 Subject: [PATCH 17/40] cleanup --- examples/example_moveit2/example_moveit2.osc | 13 ++++++------- .../gazebo/arm_sim_scenario/launch/moveit_launch.py | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/example_moveit2/example_moveit2.osc b/examples/example_moveit2/example_moveit2.osc index a7488eca..992613ee 100644 --- a/examples/example_moveit2/example_moveit2.osc +++ b/examples/example_moveit2/example_moveit2.osc @@ -4,13 +4,12 @@ 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') + 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'], diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 087cef2f..648df354 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -54,7 +54,7 @@ def generate_launch_description(): .planning_pipelines( pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"] ) - # .planning_scene_monitor(publish_robot_description=True) + .planning_scene_monitor(publish_robot_description=True) .to_moveit_configs() ) From 905f7ce06480bc285d9581b83f3fdfa19d0d5a94 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 1 Oct 2024 18:05:30 +0200 Subject: [PATCH 18/40] fix --- examples/example_moveit2/README.md | 26 +++------- .../lib_osc/moveit2.osc | 52 +++++++++++-------- 2 files changed, 38 insertions(+), 40 deletions(-) diff --git a/examples/example_moveit2/README.md b/examples/example_moveit2/README.md index 740f9bdf..63677fa2 100644 --- a/examples/example_moveit2/README.md +++ b/examples/example_moveit2/README.md @@ -1,43 +1,31 @@ -# Example Moveit2 +# Example Manipulation 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 +Source the workspace: ```bash source install/setup.bash ``` -## 5. Launch the Simulation +Now, run the following command to launch the scenario: -### a. Full 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 +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). diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index f69b63c6..c34747e7 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -2,37 +2,47 @@ import osc.standard.base import osc.robotics enum move_group_type: [ - arm, - gripper + arm, # Use the arm group to execute moveit action + gripper # Use the gripper group to execute moveit action ] +########### +# Actor +########### + actor arm inherits robot: - namespace: string = '' - arm_joints: list of string - gripper_joints: list of string - arm_group: string - gripper_group: string - end_effector: string - base_link: string + 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: string # 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 + + +########### +# Actions +########### action arm.move_to_joint_pose: - goal_pose: list of string # joint pose to move to - move_group: move_group_type - plan_only: bool = false - replan: bool = true - tolerance: float = 0.001 - max_velocity_scaling_factor: float = 0.1 + # Use Moveit2 to move the arm joints to specified joint positions + goal_pose: list of string # list joint positions to move to + move_group: move_group_type # move group type (arm, gripper) + plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution + replan: bool = true # if true, replan if plan becomes invalidated during execution + tolerance: float = 0.001 # tolerance bettwen actual state and start and final state + max_velocity_scaling_factor: float = 0.1 # Scaling factors for optionally reducing the maximum joint velocities 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 - action arm.move_to_pose: - goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] - plan_only: bool = false - replan: bool = true - tolerance: float = 0.001 - max_velocity_scaling_factor: float = 0.1 + # Use moveit2 to move the end-effector to specified position + goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] + plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution + replan: bool = true # if true, replan if plan becomes invalidated during execution + tolerance: float = 0.001 # tolerance bettwen actual state and start and final state + max_velocity_scaling_factor: float = 0.1 # Scaling factors for optionally reducing the maximum joint velocities 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 \ No newline at end of file From 1fa17b347ae58fe2bee9f745faefd83c67e3adbd Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 11:16:54 +0200 Subject: [PATCH 19/40] add docs --- docs/libraries.rst | 142 ++++++++++++++++++ .../lib_osc/moveit2.osc | 4 +- 2 files changed, 144 insertions(+), 2 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index d1813dc5..f1f732f3 100644 --- a/docs/libraries.rst +++ b/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. @@ -1436,3 +1438,143 @@ 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 `__ manipulation stack. Import it with ``import osc.moveit2``. It is provided by the package :repo_link:`libs/scenario_execution_moveit2`. + +``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. + +.. 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] (eg. [move_group_type!arm, move_group_type!gripper]) + * - ``plan_only`` + - ``bool`` + - ``false`` + - If true, the action returns an executable plan in the response but does not attempt execution + * - ``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`` + - ``false`` + - 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 specified position + + * - Parameter + - Type + - Default + - Description + * - ``goal_pose`` + - ``list of string`` + - + - end effector pose to move to [x, y, z, quatx, quaty, quatz, w] + * - ``plan_only`` + - ``bool`` + - ``false`` + - If true, the action returns an executable plan in the response but does not attempt execution + * - ``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`` + - ``false`` + - 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 \ No newline at end of file diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index c34747e7..265f6ad0 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -30,7 +30,7 @@ action arm.move_to_joint_pose: move_group: move_group_type # move group type (arm, gripper) plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution replan: bool = true # if true, replan if plan becomes invalidated during execution - tolerance: float = 0.001 # tolerance bettwen actual state and start and final state + 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 = '' # if set, it's used as namespace (instead of the associated actor's name) action_topic: string = 'move_action' # Name of action @@ -41,7 +41,7 @@ action arm.move_to_pose: goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution replan: bool = true # if true, replan if plan becomes invalidated during execution - tolerance: float = 0.001 # tolerance bettwen actual state and start and final state + 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 = '' # if set, it's used as namespace (instead of the associated actor's name) action_topic: string = 'move_action' # Name of action From 80e95b2ef087f10fecc0fd9a2a04cb3986354104 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 11:42:07 +0200 Subject: [PATCH 20/40] fix doc --- docs/libraries.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index 4bf8f322..a4dd181d 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1525,7 +1525,7 @@ Use Moveit2 to move the arm joints to specified joint positions. * - ``move_group`` - ``move_group_type`` - - - Move group type. Allowed [arm, gripper] (eg. [move_group_type!arm, move_group_type!gripper]) + - Move group type. Allowed [arm, gripper] (e.g. ``[move_group_type!arm, move_group_type!gripper]``) * - ``plan_only`` - ``bool`` - ``false`` @@ -1567,7 +1567,7 @@ Use moveit2 to move the end-effector to specified position * - ``goal_pose`` - ``list of string`` - - - end effector pose to move to [x, y, z, quatx, quaty, quatz, w] + - end effector pose to move to ``[x, y, z, quatx, quaty, quatz, w]`` * - ``plan_only`` - ``bool`` - ``false`` From db6d2aa265f414adf678b6755ca84395996f63dd Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 11:46:14 +0200 Subject: [PATCH 21/40] fix dict --- docs/dictionary.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/dictionary.txt b/docs/dictionary.txt index f46eb76c..76adebcb 100644 --- a/docs/dictionary.txt +++ b/docs/dictionary.txt @@ -33,4 +33,6 @@ png svg Kubernetes yaml -absolutized \ No newline at end of file +absolutized +moveit +replan \ No newline at end of file From b59877ae6a30dd26455c17b41db556f295f6ed2e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 14:25:48 +0200 Subject: [PATCH 22/40] cleanup --- simulation/gazebo/arm_sim_scenario/CMakeLists.txt | 2 +- .../{urdf => config}/control.urdf.xacro | 0 .../arm.urdf.xacro => config/panda.urdf.xacro} | 2 +- .../launch/arm_description_launch.py | 14 ++++++++++---- .../launch/controller_manager_launch.py | 15 ++++++++------- .../arm_sim_scenario/launch/ignition_launch.py | 2 +- .../arm_sim_scenario/launch/moveit_launch.py | 2 +- 7 files changed, 22 insertions(+), 15 deletions(-) rename simulation/gazebo/arm_sim_scenario/{urdf => config}/control.urdf.xacro (100%) rename simulation/gazebo/arm_sim_scenario/{urdf/arm.urdf.xacro => config/panda.urdf.xacro} (99%) diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index f9988310..51876156 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY launch config urdf + DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro similarity index 100% rename from simulation/gazebo/arm_sim_scenario/urdf/control.urdf.xacro rename to simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro diff --git a/simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro similarity index 99% rename from simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro rename to simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro index 7fa1503c..4e0315b6 100644 --- a/simulation/gazebo/arm_sim_scenario/urdf/arm.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro @@ -332,6 +332,6 @@ - + diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index e6a8b2d8..ae0a9f32 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -23,6 +23,7 @@ from launch.conditions import IfCondition from launch_ros.actions import Node from launch_ros.descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare ARGUMENTS = [ DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', @@ -44,14 +45,19 @@ 'arm.rviz', ]), description='file path to the config file RViz should load.',), + DeclareLaunchArgument('urdf_pkg', default_value='arm_sim_scenario', + description='Package where URDF/Xacro file is located (file should be inside the config dir of pkg/config/robot_name.urdf.xacro)'), + DeclareLaunchArgument('urdf', default_value='panda.urdf.xacro', + description='Name of URDF/Xacro file') ] def generate_launch_description(): - pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') - xacro_file = PathJoinSubstitution([pkg_arm_sim_scenario, - 'urdf', - 'arm.urdf.xacro']) + + pkg_urdf = FindPackageShare(LaunchConfiguration('urdf_pkg')) + xacro_file = PathJoinSubstitution([pkg_urdf, + 'config', + LaunchConfiguration('urdf')]) ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') virtual_joint_child_name = LaunchConfiguration('virtual_joint_child_name') virtual_joint_parent_frame = LaunchConfiguration('virtual_joint_parent_frame') diff --git a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py index 08b1355a..637c92f8 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/controller_manager_launch.py @@ -14,14 +14,13 @@ # # SPDX-License-Identifier: Apache-2.0 - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.substitutions.launch_configuration import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch.conditions import LaunchConfigurationNotEquals from launch_ros.actions import Node -import os +from launch_ros.substitutions import FindPackageShare ARGUMENTS = [ DeclareLaunchArgument('arm_group_controller', default_value='panda_arm_controller', @@ -31,19 +30,21 @@ DeclareLaunchArgument('ros2_control_hardware_type', default_value='mock_components', choices=['ignition', 'mock_components'], description='ROS2 control hardware interface type to use for the launch file'), + DeclareLaunchArgument('ros2_control_config_pkg', default_value='moveit_resources_panda_moveit_config', + description='ROS2 control config pkg (file should be inside the config dir of pkg/config/ros2_controllers.yam)') ] def generate_launch_description(): - pkg_moveit_resources_panda_moveit_config = get_package_share_directory('moveit_resources_panda_moveit_config') + pkg_controller_config = FindPackageShare(LaunchConfiguration('ros2_control_config_pkg')) arm_group_controller = LaunchConfiguration('arm_group_controller') gripper_group_controller = LaunchConfiguration('gripper_group_controller') - ros2_controllers_path = os.path.join( - pkg_moveit_resources_panda_moveit_config, + ros2_controllers_path = PathJoinSubstitution([ + pkg_controller_config, "config", - "ros2_controllers.yaml", + "ros2_controllers.yaml"] ) ros2_control_node = Node( diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 807a3532..af1b566a 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -61,7 +61,7 @@ def generate_launch_description(): "-topic", "robot_description", "-name", - "panda", + "arm", "-allow-renaming", "true", ], diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index 648df354..f63291e0 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): moveit_config_builder = MoveItConfigsBuilder("moveit_resources_panda") moveit_config_builder._MoveItConfigsBuilder__urdf_package = pkg_arm_sim_scenario # pylint: disable=W0212 - moveit_config_builder._MoveItConfigsBuilder__urdf_file_path = Path("urdf/arm.urdf.xacro") # pylint: disable=W0212 + moveit_config_builder._MoveItConfigsBuilder__urdf_file_path = Path("config/panda.urdf.xacro") # pylint: disable=W0212 moveit_config = ( moveit_config_builder From 76ece12dad6984a029a1268b849e9d95453aab3e Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 14:39:11 +0200 Subject: [PATCH 23/40] add world file --- .../gazebo/arm_sim_scenario/CMakeLists.txt | 2 +- .../launch/ignition_launch.py | 2 +- .../gazebo/arm_sim_scenario/worlds/maze.sdf | 1066 +++++++++++++++++ 3 files changed, 1068 insertions(+), 2 deletions(-) create mode 100644 simulation/gazebo/arm_sim_scenario/worlds/maze.sdf diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index 51876156..272773e2 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY launch config + DIRECTORY launch config worlds DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index af1b566a..5513d062 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -23,7 +23,7 @@ ARGUMENTS = [ DeclareLaunchArgument('world', default_value=os.path.join(get_package_share_directory( - 'tb4_sim_scenario'), 'worlds', 'maze.sdf'), + 'arm_sim_scenario'), 'worlds', 'maze.sdf'), description='Simulation World File'), DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], diff --git a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf new file mode 100644 index 00000000..cde97194 --- /dev/null +++ b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf @@ -0,0 +1,1066 @@ + + + + + + 0.003 + 1 + 1000 + + + + + + + ogre + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.90000000000000002 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0 0 -0 0 + + + + 0 0 1 0 0 0 + true + + -10.005 0 0 0 0 0 + true + + + + 0.01 20 2 + + + + + + + 0.01 20 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 10.005 0 0 0 0 0 + true + + + + 0.01 20 2 + + + + + + + 0.01 20 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 0 10.005 0 0 0 0 + true + + + + 20 0.01 2 + + + + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + 0 -10.005 0 0 0 0 + true + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + + 20 0.01 2 + + + + 0.1 0.1 0.1 1 + 0 0.01 0.05 1 + 0 0.01 0.05 1 + + + + + + + 0 0 0.5 0 0 0 + true + + -6.5 -9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -8.5 -6.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -3.5 -7 0 0 0 0 + true + + + + 1 6 1 + + + + + + + 1 6 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 -9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7 -8.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -9 -0.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -8.5 -2.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -4.5 -1.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -6 1 0 0 0 0 + true + + + + 2 2 1 + + + + + + + 2 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -7.5 4.5 0 0 0 0 + true + + + + 5 1 1 + + + + + + + 5 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -3 7.5 0 0 0 0 + true + + + + 4 1 1 + + + + + + + 4 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + 0 5.5 0 0 0 0 + true + + + + 2 9 1 + + + + + + + 2 9 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 0 -2.5 0 0 0 0 + true + + + + 2 1 1 + + + + + + + 2 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -0.5 -1.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + -0.5 -4.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 0.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 -4.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 -4.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 8.5 -5.5 0 0 0 0 + true + + + + 3 1 1 + + + + + + + 3 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 9.5 -4.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 3.5 -0.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7 0.5 0 0 0 0 + true + + + + 6 1 1 + + + + + + + 6 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7.5 2 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 1.5 5.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 2.5 6 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 3.5 7.5 0 0 0 0 + true + + + + 1 3 1 + + + + + + + 1 3 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 8.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 5.5 9 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 9.5 0 0 0 0 + true + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 4.5 3 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 5.5 4 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 6.5 5 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 7.5 6 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 8.5 7 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + 9.5 8 0 0 0 0 + true + + + + 1 2 1 + + + + + + + 1 2 1 + + + + 0.1 0.1 0.1 1 + 0 0.1 0.2 1 + 0 0.01 0.05 1 + + + + + + \ No newline at end of file From 1d74fcbaf2c53b8b413e152e5334beaf023f0ec5 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 15:07:54 +0200 Subject: [PATCH 24/40] igniton launch restructure --- .../launch/ignition_launch.py | 56 ++++++++++++------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 5513d062..05ffc750 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -21,27 +21,43 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +# Define Launch Arguments ARGUMENTS = [ - DeclareLaunchArgument('world', default_value=os.path.join(get_package_share_directory( - 'arm_sim_scenario'), 'worlds', 'maze.sdf'), - description='Simulation World File'), - DeclareLaunchArgument('use_sim_time', default_value='true', - choices=['true', 'false'], - description='use_sim_time'), + DeclareLaunchArgument( + 'world', + default_value=os.path.join( + get_package_share_directory('tb4_sim_scenario'), 'worlds', 'maze.sdf'), + description='Simulation World File' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + choices=['true', 'false'], + description='Use simulation time' + ), + DeclareLaunchArgument( + 'resource_package_path', + default_value=os.path.dirname(get_package_share_directory('moveit_resources_panda_description')), + description='Directory containing Ignition resources' + ), ] def generate_launch_description(): - + # Launch Configurations world = LaunchConfiguration('world') - pkg_moveit_resources_panda_description = get_package_share_directory('moveit_resources_panda_description') + resource_package_path = LaunchConfiguration('resource_package_path') - env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': - ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), - os.environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname(pkg_moveit_resources_panda_description)} + # Set environment variables + env = { + 'GZ_SIM_SYSTEM_PLUGIN_PATH': ':'.join([ + os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), + os.environ.get('LD_LIBRARY_PATH', '') + ]), + 'IGN_GAZEBO_RESOURCE_PATH': resource_package_path + } - # Ignition gazebo + # Ignition Gazebo Launch ignition_gazebo = ExecuteProcess( cmd=['ign', 'gazebo', world, '-r', '-v', '4'], output='screen', @@ -53,21 +69,19 @@ def generate_launch_description(): emulate_tty=True ) + # Spawn Robot Node spawn_robot_node = Node( package="ros_gz_sim", executable="create", output="screen", arguments=[ - "-topic", - "robot_description", - "-name", - "arm", - "-allow-renaming", - "true", + "-topic", "robot_description", + "-name", "arm", + "-allow-renaming", "true", ], ) - # Clock Bridge + # Clock Bridge Node clock_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", @@ -75,8 +89,10 @@ def generate_launch_description(): output="screen", ) + # Create and Return Launch Description ld = LaunchDescription(ARGUMENTS) ld.add_action(ignition_gazebo) ld.add_action(clock_bridge) ld.add_action(spawn_robot_node) + return ld From 5753cf61d94995f54641be9c342d416c2f1ae9c7 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 15:09:44 +0200 Subject: [PATCH 25/40] cleanuo --- libs/scenario_execution_moveit2/package.xml | 3 ++- simulation/gazebo/arm_sim_scenario/package.xml | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libs/scenario_execution_moveit2/package.xml b/libs/scenario_execution_moveit2/package.xml index 344afcf2..8b815748 100644 --- a/libs/scenario_execution_moveit2/package.xml +++ b/libs/scenario_execution_moveit2/package.xml @@ -6,7 +6,8 @@ Scenario Execution library for moveit2 Intel Labs Intel Labs - Apache-2.0 + Apache-2.0 + scenario_execution_ros diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index 8c57b5db..c7534492 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -6,7 +6,8 @@ Moveit2 Arm Simulation Scenario Execution Intel Labs Intel Labs - Apache-2.0 + Apache-2.0 + ament_cmake From 47de78ac8cae1de6afc2d71bfae56862711a37d6 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 15:15:30 +0200 Subject: [PATCH 26/40] fix --- simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 05ffc750..7e724aa4 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -26,7 +26,7 @@ DeclareLaunchArgument( 'world', default_value=os.path.join( - get_package_share_directory('tb4_sim_scenario'), 'worlds', 'maze.sdf'), + get_package_share_directory('arm_sim_scenario'), 'worlds', 'maze.sdf'), description='Simulation World File' ), DeclareLaunchArgument( From e526151694c260a59e2b31a0996d43c0d7b953d5 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 17:08:10 +0200 Subject: [PATCH 27/40] remove submodule --- .gitmodules | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index 36c9071e..e69de29b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +0,0 @@ -[submodule "dependencies/moveit_resources"] - path = dependencies/moveit_resources - url = https://github.com/moveit/moveit_resources - branch = humble From 353af08c0c5b545234b2f7b965ce9d25b9667e08 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 17:09:29 +0200 Subject: [PATCH 28/40] remove submodule --- dependencies/moveit_resources | 1 - 1 file changed, 1 deletion(-) delete mode 160000 dependencies/moveit_resources diff --git a/dependencies/moveit_resources b/dependencies/moveit_resources deleted file mode 160000 index 45ea3551..00000000 --- a/dependencies/moveit_resources +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 45ea3551aa74fd5dc40cc39ef53391425cda0af9 From 15d86837b30151bee41ce8ed92957da10e2371ab Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 17:14:46 +0200 Subject: [PATCH 29/40] update dependencies --- simulation/gazebo/arm_sim_scenario/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index c7534492..ac7534ea 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -24,7 +24,7 @@ ros2_control ros_gz_bridge ros_gz_sim - + moveit_resources ament_lint_auto ament_lint_common From cbe23cadc067a0e136792ecb12dab52469490a5f Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Wed, 2 Oct 2024 17:17:22 +0200 Subject: [PATCH 30/40] fix depen.. --- simulation/gazebo/arm_sim_scenario/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index ac7534ea..a086b320 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -24,7 +24,6 @@ ros2_control ros_gz_bridge ros_gz_sim - moveit_resources ament_lint_auto ament_lint_common From 569a943f572b132d74034041701c4284a7740252 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Fri, 4 Oct 2024 13:19:33 +0200 Subject: [PATCH 31/40] cleanup --- .../gazebo/arm_sim_scenario/CMakeLists.txt | 2 +- .../config/control.urdf.xacro | 35 +- .../arm_sim_scenario/config/panda.urdf.xacro | 155 ++- .../launch/ignition_launch.py | 33 +- .../gazebo/arm_sim_scenario/worlds/maze.sdf | 1066 ----------------- 5 files changed, 100 insertions(+), 1191 deletions(-) delete mode 100644 simulation/gazebo/arm_sim_scenario/worlds/maze.sdf diff --git a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt index 272773e2..51876156 100644 --- a/simulation/gazebo/arm_sim_scenario/CMakeLists.txt +++ b/simulation/gazebo/arm_sim_scenario/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(ament_cmake REQUIRED) install( - DIRECTORY launch config worlds + DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) diff --git a/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro index afa406b8..3e954d4e 100644 --- a/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/config/control.urdf.xacro @@ -1,8 +1,6 @@ - - - + mock_components/GenericSystem @@ -12,68 +10,55 @@ ign_ros2_control/IgnitionSystem + ${initial_positions['panda_joint1']} - - 0.0 - + ${initial_positions['panda_joint2']} - - 0.0 - + ${initial_positions['panda_joint3']} - - 0.0 - + ${initial_positions['panda_joint4']} - - 0.0 - + ${initial_positions['panda_joint5']} - - 0.0 - + ${initial_positions['panda_joint6']} - - 0.0 - + ${initial_positions['panda_joint7']} - - 0.0 - + @@ -104,7 +89,7 @@ - + diff --git a/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro index 4e0315b6..fc4f1848 100644 --- a/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro @@ -3,69 +3,48 @@ - - - - - - - - - - - - + + - - - - - - - - - - - - + - + - + - + - - - + + + - + - + - - - + + + @@ -75,22 +54,23 @@ + - + - + - - - + + + @@ -100,22 +80,23 @@ + - + - + - - - + + + @@ -125,22 +106,23 @@ + - + - + - - - + + + @@ -150,22 +132,23 @@ + - + - + - - - + + + @@ -175,22 +158,23 @@ + - + - + - - - + + + @@ -200,22 +184,23 @@ + - + - + - - - + + + @@ -225,6 +210,7 @@ + @@ -241,52 +227,54 @@ - + - + - - - + + + - + - + - - + + + - + - + - - + + + @@ -295,6 +283,7 @@ + @@ -303,12 +292,18 @@ + - + + + + + + - diff --git a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py index 7e724aa4..1e2ab03c 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/ignition_launch.py @@ -17,44 +17,38 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, Shutdown, ExecuteProcess +from launch.substitutions import LaunchConfiguration, EnvironmentVariable from launch_ros.actions import Node # Define Launch Arguments ARGUMENTS = [ DeclareLaunchArgument( 'world', - default_value=os.path.join( - get_package_share_directory('arm_sim_scenario'), 'worlds', 'maze.sdf'), + default_value="empty.sdf", description='Simulation World File' - ), - DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - choices=['true', 'false'], - description='Use simulation time' - ), - DeclareLaunchArgument( - 'resource_package_path', - default_value=os.path.dirname(get_package_share_directory('moveit_resources_panda_description')), - description='Directory containing Ignition resources' - ), + ) ] def generate_launch_description(): - # Launch Configurations + world = LaunchConfiguration('world') - resource_package_path = LaunchConfiguration('resource_package_path') # Set environment variables + ign_gazebo_resource_path = SetEnvironmentVariable( + name='IGN_GAZEBO_RESOURCE_PATH', + value=[ + EnvironmentVariable('IGN_GAZEBO_RESOURCE_PATH', default_value=os.path.dirname( + get_package_share_directory('moveit_resources_panda_description'))) + ] + ) + env = { 'GZ_SIM_SYSTEM_PLUGIN_PATH': ':'.join([ os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), os.environ.get('LD_LIBRARY_PATH', '') ]), - 'IGN_GAZEBO_RESOURCE_PATH': resource_package_path } # Ignition Gazebo Launch @@ -91,6 +85,7 @@ def generate_launch_description(): # Create and Return Launch Description ld = LaunchDescription(ARGUMENTS) + ld.add_action(ign_gazebo_resource_path) ld.add_action(ignition_gazebo) ld.add_action(clock_bridge) ld.add_action(spawn_robot_node) diff --git a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf b/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf deleted file mode 100644 index cde97194..00000000 --- a/simulation/gazebo/arm_sim_scenario/worlds/maze.sdf +++ /dev/null @@ -1,1066 +0,0 @@ - - - - - - 0.003 - 1 - 1000 - - - - - - - ogre - - - - 1 - 0 0 10 0 -0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.90000000000000002 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - 0 - 0 - 0 - - - 0 0 -9.8 - 6e-06 2.3e-05 -4.2e-05 - - - 0.4 0.4 0.4 1 - 0.7 0.7 0.7 1 - 1 - - - 1 - - - - - 0 0 1 - 100 100 - - - - - - - - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - 0 0 0 0 -0 0 - - - - 0 0 1 0 0 0 - true - - -10.005 0 0 0 0 0 - true - - - - 0.01 20 2 - - - - - - - 0.01 20 2 - - - - 0.1 0.1 0.1 1 - 0 0.01 0.05 1 - 0 0.01 0.05 1 - - - - - 10.005 0 0 0 0 0 - true - - - - 0.01 20 2 - - - - - - - 0.01 20 2 - - - - 0.1 0.1 0.1 1 - 0 0.01 0.05 1 - 0 0.01 0.05 1 - - - - - 0 10.005 0 0 0 0 - true - - - - 20 0.01 2 - - - - - - - 20 0.01 2 - - - - 0.1 0.1 0.1 1 - 0 0.01 0.05 1 - 0 0.01 0.05 1 - - - - - 0 -10.005 0 0 0 0 - true - - - - 20 0.01 2 - - - - 0.1 0.1 0.1 1 - 0 0.01 0.05 1 - 0 0.01 0.05 1 - - - - - - 20 0.01 2 - - - - 0.1 0.1 0.1 1 - 0 0.01 0.05 1 - 0 0.01 0.05 1 - - - - - - - 0 0 0.5 0 0 0 - true - - -6.5 -9 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -8.5 -6.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -3.5 -7 0 0 0 0 - true - - - - 1 6 1 - - - - - - - 1 6 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 2.5 -9 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 7 -8.5 0 0 0 0 - true - - - - 2 1 1 - - - - - - - 2 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -9 -0.5 0 0 0 0 - true - - - - 2 1 1 - - - - - - - 2 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -8.5 -2.5 0 0 0 0 - true - - - - 1 3 1 - - - - - - - 1 3 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -4.5 -1.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -6 1 0 0 0 0 - true - - - - 2 2 1 - - - - - - - 2 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -7.5 4.5 0 0 0 0 - true - - - - 5 1 1 - - - - - - - 5 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -3 7.5 0 0 0 0 - true - - - - 4 1 1 - - - - - - - 4 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - 0 5.5 0 0 0 0 - true - - - - 2 9 1 - - - - - - - 2 9 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 0 -2.5 0 0 0 0 - true - - - - 2 1 1 - - - - - - - 2 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -0.5 -1.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - -0.5 -4.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 0.5 -5.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 2.5 -4.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 4.5 -5.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 6.5 -4.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 8.5 -5.5 0 0 0 0 - true - - - - 3 1 1 - - - - - - - 3 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 9.5 -4.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 3.5 -0.5 0 0 0 0 - true - - - - 1 3 1 - - - - - - - 1 3 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 7 0.5 0 0 0 0 - true - - - - 6 1 1 - - - - - - - 6 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 7.5 2 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 1.5 5.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 2.5 6 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 3.5 7.5 0 0 0 0 - true - - - - 1 3 1 - - - - - - - 1 3 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 4.5 8.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 5.5 9 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 6.5 9.5 0 0 0 0 - true - - - - 1 1 1 - - - - - - - 1 1 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 4.5 3 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 5.5 4 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 6.5 5 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 7.5 6 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 8.5 7 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - 9.5 8 0 0 0 0 - true - - - - 1 2 1 - - - - - - - 1 2 1 - - - - 0.1 0.1 0.1 1 - 0 0.1 0.2 1 - 0 0.01 0.05 1 - - - - - - \ No newline at end of file From 688169340b911a872c99c0958d72bd833a452da9 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 7 Oct 2024 10:54:39 +0200 Subject: [PATCH 32/40] fix urdf --- simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro index fc4f1848..16923ac8 100644 --- a/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro +++ b/simulation/gazebo/arm_sim_scenario/config/panda.urdf.xacro @@ -303,7 +303,7 @@ - + From 37d34835f0bf1524a22443e59f5b8ca99e6868e2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 7 Oct 2024 15:11:06 +0200 Subject: [PATCH 33/40] fix doc and action arg type --- docs/libraries.rst | 19 +++++++++++-------- examples/example_moveit2/example_moveit2.osc | 2 +- .../actions/move_to_pose.py | 12 ++---------- .../lib_osc/moveit2.osc | 2 +- 4 files changed, 15 insertions(+), 20 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index a4dd181d..62e541e4 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1462,8 +1462,11 @@ Moveit2 The library contains actions to interact with the `Moveit2 `__ 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:: @@ -1505,9 +1508,9 @@ An articulated arm actor inheriting from the more general ``robot`` actor - 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. +Use MoveIt2 to move the arm joints to specified joint positions, utilizing `MoveGroup action `__ from the move_group node () by specifying target joint values. .. list-table:: :widths: 15 15 5 65 @@ -1540,7 +1543,7 @@ Use Moveit2 to move the arm joints to specified joint positions. - The acceptable range of variation around both the start and goal positions. * - ``max_velocity_scaling_factor`` - ``float`` - - ``false`` + - ``0.1`` - Scaling factors for optionally reducing the maximum joint velocities * - ``namespace_override`` - ``string`` @@ -1556,16 +1559,16 @@ Use Moveit2 to move the arm joints to specified joint positions. - Succeed on goal acceptance ``arm.move_to_pose`` -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^ -Use moveit2 to move the end-effector to specified position +Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup action `__ from the move_group node by specifying the desired end-effector position and orientation. * - Parameter - Type - Default - Description * - ``goal_pose`` - - ``list of string`` + - ``pose_3d`` - - end effector pose to move to ``[x, y, z, quatx, quaty, quatz, w]`` * - ``plan_only`` @@ -1582,7 +1585,7 @@ Use moveit2 to move the end-effector to specified position - The acceptable range of variation around both the start and goal positions. * - ``max_velocity_scaling_factor`` - ``float`` - - ``false`` + - ``0.1`` - Scaling factors for optionally reducing the maximum joint velocities * - ``namespace_override`` - ``string`` diff --git a/examples/example_moveit2/example_moveit2.osc b/examples/example_moveit2/example_moveit2.osc index 992613ee..f4d5c0f1 100644 --- a/examples/example_moveit2/example_moveit2.osc +++ b/examples/example_moveit2/example_moveit2.osc @@ -18,7 +18,7 @@ scenario example_moveit2: 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']) + 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) diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py index c074414a..4cce7ae0 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_pose.py @@ -15,10 +15,10 @@ # SPDX-License-Identifier: Apache-2.0 from rclpy.duration import Duration +from scenario_execution_ros.actions.common import get_pose_stamped 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 @@ -50,15 +50,7 @@ def get_goal_msg(self): 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]) + target_pose = get_pose_stamped(self.node.get_clock().now().to_msg(), self.goal_pose) # Create PositionConstraint position_constraint = PositionConstraint() diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index 265f6ad0..a64916a3 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -38,7 +38,7 @@ action arm.move_to_joint_pose: action arm.move_to_pose: # Use moveit2 to move the end-effector to specified position - goal_pose: list of string # end effector pose to move to [x, y, z, quatx, quaty, quatz, w] + goal_pose: pose_3d # end effector pose to move to plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution 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. From d135b1f10855385bc5e7d59310500e66a067ab03 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 7 Oct 2024 15:53:39 +0200 Subject: [PATCH 34/40] doc update --- docs/libraries.rst | 4 +- .../lib_osc/moveit2.osc | 4 +- .../gazebo/arm_sim_scenario/config/arm.rviz | 197 ++++++++++++++---- .../launch/arm_description_launch.py | 29 --- .../arm_sim_scenario/launch/moveit_launch.py | 30 ++- .../launch/sim_moveit_scenario_launch.py | 2 +- 6 files changed, 187 insertions(+), 79 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index 62e541e4..704c57fe 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1532,7 +1532,7 @@ Use MoveIt2 to move the arm joints to specified joint positions, utilizing `Move * - ``plan_only`` - ``bool`` - ``false`` - - If true, the action returns an executable plan in the response but does not attempt execution + - If true, the plan is calculated but not executed. The calculated plan can be visualized in rviz. * - ``replan`` - ``bool`` - ``true`` @@ -1574,7 +1574,7 @@ Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup a * - ``plan_only`` - ``bool`` - ``false`` - - If true, the action returns an executable plan in the response but does not attempt execution + - If true, the plan is calculated but not executed. The calculated plan can be visualized in rviz. * - ``replan`` - ``bool`` - ``true`` diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index a64916a3..dc1328d3 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -28,7 +28,7 @@ action arm.move_to_joint_pose: # Use Moveit2 to move the arm joints to specified joint positions goal_pose: list of string # list joint positions to move to move_group: move_group_type # move group type (arm, gripper) - plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution + 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 @@ -39,7 +39,7 @@ action arm.move_to_joint_pose: action arm.move_to_pose: # Use moveit2 to move the end-effector to specified position goal_pose: pose_3d # end effector pose to move to - plan_only: bool = false # if true, the action returns an executable plan in the response but does not attempt execution + plan_only: bool = false # If true, the plan is calculated but not executed. The calculated plan can then 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 diff --git a/simulation/gazebo/arm_sim_scenario/config/arm.rviz b/simulation/gazebo/arm_sim_scenario/config/arm.rviz index 6b5476fd..112f4c65 100644 --- a/simulation/gazebo/arm_sim_scenario/config/arm.rviz +++ b/simulation/gazebo/arm_sim_scenario/config/arm.rviz @@ -1,11 +1,11 @@ Panels: - Class: rviz_common/Displays - Help Height: 0 + Help Height: 78 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 234 + Tree Height: 244 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -17,16 +17,8 @@ Panels: - Class: rviz_common/Views Expanded: - /Current View1 - - /Current View1/Focal Point1 Name: Views Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: scenario_execution_rviz/Scenario Control - Name: Scenario Control - Class: scenario_execution_rviz/Scenario View Name: Scenario View snapshot_topic: /scenario_execution/snapshots @@ -51,24 +43,33 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /rviz_visual_tools + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false Enabled: true + Interrupt Display: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + ground_plane_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true panda_hand: Alpha: 1 Show Axes: false @@ -128,18 +129,128 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 + random: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + ground_plane_box: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + random: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true Value: true - Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Fixed Frame: panda_link0 Frame Rate: 30 Name: root Tools: @@ -182,45 +293,43 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.4266018867492676 + Distance: 3.119211196899414 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.16563944518566132 - Y: -0.11561775952577591 - Z: 0.2661708891391754 - Focal Shape Fixed Size: false + X: -0.05966663733124733 + Y: 0.006320085376501083 + Z: 0.08645729720592499 + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.31039851903915405 + Pitch: 0.4253983497619629 Target Frame: Value: Orbit (rviz) - Yaw: 1.0954009294509888 + Yaw: 0.985392689704895 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 839 + Height: 902 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 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 - Scenario Control: - collapsed: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c200000324fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004600000182000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004700fffffffb0000001a005300630065006e006100720069006f0020005600690065007701000001cd0000019d0000007100ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000ab00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003d20000032400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Scenario View: collapsed: false Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false - Views: + Trajectory - Trajectory Slider: collapsed: false - Width: 1564 - X: 173 - Y: 144 + Views: + collapsed: true + Width: 1433 + X: 167 + Y: 148 diff --git a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py index ae0a9f32..afbb2003 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/arm_description_launch.py @@ -14,13 +14,10 @@ # # SPDX-License-Identifier: Apache-2.0 - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, PathJoinSubstitution from launch.substitutions.launch_configuration import LaunchConfiguration -from launch.conditions import IfCondition from launch_ros.actions import Node from launch_ros.descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare @@ -36,15 +33,6 @@ description='arm base_link name'), DeclareLaunchArgument('virtual_joint_parent_frame', default_value='world', description='virtual_joint_parent_frame name to which arm is attached to'), - DeclareLaunchArgument('use_rviz', default_value='false', - choices=['true', 'false'], - description='launches RViz if set to `true`.'), - DeclareLaunchArgument('rviz_config', - default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), - 'config', - 'arm.rviz', - ]), - description='file path to the config file RViz should load.',), DeclareLaunchArgument('urdf_pkg', default_value='arm_sim_scenario', description='Package where URDF/Xacro file is located (file should be inside the config dir of pkg/config/robot_name.urdf.xacro)'), DeclareLaunchArgument('urdf', default_value='panda.urdf.xacro', @@ -61,9 +49,7 @@ def generate_launch_description(): ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') virtual_joint_child_name = LaunchConfiguration('virtual_joint_child_name') virtual_joint_parent_frame = LaunchConfiguration('virtual_joint_parent_frame') - rviz_config = LaunchConfiguration('rviz_config') use_sim_time = LaunchConfiguration('use_sim_time') - use_rviz = LaunchConfiguration('use_rviz') robot_state_publisher = Node( package="robot_state_publisher", @@ -95,24 +81,9 @@ def generate_launch_description(): output={'both': 'log'}, ) - rviz_node = Node( - condition=IfCondition(use_rviz), - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=[ - '-d', rviz_config, - ], - parameters=[{ - 'use_sim_time': use_sim_time, - }], - output={'both': 'log'}, - ) - # Define LaunchDescription variable ld = LaunchDescription(ARGUMENTS) ld.add_action(robot_state_publisher) ld.add_action(joint_state_publisher) ld.add_action(static_tf_node) - ld.add_action(rviz_node) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py index f63291e0..dc438505 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/moveit_launch.py @@ -17,7 +17,8 @@ from pathlib import Path from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder @@ -29,6 +30,15 @@ DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='use_sim_time'), + DeclareLaunchArgument('use_rviz', default_value='false', + choices=['true', 'false'], + description='launches RViz if set to `true`.'), + DeclareLaunchArgument('rviz_config', + default_value=PathJoinSubstitution([get_package_share_directory('arm_sim_scenario'), + 'config', + 'arm.rviz', + ]) + ) ] @@ -36,6 +46,8 @@ def generate_launch_description(): pkg_arm_sim_scenario = get_package_share_directory('arm_sim_scenario') ros2_control_hardware_type = LaunchConfiguration('ros2_control_hardware_type') use_sim_time = LaunchConfiguration('use_sim_time') + rviz_config = LaunchConfiguration('rviz_config') + use_rviz = LaunchConfiguration('use_rviz') moveit_config_builder = MoveItConfigsBuilder("moveit_resources_panda") @@ -67,6 +79,22 @@ def generate_launch_description(): arguments=["--ros-args", "--log-level", "info"], ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ], + condition=IfCondition(use_rviz), + ) + ld = LaunchDescription(ARGUMENTS) ld.add_action(move_group_node) + ld.add_action(rviz_node) return ld diff --git a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py index c599c1bc..a84238e3 100644 --- a/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py +++ b/simulation/gazebo/arm_sim_scenario/launch/sim_moveit_scenario_launch.py @@ -57,6 +57,7 @@ def generate_launch_description(): moveit_bringup = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'moveit_launch.py'])]), launch_arguments={ + 'use_rviz': use_rviz, 'arg_ros2_control_hardware_type': ros2_control_hardware_type, 'use_sim_time': use_sim_time, }.items(), @@ -65,7 +66,6 @@ def generate_launch_description(): arm_description = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([arm_sim_scenario_dir, 'launch', 'arm_description_launch.py'])]), launch_arguments={ - 'use_rviz': use_rviz, 'arg_ros2_control_hardware_type': ros2_control_hardware_type, 'use_sim_time': use_sim_time, }.items() From 02e3aa8509aa7ac9483edbd3d2521b433556b6f0 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Mon, 7 Oct 2024 17:03:09 +0200 Subject: [PATCH 35/40] fix docs --- docs/libraries.rst | 6 +++--- examples/example_moveit2/example_moveit2.osc | 4 ++-- libs/scenario_execution_moveit2/README.md | 2 +- libs/scenario_execution_moveit2/package.xml | 2 +- simulation/gazebo/arm_sim_scenario/package.xml | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index 704c57fe..26b90d0a 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1466,7 +1466,7 @@ Actors ^^^^^^ ``arm`` -^^^^^^^ +""""""" An articulated arm actor inheriting from the more general ``robot`` actor .. list-table:: @@ -1510,7 +1510,7 @@ An articulated arm actor inheriting from the more general ``robot`` actor ``arm.move_to_joint_pose()`` ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Use MoveIt2 to move the arm joints to specified joint positions, utilizing `MoveGroup action `__ from the move_group node () by specifying target joint values. +Use MoveIt2 to move the arm joints to specified joint positions, utilizing `MoveGroup action `__ from the move_group node by specifying target joint values. .. list-table:: :widths: 15 15 5 65 @@ -1561,7 +1561,7 @@ Use MoveIt2 to move the arm joints to specified joint positions, utilizing `Move ``arm.move_to_pose`` ^^^^^^^^^^^^^^^^^^^^ -Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup action `__ from the move_group node by specifying the desired end-effector position and orientation. +Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup action `__ from the move_group node by specifying the desired end-effector position and orientation. * - Parameter - Type diff --git a/examples/example_moveit2/example_moveit2.osc b/examples/example_moveit2/example_moveit2.osc index f4d5c0f1..eea05b85 100644 --- a/examples/example_moveit2/example_moveit2.osc +++ b/examples/example_moveit2/example_moveit2.osc @@ -15,12 +15,12 @@ scenario example_moveit2: 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'], + goal_pose: ['0.04', '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'], + goal_pose: ['0.04', '0.04'], move_group: move_group_type!gripper) wait elapsed(1s) emit end \ No newline at end of file diff --git a/libs/scenario_execution_moveit2/README.md b/libs/scenario_execution_moveit2/README.md index 072e0c37..c7a0b4f2 100644 --- a/libs/scenario_execution_moveit2/README.md +++ b/libs/scenario_execution_moveit2/README.md @@ -1,4 +1,4 @@ -# Scenario Execution Library for moveit2 +# 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. diff --git a/libs/scenario_execution_moveit2/package.xml b/libs/scenario_execution_moveit2/package.xml index 8b815748..9e9110d4 100644 --- a/libs/scenario_execution_moveit2/package.xml +++ b/libs/scenario_execution_moveit2/package.xml @@ -3,7 +3,7 @@ scenario_execution_moveit2 1.2.0 - Scenario Execution library for moveit2 + Scenario Execution library for moveIt2 Intel Labs Intel Labs Apache-2.0 diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index a086b320..c0e1d012 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -3,7 +3,7 @@ arm_sim_scenario 1.2.0 - Moveit2 Arm Simulation Scenario Execution + MoveIt2 Arm Simulation Scenario Execution Intel Labs Intel Labs Apache-2.0 From 9b84e1612a851b5bb3296590292655427f93dec2 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 8 Oct 2024 13:07:49 +0200 Subject: [PATCH 36/40] change string to float --- docs/libraries.rst | 2 +- examples/example_moveit2/example_moveit2.osc | 6 +++--- .../actions/move_to_joint_pose.py | 4 ++-- .../scenario_execution_moveit2/lib_osc/moveit2.osc | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index 26b90d0a..c806a32a 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1522,7 +1522,7 @@ Use MoveIt2 to move the arm joints to specified joint positions, utilizing `Move - Default - Description * - ``goal_pose`` - - ``list of string`` + - ``list of float`` - - List joint positions to move to * - ``move_group`` diff --git a/examples/example_moveit2/example_moveit2.osc b/examples/example_moveit2/example_moveit2.osc index eea05b85..9b6daca6 100644 --- a/examples/example_moveit2/example_moveit2.osc +++ b/examples/example_moveit2/example_moveit2.osc @@ -12,15 +12,15 @@ scenario example_moveit2: 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'], + 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', '0.04'], + goal_pose: [0.04, 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.04', '0.04'], + goal_pose: [0.04, 0.04], move_group: move_group_type!gripper) wait elapsed(1s) emit end \ No newline at end of file diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py index 2a4f8572..da4e8382 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/actions/move_to_joint_pose.py @@ -60,10 +60,10 @@ def get_goal_msg(self): 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): + for joint_name, position 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.position = float(position) joint_constraint.tolerance_above = self.tolerance joint_constraint.tolerance_below = self.tolerance joint_constraint.weight = 1.0 # Set weight (importance) of this constraint diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index dc1328d3..1c0dfb5f 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -26,7 +26,7 @@ actor arm inherits robot: action arm.move_to_joint_pose: # Use Moveit2 to move the arm joints to specified joint positions - goal_pose: list of string # list joint positions to move to + goal_pose: list of float # list joint positions to move to move_group: move_group_type # move group type (arm, 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 From f56bf71293e9652f37cfd6c6db40c66fb04bcccb Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 8 Oct 2024 13:19:23 +0200 Subject: [PATCH 37/40] update docs --- docs/libraries.rst | 2 +- .../scenario_execution_moveit2/lib_osc/moveit2.osc | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/libraries.rst b/docs/libraries.rst index c806a32a..0086f531 100644 --- a/docs/libraries.rst +++ b/docs/libraries.rst @@ -1570,7 +1570,7 @@ Use MoveIt2 to move the end-effector to a specified pose, utilizing `MoveGroup a * - ``goal_pose`` - ``pose_3d`` - - - end effector pose to move to ``[x, y, z, quatx, quaty, quatz, w]`` + - end effector pose to move to * - ``plan_only`` - ``bool`` - ``false`` diff --git a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc index 1c0dfb5f..457bde61 100644 --- a/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc +++ b/libs/scenario_execution_moveit2/scenario_execution_moveit2/lib_osc/moveit2.osc @@ -11,6 +11,7 @@ enum move_group_type: [ ########### actor arm inherits robot: + # An articulated arm actor inheriting from the more general robot actor 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 From f0ddb0990f071bdf428ea7e4d99bbcd7ca50705c Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 8 Oct 2024 13:55:48 +0200 Subject: [PATCH 38/40] update --- libs/scenario_execution_moveit2/package.xml | 1 - simulation/gazebo/arm_sim_scenario/README.md | 2 +- simulation/gazebo/arm_sim_scenario/package.xml | 9 ++++----- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/libs/scenario_execution_moveit2/package.xml b/libs/scenario_execution_moveit2/package.xml index 9e9110d4..9fb25b9d 100644 --- a/libs/scenario_execution_moveit2/package.xml +++ b/libs/scenario_execution_moveit2/package.xml @@ -8,7 +8,6 @@ Intel Labs Apache-2.0 - scenario_execution_ros rclpy diff --git a/simulation/gazebo/arm_sim_scenario/README.md b/simulation/gazebo/arm_sim_scenario/README.md index 46f4e359..b3beb83b 100644 --- a/simulation/gazebo/arm_sim_scenario/README.md +++ b/simulation/gazebo/arm_sim_scenario/README.md @@ -1 +1 @@ -# Moveit2 Arm Simulation Scenario Execution \ No newline at end of file +# MoveIt2 Arm Simulation Scenario Execution \ No newline at end of file diff --git a/simulation/gazebo/arm_sim_scenario/package.xml b/simulation/gazebo/arm_sim_scenario/package.xml index c0e1d012..95182f06 100644 --- a/simulation/gazebo/arm_sim_scenario/package.xml +++ b/simulation/gazebo/arm_sim_scenario/package.xml @@ -8,22 +8,21 @@ Intel Labs Apache-2.0 - ament_cmake - moveit + moveit_resources + moveit_planners_chomp scenario_execution scenario_execution_moveit2 - moveit_resources controller_manager - ompl - moveit_planners_chomp gripper_controllers joint_trajectory_controller ros2_control + ompl ros_gz_bridge ros_gz_sim + ament_lint_auto ament_lint_common From 6f0689dbe0a31be307b2501106d27286c0a41d84 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 8 Oct 2024 15:57:51 +0200 Subject: [PATCH 39/40] add ci --- .github/workflows/test_build.yml | 35 ++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index 7875e579..6b688e91 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -358,6 +358,39 @@ jobs: with: name: test-example-external-method-result path: test_example_external_method/test.xml + test-example-moveit2: + needs: [build] + runs-on: intellabs-01 + container: + image: ghcr.io/intellabs/scenario-execution:${{ github.event.pull_request.base.ref }} + credentials: + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + steps: + - name: Restore cache + uses: actions/cache@0c45773b623bea8c8e75f6c82b208c3cf94ea4f9 # v4.0.2 + with: + key: ${{ runner.os }}-build-${{ github.run_number }} + path: . + - name: Test Example Moveit2 + shell: bash + run: | + source /opt/ros/${{ github.event.pull_request.base.ref == 'main' && 'humble' || github.event.pull_request.base.ref }}/setup.bash + source install/setup.bash + Xvfb :1 -screen 0 800x600x16 & + export DISPLAY=:1.0 + export -n CYCLONEDDS_URI + export ROS_DOMAIN_ID=2 + export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} + sed -i 's/60s/600s/g' examples/example_moveit2/example_moveit2.osc + # shellcheck disable=SC1083 + scenario_batch_execution -i examples/example_moveit2/ -o test_example_moveit2 -- ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=true + - name: Upload result + uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 + if: always() + with: + name: test-example-moveit2-result + path: test_example_moveit2/test.xml test-scenario-execution-gazebo: needs: [build] runs-on: intellabs-01 @@ -515,6 +548,7 @@ jobs: - test-example-nav2 - test-example-simulation - test-example-multirobot + - test-example-moveit2 - test-example-external-method - test-scenario-execution-gazebo - test-scenario-execution-nav2 @@ -555,6 +589,7 @@ jobs: downloaded-artifacts/test-example-simulation-result/test.xml downloaded-artifacts/test-example-multirobot-result/test.xml downloaded-artifacts/test-example-external-method-result/test.xml + downloaded-artifacts/test-example-moveit2-result/test.xml downloaded-artifacts/test-scenario-execution-gazebo/test.xml downloaded-artifacts/test-scenario-execution-nav2/test.xml downloaded-artifacts/test-scenario-execution-pybullet/test.xml From 7fa8f8325ecec483a2c8854ee04e1bcc716e3916 Mon Sep 17 00:00:00 2001 From: Nikhil Singhal Date: Tue, 8 Oct 2024 16:12:08 +0200 Subject: [PATCH 40/40] update ci --- .github/workflows/test_build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test_build.yml b/.github/workflows/test_build.yml index 6b688e91..6f613217 100644 --- a/.github/workflows/test_build.yml +++ b/.github/workflows/test_build.yml @@ -384,7 +384,7 @@ jobs: export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID} sed -i 's/60s/600s/g' examples/example_moveit2/example_moveit2.osc # shellcheck disable=SC1083 - scenario_batch_execution -i examples/example_moveit2/ -o test_example_moveit2 -- ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=true + scenario_batch_execution -i examples/example_moveit2/ -o test_example_moveit2 -- ros2 launch arm_sim_scenario sim_moveit_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} - name: Upload result uses: actions/upload-artifact@0b2256b8c012f0828dc542b3febcab082c67f72b # v4.3.4 if: always()