From fb7c529dd029a2a71c7bb368b26cf389af20d8c1 Mon Sep 17 00:00:00 2001 From: "A. Yilmaz" <79148268+yilmazabdurrah@users.noreply.github.com> Date: Wed, 31 Jan 2024 16:22:16 +0000 Subject: [PATCH 01/14] semantic description publish enabled --- franka_moveit_config/launch/moveit.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/franka_moveit_config/launch/moveit.launch.py b/franka_moveit_config/launch/moveit.launch.py index ad0f275e..4d34057e 100644 --- a/franka_moveit_config/launch/moveit.launch.py +++ b/franka_moveit_config/launch/moveit.launch.py @@ -122,6 +122,7 @@ def generate_launch_description(): 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, + 'publish_robot_description_semantic': True, } # Start the actual move_group node/action server From 065aad0ad07fcf933bd6705dc0b0d9c3d4dcbfee Mon Sep 17 00:00:00 2001 From: Marc Hanheide Date: Mon, 5 Feb 2024 16:51:20 +0000 Subject: [PATCH 02/14] added lcas ros-ci.yml --- .github/workflows/ros-ci.yml | 68 ++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 .github/workflows/ros-ci.yml diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml new file mode 100644 index 00000000..f6f4bd6f --- /dev/null +++ b/.github/workflows/ros-ci.yml @@ -0,0 +1,68 @@ +name: ros CI + +on: + push: + # you may want to configure the branches that this should be run on here. + branches: [ "humble" ] + pull_request: + branches: [ "humble" ] + +jobs: + test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. + runs-on: ubuntu-latest + strategy: + matrix: + ros_distribution: + # - noetic + - humble + # - iron + + # Define the Docker image(s) associated with each ROS distribution. + # The include syntax allows additional variables to be defined, like + # docker_image in this case. See documentation: + # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build + # + # Platforms are defined in REP 3 and REP 2000: + # https://ros.org/reps/rep-0003.html + # https://ros.org/reps/rep-2000.html + include: + # Noetic Ninjemys (May 2020 - May 2025) + # - docker_image: ubuntu:focal + # ros_distribution: noetic + # ros_version: 1 + + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 + + # Iron Irwini (May 2023 - November 2024) + # - docker_image: ubuntu:jammy + # ros_distribution: iron + # ros_version: 2 + + # # Rolling Ridley (No End-Of-Life) + # - docker_image: ubuntu:jammy + # ros_distribution: rolling + # ros_version: 2 + + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v3 + - name: setup ROS environment + uses: LCAS/setup-ros@master + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + - name: build and test ROS 1 + if: ${{ matrix.ros_version == 1 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros1-distro: ${{ matrix.ros_distribution }} + - name: build and test ROS 2 + if: ${{ matrix.ros_version == 2 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros2-distro: ${{ matrix.ros_distribution }} From 2d8fa98bdf34c1918d74e8bf87a48815b16bd514 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:46:36 +0100 Subject: [PATCH 03/14] Velocity command interface added --- franka_bringup/config/controllers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/franka_bringup/config/controllers.yaml b/franka_bringup/config/controllers.yaml index b1c1bff6..fb241379 100644 --- a/franka_bringup/config/controllers.yaml +++ b/franka_bringup/config/controllers.yaml @@ -44,7 +44,7 @@ joint_trajectory_controller: - panda_joint6 - panda_joint7 command_interfaces: - - effort + - velocity state_interfaces: - position - velocity @@ -153,4 +153,4 @@ cartesian_velocity_example_controller: - 1.0 - 1.0 - 1.0 - - 0.5 \ No newline at end of file + - 0.5 From 759058ecb78ee8d528ad10bf743d0d35b24f97f9 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:50:14 +0100 Subject: [PATCH 04/14] Arm joint limits reduced to eliminate controller errors --- franka_description/robots/panda_arm.xacro | 28 +++++++++++------------ 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro index b44b230a..0d639b1a 100644 --- a/franka_description/robots/panda_arm.xacro +++ b/franka_description/robots/panda_arm.xacro @@ -61,12 +61,12 @@ - + - + @@ -94,12 +94,12 @@ - + - + @@ -127,12 +127,12 @@ - + - + @@ -160,12 +160,12 @@ - + - + @@ -212,12 +212,12 @@ - + - + @@ -245,12 +245,12 @@ - + - + @@ -278,12 +278,12 @@ - + - + From 1d3317642e546fc96bdc1a8740796ceaed279244 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:50:47 +0100 Subject: [PATCH 05/14] Initial configuration changed --- .../robots/panda_arm.ros2_control.xacro | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/franka_description/robots/panda_arm.ros2_control.xacro b/franka_description/robots/panda_arm.ros2_control.xacro index ddc9c198..4c8e98f8 100644 --- a/franka_description/robots/panda_arm.ros2_control.xacro +++ b/franka_description/robots/panda_arm.ros2_control.xacro @@ -27,14 +27,14 @@ - - + + - + - - + + - \ No newline at end of file + From 58ce0dea26ea142803da445a4db5d20418885c67 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:51:51 +0100 Subject: [PATCH 06/14] Arm vision extension --- franka_description/launch/visualize_franka.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/franka_description/launch/visualize_franka.launch.py b/franka_description/launch/visualize_franka.launch.py index e7709d01..e1b194fe 100644 --- a/franka_description/launch/visualize_franka.launch.py +++ b/franka_description/launch/visualize_franka.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): load_gripper = LaunchConfiguration(load_gripper_parameter_name) franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', - 'panda_arm.urdf.xacro') + 'panda_arm_camera.urdf.xacro') robot_description = Command( [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper]) From 8019623d4cb103fdc0ec242d2d929efcade48c57 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:52:46 +0100 Subject: [PATCH 07/14] Panda platform (arm + camera) defined --- .../robots/panda_arm_platform.urdf.xacro | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 franka_description/robots/panda_arm_platform.urdf.xacro diff --git a/franka_description/robots/panda_arm_platform.urdf.xacro b/franka_description/robots/panda_arm_platform.urdf.xacro new file mode 100644 index 00000000..669b47fb --- /dev/null +++ b/franka_description/robots/panda_arm_platform.urdf.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + From d8368e0295af207d401095764f668fcccff6b2dd Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:54:32 +0100 Subject: [PATCH 08/14] Panda vision move group kinematics added --- franka_moveit_config/config/kinematics.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/franka_moveit_config/config/kinematics.yaml b/franka_moveit_config/config/kinematics.yaml index e2ac240e..b2cc0349 100644 --- a/franka_moveit_config/config/kinematics.yaml +++ b/franka_moveit_config/config/kinematics.yaml @@ -10,3 +10,7 @@ right_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 +panda_vision: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 From 261428a50708825ff112e0a56eff598936568d7a Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:55:22 +0100 Subject: [PATCH 09/14] Planning constraints added --- .../config/ompl_planning.yaml | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/franka_moveit_config/config/ompl_planning.yaml b/franka_moveit_config/config/ompl_planning.yaml index ac7c10cf..0a71e81a 100644 --- a/franka_moveit_config/config/ompl_planning.yaml +++ b/franka_moveit_config/config/ompl_planning.yaml @@ -124,6 +124,8 @@ planner_configs: type: geometric::TrajOpt panda_arm: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1, panda_joint2) planner_configs: - SBLkConfigDefault - ESTkConfigDefault @@ -150,6 +152,36 @@ panda_arm: - SPARStwokConfigDefault - TrajOptDefault panda_arm_hand: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1, panda_joint2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +panda_vision: + enforce_constrained_state_space: true + projection_evaluator: joints(panda_joint1, panda_joint2) planner_configs: - SBLkConfigDefault - ESTkConfigDefault From 4d63379cace394a562f6c26c947cc2dcb94399e8 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:56:40 +0100 Subject: [PATCH 10/14] Franka platform (arm + camera) running --- .../launch/moveit_real_arm_platform.launch.py | 296 ++++++++++++++++++ 1 file changed, 296 insertions(+) create mode 100644 franka_moveit_config/launch/moveit_real_arm_platform.launch.py diff --git a/franka_moveit_config/launch/moveit_real_arm_platform.launch.py b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py new file mode 100644 index 00000000..2caa019c --- /dev/null +++ b/franka_moveit_config/launch/moveit_real_arm_platform.launch.py @@ -0,0 +1,296 @@ +# Copyright (c) 2021 Franka Emika GmbH +# +# 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. + +# This file is an adapted version of +# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + Shutdown) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + load_gripper_parameter_name = 'load_gripper' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + load_camera_parameter_name = 'load_camera' + + camera_id_parameter_name = 'serial' + camera_model_parameter_name = 'camera_type' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + load_gripper = LaunchConfiguration(load_gripper_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + load_camera = LaunchConfiguration(load_camera_parameter_name) + + camera_id = LaunchConfiguration(camera_id_parameter_name) + camera_model = LaunchConfiguration(camera_model_parameter_name) + + # Command-line arguments + db_arg = DeclareLaunchArgument( + 'db', default_value='False', description='Database flag' + ) + + # planning_context + franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', + 'panda_arm_platform.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper, ' camera:=', load_camera, ' camera_model:=', camera_model, + ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, + ' fake_sensor_commands:=', fake_sensor_commands]) + + robot_description = {'robot_description': robot_description_config} + + franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'), + 'srdf', + 'panda_arm_platform.srdf.xacro') + robot_description_semantic_config = Command( + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper, ' camera:=', load_camera] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + 'franka_moveit_config', 'config/kinematics.yaml' + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + 'move_group': { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization ' + 'default_planner_request_adapters/ResolveConstraintFrames ' + 'default_planner_request_adapters/FixWorkspaceBounds ' + 'default_planner_request_adapters/FixStartStateBounds ' + 'default_planner_request_adapters/FixStartStateCollision ' + 'default_planner_request_adapters/FixStartStatePathConstraints', + 'start_state_max_bounds_error': 0.1, + } + } + ompl_planning_yaml = load_yaml( + 'franka_moveit_config', 'config/ompl_planning.yaml' + ) + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + 'franka_moveit_config', 'config/panda_controllers.yaml' + ) + moveit_controllers = { + 'moveit_simple_controller_manager': moveit_simple_controllers_yaml, + 'moveit_controller_manager': 'moveit_simple_controller_manager' + '/MoveItSimpleControllerManager', + } + + trajectory_execution = { + 'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01, + } + + planning_scene_monitor_parameters = { + 'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True, + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {"publish_robot_description_semantic": True}, + ], + ) + + # RViz + rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz') + rviz_full_config = os.path.join(rviz_base, 'moveit.rviz') + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_full_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + # Publish TF + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description], + ) + + ros2_controllers_path = os.path.join( + get_package_share_directory('franka_moveit_config'), + 'config', + 'panda_ros_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, ros2_controllers_path], + remappings=[('joint_states', 'franka/joint_states')], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + on_exit=Shutdown(), + ) + + # Load controllers + load_controllers = [] + for controller in ['panda_arm_controller', 'joint_state_broadcaster']: + load_controllers += [ + ExecuteProcess( + cmd=['ros2 run controller_manager spawner {}'.format(controller)], + shell=True, + output='screen', + ) + ] + + # Warehouse mongodb server + db_config = LaunchConfiguration('db') + mongodb_server_node = Node( + package='warehouse_ros_mongo', + executable='mongo_wrapper_ros.py', + parameters=[ + {'warehouse_port': 33829}, + {'warehouse_host': 'localhost'}, + {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}, + ], + output='screen', + condition=IfCondition(db_config) + ) + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[ + {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}], + ) + robot_arg = DeclareLaunchArgument( + robot_ip_parameter_name, + description='Hostname or IP address of the robot.') + + use_fake_hardware_arg = DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware') + + load_camera_arg = DeclareLaunchArgument( + load_camera_parameter_name, + default_value='true', + description='Use Flir camera as an end-effector, otherwise, the robot is loaded ' + 'without an end-effector.') + + load_gripper_arg = DeclareLaunchArgument( + load_gripper_parameter_name, + default_value='false', + description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded ' + 'without an end-effector.') + + camera_id_arg = DeclareLaunchArgument( + camera_id_parameter_name, + default_value="'22141921'", + description='Camera id, serial number.') + + camera_model_arg = DeclareLaunchArgument( + camera_model_parameter_name, + default_value='blackfly_s', + description='Camera model, example blackfly_s.') + + fake_sensor_commands_arg = DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)) + + gripper_launch_file = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]), + launch_arguments={'robot_ip': robot_ip, + use_fake_hardware_parameter_name: use_fake_hardware}.items(), + condition=IfCondition(load_gripper) + ) + + camera_launch_file = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('spinnaker_camera_driver'), 'launch', 'driver_node.launch.py'])]), + launch_arguments={'camera_type': camera_model, + 'serial': camera_id}.items(), + condition=IfCondition(load_camera) + ) + + return LaunchDescription( + [robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + load_gripper_arg, + load_camera_arg, + camera_id_arg, + camera_model_arg, + db_arg, + rviz_node, + run_move_group_node, + robot_state_publisher, + ros2_control_node, + mongodb_server_node, + joint_state_publisher, + gripper_launch_file, + camera_launch_file, + *load_controllers]) From 24d1b32a9475e1b65eab82adc3e2fc391bd41eef Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:57:53 +0100 Subject: [PATCH 11/14] panda_vision move_group defined in semantic kinematic components --- franka_moveit_config/srdf/camera.xacro | 17 ++++++++++ .../srdf/panda_arm_camera.xacro | 19 +++++++++++ .../srdf/panda_arm_platform.srdf.xacro | 33 +++++++++++++++++++ 3 files changed, 69 insertions(+) create mode 100644 franka_moveit_config/srdf/camera.xacro create mode 100644 franka_moveit_config/srdf/panda_arm_camera.xacro create mode 100644 franka_moveit_config/srdf/panda_arm_platform.srdf.xacro diff --git a/franka_moveit_config/srdf/camera.xacro b/franka_moveit_config/srdf/camera.xacro new file mode 100644 index 00000000..b6419aba --- /dev/null +++ b/franka_moveit_config/srdf/camera.xacro @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/franka_moveit_config/srdf/panda_arm_camera.xacro b/franka_moveit_config/srdf/panda_arm_camera.xacro new file mode 100644 index 00000000..010bed45 --- /dev/null +++ b/franka_moveit_config/srdf/panda_arm_camera.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/franka_moveit_config/srdf/panda_arm_platform.srdf.xacro b/franka_moveit_config/srdf/panda_arm_platform.srdf.xacro new file mode 100644 index 00000000..19140d4d --- /dev/null +++ b/franka_moveit_config/srdf/panda_arm_platform.srdf.xacro @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 4aeb7e3a24fab5d2d1b1b0ad652273ff5ac96d84 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:58:30 +0100 Subject: [PATCH 12/14] Rviz configuration changed --- franka_moveit_config/rviz/moveit.rviz | 166 +++++++++++++++++++++----- 1 file changed, 136 insertions(+), 30 deletions(-) diff --git a/franka_moveit_config/rviz/moveit.rviz b/franka_moveit_config/rviz/moveit.rviz index d301a34f..c10e62cc 100644 --- a/franka_moveit_config/rviz/moveit.rviz +++ b/franka_moveit_config/rviz/moveit.rviz @@ -3,9 +3,14 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /MarkerArray1 + - /PlanningScene1 + - /Pose1 + - /TF1 + - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 814 + Tree Height: 907 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -62,20 +67,20 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - panda_hand: + camera: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_hand_tcp: + camera_lens: Alpha: 1 Show Axes: false Show Trail: false - panda_leftfinger: + Value: true + camera_optical: Alpha: 1 Show Axes: false Show Trail: false - Value: true panda_link0: Alpha: 1 Show Axes: false @@ -121,11 +126,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Loop Animation: false Name: Trajectory Robot Alpha: 0.5 @@ -137,6 +137,7 @@ Visualization Manager: 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 @@ -159,20 +160,20 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - panda_hand: + camera: Alpha: 1 Show Axes: false Show Trail: false Value: true - panda_hand_tcp: + camera_lens: Alpha: 1 Show Axes: false Show Trail: false - panda_leftfinger: + Value: true + camera_optical: Alpha: 1 Show Axes: false Show Trail: false - Value: true panda_link0: Alpha: 1 Show Axes: false @@ -218,11 +219,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true @@ -239,8 +235,6 @@ Visualization Manager: MoveIt_Planning_Time: 5 MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 MoveIt_Workspace: Center: X: 0 @@ -269,6 +263,7 @@ Visualization Manager: State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: /display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false @@ -310,6 +305,114 @@ Visualization Manager: Show Robot Visual: true Value: false Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Axes Length: 0.20000000298023224 + Axes Radius: 0.019999999552965164 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arm_next_node_pose + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lattice_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + camera: + Value: true + camera_lens: + Value: false + camera_optical: + Value: false + panda_link0: + Value: true + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: true + plant: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + camera: + camera_lens: + camera_optical: + {} + plant: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -325,6 +428,9 @@ Visualization Manager: - 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 @@ -353,7 +459,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.119211196899414 + Distance: 2.655773401260376 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -368,22 +474,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5953981876373291 + Pitch: 0.7403981685638428 Target Frame: Value: Orbit (rviz) - Yaw: 5.958578109741211 + Yaw: 5.028563022613525 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1043 + Height: 1136 Hide Left Dock: false Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: @@ -392,6 +498,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 - Y: 454 \ No newline at end of file + Width: 1850 + X: 70 + Y: 27 From 797e332b2d8a9669142e4fe3501803e378a8e368 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 15:59:17 +0100 Subject: [PATCH 13/14] panda_vision move_group defined --- franka_moveit_config/srdf/panda_arm.xacro | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/franka_moveit_config/srdf/panda_arm.xacro b/franka_moveit_config/srdf/panda_arm.xacro index ca95cb1b..b15800b1 100644 --- a/franka_moveit_config/srdf/panda_arm.xacro +++ b/franka_moveit_config/srdf/panda_arm.xacro @@ -1,10 +1,11 @@ - + + From e14a3b98abe3a51de4b51b0d05a00b790a801ca4 Mon Sep 17 00:00:00 2001 From: Abdurrahman Yilmaz Date: Fri, 20 Sep 2024 16:03:18 +0100 Subject: [PATCH 14/14] new launch file covers, so removed --- franka_moveit_config/launch/moveit.launch.py | 261 ------------------- 1 file changed, 261 deletions(-) delete mode 100644 franka_moveit_config/launch/moveit.launch.py diff --git a/franka_moveit_config/launch/moveit.launch.py b/franka_moveit_config/launch/moveit.launch.py deleted file mode 100644 index 4d34057e..00000000 --- a/franka_moveit_config/launch/moveit.launch.py +++ /dev/null @@ -1,261 +0,0 @@ -# Copyright (c) 2021 Franka Emika GmbH -# -# 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. - -# This file is an adapted version of -# https://github.com/ros-planning/moveit_resources/blob/ca3f7930c630581b5504f3b22c40b4f82ee6369d/panda_moveit_config/launch/demo.launch.py - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - Shutdown) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -import yaml - - -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, 'r') as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - -def generate_launch_description(): - robot_ip_parameter_name = 'robot_ip' - use_fake_hardware_parameter_name = 'use_fake_hardware' - load_gripper_parameter_name = 'load_gripper' - fake_sensor_commands_parameter_name = 'fake_sensor_commands' - - robot_ip = LaunchConfiguration(robot_ip_parameter_name) - use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) - load_gripper = LaunchConfiguration(load_gripper_parameter_name) - fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) - - - # Command-line arguments - - db_arg = DeclareLaunchArgument( - 'db', default_value='False', description='Database flag' - ) - - # planning_context - franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', - 'panda_arm.urdf.xacro') - robot_description_config = Command( - [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper, - ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, - ' fake_sensor_commands:=', fake_sensor_commands]) - - robot_description = {'robot_description': robot_description_config} - - franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'), - 'srdf', - 'panda_arm.srdf.xacro') - robot_description_semantic_config = Command( - [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper] - ) - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config - } - - kinematics_yaml = load_yaml( - 'franka_moveit_config', 'config/kinematics.yaml' - ) - - # Planning Functionality - ompl_planning_pipeline_config = { - 'move_group': { - 'planning_plugin': 'ompl_interface/OMPLPlanner', - 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization ' - 'default_planner_request_adapters/ResolveConstraintFrames ' - 'default_planner_request_adapters/FixWorkspaceBounds ' - 'default_planner_request_adapters/FixStartStateBounds ' - 'default_planner_request_adapters/FixStartStateCollision ' - 'default_planner_request_adapters/FixStartStatePathConstraints', - 'start_state_max_bounds_error': 0.1, - } - } - ompl_planning_yaml = load_yaml( - 'franka_moveit_config', 'config/ompl_planning.yaml' - ) - ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - moveit_simple_controllers_yaml = load_yaml( - 'franka_moveit_config', 'config/panda_controllers.yaml' - ) - moveit_controllers = { - 'moveit_simple_controller_manager': moveit_simple_controllers_yaml, - 'moveit_controller_manager': 'moveit_simple_controller_manager' - '/MoveItSimpleControllerManager', - } - - trajectory_execution = { - 'moveit_manage_controllers': True, - 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.01, - } - - planning_scene_monitor_parameters = { - 'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True, - 'publish_robot_description_semantic': True, - } - - # Start the actual move_group node/action server - run_move_group_node = Node( - package='moveit_ros_move_group', - executable='move_group', - output='screen', - parameters=[ - robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - ], - ) - - # RViz - rviz_base = os.path.join(get_package_share_directory('franka_moveit_config'), 'rviz') - rviz_full_config = os.path.join(rviz_base, 'moveit.rviz') - - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments=['-d', rviz_full_config], - parameters=[ - robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml, - ], - ) - - # Publish TF - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[robot_description], - ) - - ros2_controllers_path = os.path.join( - get_package_share_directory('franka_moveit_config'), - 'config', - 'panda_ros_controllers.yaml', - ) - ros2_control_node = Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[robot_description, ros2_controllers_path], - remappings=[('joint_states', 'franka/joint_states')], - output={ - 'stdout': 'screen', - 'stderr': 'screen', - }, - on_exit=Shutdown(), - ) - - # Load controllers - load_controllers = [] - for controller in ['panda_arm_controller', 'joint_state_broadcaster']: - load_controllers += [ - ExecuteProcess( - cmd=['ros2 run controller_manager spawner {}'.format(controller)], - shell=True, - output='screen', - ) - ] - - # Warehouse mongodb server - db_config = LaunchConfiguration('db') - mongodb_server_node = Node( - package='warehouse_ros_mongo', - executable='mongo_wrapper_ros.py', - parameters=[ - {'warehouse_port': 33829}, - {'warehouse_host': 'localhost'}, - {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}, - ], - output='screen', - condition=IfCondition(db_config) - ) - - joint_state_publisher = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[ - {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}], - ) - robot_arg = DeclareLaunchArgument( - robot_ip_parameter_name, - description='Hostname or IP address of the robot.') - - use_fake_hardware_arg = DeclareLaunchArgument( - use_fake_hardware_parameter_name, - default_value='false', - description='Use fake hardware') - load_gripper_arg = DeclareLaunchArgument( - load_gripper_parameter_name, - default_value='false', - description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded ' - 'without an end-effector.') - - fake_sensor_commands_arg = DeclareLaunchArgument( - fake_sensor_commands_parameter_name, - default_value='false', - description="Fake sensor commands. Only valid when '{}' is true".format( - use_fake_hardware_parameter_name)) - gripper_launch_file = IncludeLaunchDescription( - PythonLaunchDescriptionSource([PathJoinSubstitution( - [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]), - launch_arguments={'robot_ip': robot_ip, - use_fake_hardware_parameter_name: use_fake_hardware}.items(), - condition=IfCondition(load_gripper) - ) - return LaunchDescription( - [robot_arg, - use_fake_hardware_arg, - fake_sensor_commands_arg, - load_gripper_arg, - db_arg, - rviz_node, - robot_state_publisher, - run_move_group_node, - ros2_control_node, - mongodb_server_node, - joint_state_publisher, - gripper_launch_file - ] - + load_controllers - )