diff --git a/my_panda_gazebo/launch/simulation.launch.py b/my_panda_gazebo/launch/simulation.launch.py index 3f81155..83dca52 100644 --- a/my_panda_gazebo/launch/simulation.launch.py +++ b/my_panda_gazebo/launch/simulation.launch.py @@ -20,7 +20,14 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - + + load_gripper = LaunchConfiguration("load_gripper") + declare_load_gripper_arg = DeclareLaunchArgument( + "load_gripper", + default_value="False", + description="Use Franka Gripper as end-effector if True", + ) + ## Spawn empty world in ignition-gazebo. gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -33,9 +40,60 @@ def generate_launch_description(): ), launch_arguments={'gz_args': '-r empty.sdf'}.items(), ) - - return LaunchDescription( + + ## Set name of controller configuration file. + controller_config_name = PythonExpression( + [ + "'panda_ros_controllers.yaml'" + ] + ) + + # Load robot controllers file given the configuration file above. + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("my_panda_controller"), + "config", + controller_config_name, + ] + ) + + # Load robot description content that will be used by 'gz_spawn_entity" node below. + robot_description_content = Command( [ - gz_sim + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("my_panda_description"), + "robots", + "panda_arm.urdf.xacro", + ] + ), + "hand:=", + load_gripper, + "use_sim:=True", + "simulation_controllers_config_file:=", + robot_controllers, + ] + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "panda_arm_manipulator", + "-allow_renaming", + "true", + "-string", + robot_description_content + ], + output="screen", + ) + + return LaunchDescription( + [ declare_load_gripper_arg, + gz_sim, ## Spawn empty world in ignition-gazebo. + gz_spawn_entity ## Spawn Franka Emika Robot on the table. ] ) diff --git a/my_panda_moveit_config/CMakeLists.txt b/my_panda_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..fa171e4 --- /dev/null +++ b/my_panda_moveit_config/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(my_panda_moveit_config) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY config launch srdf rviz + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(${PROJECT_NAME}_srdf_tests test/srdf_tests.py) +endif() + +ament_package() diff --git a/my_panda_moveit_config/config/kinematics.yaml b/my_panda_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..d3f2a44 --- /dev/null +++ b/my_panda_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +panda_arm: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 diff --git a/my_panda_moveit_config/config/ompl_planning.yaml b/my_panda_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..b65480e --- /dev/null +++ b/my_panda_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,178 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +panda_arm: + 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_arm_hand: + 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 + diff --git a/my_panda_moveit_config/config/panda_controllers.yaml b/my_panda_moveit_config/config/panda_controllers.yaml new file mode 100644 index 0000000..c245568 --- /dev/null +++ b/my_panda_moveit_config/config/panda_controllers.yaml @@ -0,0 +1,24 @@ +controller_names: + - panda_arm_controller + - panda_gripper + +panda_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + +panda_gripper: + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - panda_finger_joint1 + - panda_finger_joint2 \ No newline at end of file diff --git a/my_panda_moveit_config/config/panda_ros_controllers.yaml b/my_panda_moveit_config/config/panda_ros_controllers.yaml new file mode 100644 index 0000000..5224cd7 --- /dev/null +++ b/my_panda_moveit_config/config/panda_ros_controllers.yaml @@ -0,0 +1,40 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + panda_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + franka_robot_state_broadcaster: + type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster + +franka_robot_state_broadcaster: + ros__parameters: + arm_id: panda + +panda_arm_controller: + ros__parameters: + command_interfaces: + - position + state_interfaces: + - position + - velocity + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + gains: + panda_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. } + panda_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. } + panda_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. } + panda_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. } + panda_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. } + panda_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. } + panda_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. } diff --git a/my_panda_moveit_config/launch/move_group.launch.py b/my_panda_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..5f889cf --- /dev/null +++ b/my_panda_moveit_config/launch/move_group.launch.py @@ -0,0 +1,113 @@ +# Copyright (c) 2023 Franka Robotics 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, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, +) +from launch_ros.actions import Node +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' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_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('my_panda_description'), + 'robots', + 'panda_arm.urdf.xacro', + ) + robot_description_config = Command( + [ + FindExecutable(name='xacro'), + ' ', + franka_xacro_file, + ' hand:=true', + ' 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('my_panda_moveit_config'), + 'srdf', + 'panda_arm.srdf.xacro', + ) + robot_description_semantic_config = Command( + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true'] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config + } + + kinematics_yaml = load_yaml('my_panda_moveit_config', 'config/kinematics.yaml') + # Start the actual move_group node/action server + run_move_group_node = Node( + package='moveit_ros_move_group', + executable='move_group', + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ], + ) + + return LaunchDescription( + [ + db_arg, + run_move_group_node, + ] + ) diff --git a/my_panda_moveit_config/launch/moveit.launch.py b/my_panda_moveit_config/launch/moveit.launch.py new file mode 100644 index 0000000..713c397 --- /dev/null +++ b/my_panda_moveit_config/launch/moveit.launch.py @@ -0,0 +1,244 @@ +# Copyright (c) 2023 Franka Robotics 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 UnlessCondition +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' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_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('my_panda_description'), 'robots', + 'panda_arm.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true', + ' 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('my_panda_moveit_config'), + 'srdf', + 'panda_arm.srdf.xacro') + robot_description_semantic_config = Command( + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=true'] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + 'my_panda_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( + 'my_panda_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( + 'my_panda_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, + ], + ) + + # RViz + rviz_base = os.path.join(get_package_share_directory('my_panda_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('my_panda_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', + ) + ] + + 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}], + ) + + franka_robot_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + arguments=['franka_robot_state_broadcaster'], + output='screen', + condition=UnlessCondition(use_fake_hardware), + ) + + 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') + 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(), + ) + return LaunchDescription( + [robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + db_arg, + rviz_node, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + joint_state_publisher, + franka_robot_state_broadcaster, + gripper_launch_file + ] + + load_controllers + ) diff --git a/my_panda_moveit_config/package.xml b/my_panda_moveit_config/package.xml new file mode 100644 index 0000000..8639f0f --- /dev/null +++ b/my_panda_moveit_config/package.xml @@ -0,0 +1,34 @@ + + + + my_panda_moveit_config + 0.1.13 + Contains Moveit2 configuration files for Franka Robotics research robots + CP + Apache 2.0 + + ament_cmake + + my_panda_description + franka_description + franka_hardware + franka_gripper + joint_state_publisher + robot_state_publisher + joint_state_broadcaster + controller_manager + moveit_ros_move_group + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + rviz2 + xacro + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + + ament_cmake + + diff --git a/my_panda_moveit_config/rviz/moveit.rviz b/my_panda_moveit_config/rviz/moveit.rviz new file mode 100644 index 0000000..5172c5c --- /dev/null +++ b/my_panda_moveit_config/rviz/moveit.rviz @@ -0,0 +1,550 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.5 + Tree Height: 415 + - 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 + Name: Views + Splitter Ratio: 0.5 +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 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 100 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + 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 + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + Value: true + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + 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 + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + 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 + 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 + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + 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.119211196899414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02386285550892353 + Y: 0.15478567779064178 + Z: 0.039489321410655975 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5953981876373291 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.958578109741211 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/my_panda_moveit_config/srdf/group_definition.xacro b/my_panda_moveit_config/srdf/group_definition.xacro new file mode 100644 index 0000000..8894c42 --- /dev/null +++ b/my_panda_moveit_config/srdf/group_definition.xacro @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/my_panda_moveit_config/srdf/hand.xacro b/my_panda_moveit_config/srdf/hand.xacro new file mode 100644 index 0000000..5647a95 --- /dev/null +++ b/my_panda_moveit_config/srdf/hand.xacro @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/my_panda_moveit_config/srdf/panda_arm.srdf.xacro b/my_panda_moveit_config/srdf/panda_arm.srdf.xacro new file mode 100644 index 0000000..4e53295 --- /dev/null +++ b/my_panda_moveit_config/srdf/panda_arm.srdf.xacro @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/my_panda_moveit_config/srdf/panda_arm.xacro b/my_panda_moveit_config/srdf/panda_arm.xacro new file mode 100644 index 0000000..72dee7b --- /dev/null +++ b/my_panda_moveit_config/srdf/panda_arm.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/my_panda_moveit_config/srdf/panda_arm_hand.xacro b/my_panda_moveit_config/srdf/panda_arm_hand.xacro new file mode 100644 index 0000000..875b593 --- /dev/null +++ b/my_panda_moveit_config/srdf/panda_arm_hand.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/my_panda_moveit_config/test/srdf_tests.py b/my_panda_moveit_config/test/srdf_tests.py new file mode 100644 index 0000000..6bcb9a4 --- /dev/null +++ b/my_panda_moveit_config/test/srdf_tests.py @@ -0,0 +1,42 @@ +# Copyright (c) 2023 Franka Robotics 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. + +from os import path + +from ament_index_python.packages import get_package_share_directory +import xacro + +panda_xacro_file_name = path.join(get_package_share_directory('franka_moveit_config'), 'srdf', + 'panda_arm.srdf.xacro') + + +def test_load(): + urdf = xacro.process_file(panda_xacro_file_name).toxml() + assert urdf.find('panda_rightfinger') != -1 + + +def test_load_without_gripper(): + urdf = xacro.process_file(panda_xacro_file_name, + mappings={'hand': 'false'}).toxml() + assert urdf.find('panda_rightfinger') == -1 + + +def test_load_with_arm_id(): + urdf = xacro.process_file(panda_xacro_file_name, + mappings={'arm_id': 'totally_different_arm'}).toxml() + assert urdf.find('totally_different_arm_joint1') != -1 + + +if __name__ == '__main__': + pass