├── .gitignore ├── panda_description ├── panda │ ├── thumbnails │ │ ├── 1.png │ │ └── 2.png │ ├── meshes │ │ └── collision │ │ │ ├── hand.stl │ │ │ ├── finger.stl │ │ │ ├── link0.stl │ │ │ ├── link1.stl │ │ │ ├── link2.stl │ │ │ ├── link3.stl │ │ │ ├── link4.stl │ │ │ ├── link5.stl │ │ │ ├── link6.stl │ │ │ └── link7.stl │ └── model.config ├── cmake │ └── environment_hooks │ │ ├── sdf_path.bash │ │ └── ign_gazebo_resource_path.bash ├── config │ └── initial_joint_positions.yaml ├── urdf │ ├── panda.gazebo │ ├── panda.urdf.xacro │ ├── panda_inertial.xacro │ ├── panda_gripper.xacro │ ├── panda_utils.xacro │ ├── panda.ros2_control │ ├── panda_arm.xacro │ └── panda.urdf ├── CMakeLists.txt ├── package.xml ├── scripts │ ├── xacro2urdf.bash │ ├── xacro2sdf.bash │ └── estimate_inertial_properties.py ├── README.md ├── launch │ ├── view_ign.launch.py │ └── view.launch.py └── rviz │ └── view.rviz ├── .git_hooks ├── setup.bash └── pre-commit_convert_xacros.bash ├── panda_moveit_config ├── CMakeLists.txt ├── config │ ├── kinematics.yaml │ ├── controllers_position.yaml │ ├── controllers_position,velocity.yaml │ ├── moveit_controller_manager.yaml │ ├── controllers_effort.yaml │ ├── joint_limits.yaml │ ├── controllers_velocity.yaml │ ├── servo.yaml │ └── ompl_planning.yaml ├── scripts │ └── xacro2srdf.bash ├── srdf │ ├── panda_gripper.xacro │ ├── panda.srdf.xacro │ ├── panda_arm.xacro │ └── panda.srdf ├── package.xml ├── launch │ ├── ex_fake_control.launch.py │ ├── ex_ign_control.launch.py │ └── move_group.launch.py ├── README.md └── rviz │ └── moveit.rviz ├── .dockerignore ├── panda_ign_moveit2.repos ├── panda ├── README.md ├── package.xml └── CMakeLists.txt ├── .docker ├── devel.bash ├── build.bash └── run.bash ├── LICENSE ├── Dockerfile ├── .pre-commit-config.yaml └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Colcon 2 | **/build 3 | **/install 4 | **/log 5 | 6 | # Python 7 | **/__pycache__ 8 | -------------------------------------------------------------------------------- /panda_description/panda/thumbnails/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/thumbnails/1.png -------------------------------------------------------------------------------- /panda_description/panda/thumbnails/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/thumbnails/2.png -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/hand.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/finger.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link0.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link1.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link2.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link3.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link4.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link5.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link6.stl -------------------------------------------------------------------------------- /panda_description/panda/meshes/collision/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AndrejOrsula/panda_gz_moveit2/HEAD/panda_description/panda/meshes/collision/link7.stl -------------------------------------------------------------------------------- /panda_description/cmake/environment_hooks/sdf_path.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | PKG_SHARE_DIR="$(cd "$(dirname "$(dirname "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 4 | ament_prepend_unique_value SDF_PATH "${PKG_SHARE_DIR}" 5 | -------------------------------------------------------------------------------- /panda_description/cmake/environment_hooks/ign_gazebo_resource_path.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | PKG_SHARE_DIR="$(cd "$(dirname "$(dirname "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 4 | ament_prepend_unique_value IGN_GAZEBO_RESOURCE_PATH "${PKG_SHARE_DIR}" 5 | -------------------------------------------------------------------------------- /panda_description/panda/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda 4 | 1.0.0 5 | model.sdf 6 | 7 | 8 | Franka Emika Panda robot compatible with Ignition and MoveIt2 9 | 10 | 11 | -------------------------------------------------------------------------------- /.git_hooks/setup.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # This script setups git hooks for this repository. 3 | 4 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 5 | REPO_DIR="$(dirname "${SCRIPT_DIR}")" 6 | 7 | pip install --user pre-commit && 8 | cd "${REPO_DIR}" && 9 | pre-commit install && 10 | cd - || exit 11 | -------------------------------------------------------------------------------- /panda_description/config/initial_joint_positions.yaml: -------------------------------------------------------------------------------- 1 | initial_joint_positions: 2 | panda_arm: 3 | joint1: 0.0 4 | joint2: -0.7853981633974483 5 | joint3: 0.0 6 | joint4: -2.356194490192345 7 | joint5: 0.0 8 | joint6: 1.5707963267948966 9 | joint7: 0.7853981633974483 10 | panda_gripper: 11 | joint1: 0.0 12 | joint2: 0.0 13 | -------------------------------------------------------------------------------- /panda_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(panda_moveit_config) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(panda_description REQUIRED) 7 | 8 | # Install directories 9 | install(DIRECTORY config launch rviz srdf DESTINATION share/${PROJECT_NAME}) 10 | 11 | # Setup the project 12 | ament_package() 13 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | # Docker 2 | **/Dockerfile 3 | **/.dockerignore 4 | **/.docker/host 5 | **/.docker/*.bash 6 | 7 | # Git 8 | **/.git 9 | **/.gitignore 10 | 11 | # Pre-commit 12 | **/.git_hooks 13 | **/.pre-commit-config.yaml 14 | 15 | # Colcon 16 | **/build 17 | **/install 18 | **/log 19 | 20 | # Python 21 | **/__pycache__ 22 | 23 | # Markdown 24 | **/README.md 25 | **/CHANGELOG.md 26 | -------------------------------------------------------------------------------- /panda_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | # kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin 4 | # kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin 5 | kinematics_solver_search_resolution: 0.0025 6 | kinematics_solver_timeout: 0.05 7 | kinematics_solver_attempts: 5 8 | # max_cache_size: 100000 9 | -------------------------------------------------------------------------------- /panda_ign_moveit2.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros_ign: 3 | type: git 4 | url: https://github.com/ignitionrobotics/ros_ign.git 5 | version: galactic 6 | ign_ros2_control: 7 | type: git 8 | url: https://github.com/AndrejOrsula/ign_ros2_control.git 9 | version: devel 10 | ros2_controllers: 11 | type: git 12 | url: https://github.com/AndrejOrsula/ros2_controllers.git 13 | version: jtc_effort 14 | -------------------------------------------------------------------------------- /.git_hooks/pre-commit_convert_xacros.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # Pre-commit hook that convert xacros to URDF/SDF/SRDF. It makes sure that edits in xacros propagate to these files. 3 | 4 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 5 | REPO_DIR="$(dirname "${SCRIPT_DIR}")" 6 | 7 | # Convert xacros to URDF, SDF and SRDF 8 | "${REPO_DIR}/panda_description/scripts/xacro2urdf.bash" 9 | "${REPO_DIR}/panda_description/scripts/xacro2sdf.bash" 10 | "${REPO_DIR}/panda_moveit_config/scripts/xacro2srdf.bash" 11 | -------------------------------------------------------------------------------- /panda/README.md: -------------------------------------------------------------------------------- 1 | # panda 2 | 3 | Metapackage for Franka Emika Panda. 4 | 5 | ## Functionality 6 | 7 | During the build stage, this package converts xacros of [panda_description](../panda_description) and [panda_moveit_config](../panda_moveit_config) into auto-generated URDF, SDF and SRDF descriptions for convenience. 8 | 9 | ## Directory Structure 10 | 11 | The following directory structure is utilised for this package. 12 | 13 | ```bash 14 | . 15 | ├── CMakeLists.txt # Colcon-enabled CMake recipe 16 | └── package.xml # ROS 2 package metadata 17 | ``` 18 | -------------------------------------------------------------------------------- /.docker/devel.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 4 | PROJECT_DIR="$(dirname "${SCRIPT_DIR}")" 5 | 6 | DOCKER_HOME="/root" 7 | DOCKER_WS_DIR="${DOCKER_HOME}/ws" 8 | DOCKER_WS_SRC_DIR="${DOCKER_WS_DIR}/src" 9 | DOCKER_TARGET_SRC_DIR="${DOCKER_WS_SRC_DIR}/$(basename "${PROJECT_DIR}")" 10 | 11 | echo -e "\033[2;37mDevelopment volume: ${PROJECT_DIR} -> ${DOCKER_TARGET_SRC_DIR}\033[0m" | xargs 12 | 13 | exec "${SCRIPT_DIR}/run.bash" -v "${PROJECT_DIR}:${DOCKER_TARGET_SRC_DIR}" "${@}" 14 | -------------------------------------------------------------------------------- /panda_description/urdf/panda.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | ${controller_parameters} 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /panda_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(panda_description) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | # Setup environment hooks that automatically prepend IGN_GAZEBO_RESOURCE_PATH and SDF_PATH when sourcing local_setup. 8 | ament_environment_hooks( 9 | "cmake/environment_hooks/ign_gazebo_resource_path.bash" 10 | "cmake/environment_hooks/sdf_path.bash" 11 | ) 12 | 13 | # Install directories 14 | install(DIRECTORY config launch panda panda/meshes rviz urdf DESTINATION share/${PROJECT_NAME}) 15 | 16 | # Setup the project 17 | ament_package() 18 | -------------------------------------------------------------------------------- /panda/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda 4 | 1.0.0 5 | Metapackage for Franka Emika Panda 6 | Andrej Orsula 7 | Andrej Orsula 8 | BSD 9 | 10 | ament_cmake 11 | 12 | panda_description 13 | panda_moveit_config 14 | xacro 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /panda_moveit_config/scripts/xacro2srdf.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # This script converts xacro (SRDF variant) into SRDF for `panda_description` package 3 | 4 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 5 | XACRO_PATH="$(dirname "${SCRIPT_DIR}")/srdf/panda.srdf.xacro" 6 | SRDF_PATH="$(dirname "${SCRIPT_DIR}")/srdf/panda.srdf" 7 | 8 | # Arguments for xacro 9 | XACRO_ARGS=( 10 | name:=panda 11 | ) 12 | 13 | # Remove old SRDF file 14 | rm "${SRDF_PATH}" 2>/dev/null 15 | 16 | # Process xacro into SRDF 17 | xacro "${XACRO_PATH}" "${XACRO_ARGS[@]}" -o "${SRDF_PATH}" && 18 | echo "Created new ${SRDF_PATH}" 19 | -------------------------------------------------------------------------------- /.docker/build.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 4 | PROJECT_DIR="$(dirname "${SCRIPT_DIR}")" 5 | 6 | TAG="andrejorsula/$(basename "${PROJECT_DIR}")" 7 | 8 | if [ "${#}" -gt "0" ]; then 9 | if [[ "${1}" != "-"* ]]; then 10 | TAG="${TAG}:${1}" 11 | BUILD_ARGS=${*:2} 12 | else 13 | BUILD_ARGS=${*:1} 14 | fi 15 | fi 16 | 17 | DOCKER_BUILD_CMD=( 18 | docker build 19 | "${PROJECT_DIR}" 20 | --tag "${TAG}" 21 | "${BUILD_ARGS}" 22 | ) 23 | 24 | echo -e "\033[1;30m${DOCKER_BUILD_CMD[*]}\033[0m" | xargs 25 | 26 | # shellcheck disable=SC2048 27 | exec ${DOCKER_BUILD_CMD[*]} 28 | -------------------------------------------------------------------------------- /panda_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_description 4 | 3.0.0 5 | URDF and SDF description of Franka Emika Panda 6 | Andrej Orsula 7 | Andrej Orsula 8 | BSD 9 | 10 | ament_cmake 11 | 12 | joint_state_publisher_gui 13 | robot_state_publisher 14 | ros_ign_gazebo 15 | rviz2 16 | urdf 17 | xacro 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /panda_description/scripts/xacro2urdf.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # This script converts xacro (URDF variant) into URDF for `panda_description` package 3 | 4 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 5 | XACRO_PATH="$(dirname "${SCRIPT_DIR}")/urdf/panda.urdf.xacro" 6 | URDF_PATH="$(dirname "${SCRIPT_DIR}")/urdf/panda.urdf" 7 | 8 | # Arguments for xacro 9 | XACRO_ARGS=( 10 | name:=panda 11 | gripper:=true 12 | collision_arm:=true 13 | collision_gripper:=true 14 | ros2_control:=true 15 | ros2_control_plugin:=ign 16 | ros2_control_command_interface:=effort 17 | gazebo_preserve_fixed_joint:=false 18 | ) 19 | 20 | # Remove old URDF file 21 | rm "${URDF_PATH}" 2>/dev/null 22 | 23 | # Process xacro into URDF 24 | xacro "${XACRO_PATH}" "${XACRO_ARGS[@]}" -o "${URDF_PATH}" && 25 | echo "Created new ${URDF_PATH}" 26 | -------------------------------------------------------------------------------- /panda/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(panda) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(${PROJECT_NAME}_description REQUIRED) 7 | find_package(${PROJECT_NAME}_moveit_config REQUIRED) 8 | 9 | # Convert xacros of description and moveit_config packages to URDF, SDF and SRDF 10 | get_filename_component(${PROJECT_NAME}_REPO_DIR ${CMAKE_CURRENT_SOURCE_DIR} DIRECTORY) 11 | set(${PROJECT_NAME}_DESCRIPTION_SCRIPTS_DIR ${${PROJECT_NAME}_REPO_DIR}/${PROJECT_NAME}_description/scripts) 12 | set(${PROJECT_NAME}_MOVEIT_CONFIG_SCRIPTS_DIR ${${PROJECT_NAME}_REPO_DIR}/${PROJECT_NAME}_moveit_config/scripts) 13 | execute_process( 14 | COMMAND ${${PROJECT_NAME}_DESCRIPTION_SCRIPTS_DIR}/xacro2urdf.bash 15 | ) 16 | execute_process( 17 | COMMAND ${${PROJECT_NAME}_DESCRIPTION_SCRIPTS_DIR}/xacro2sdf.bash 18 | ) 19 | execute_process( 20 | COMMAND ${${PROJECT_NAME}_MOVEIT_CONFIG_SCRIPTS_DIR}/xacro2srdf.bash 21 | ) 22 | 23 | # Setup the project 24 | ament_package() 25 | -------------------------------------------------------------------------------- /panda_moveit_config/config/controllers_position.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 250 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | gripper_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | joint_trajectory_controller: 15 | ros__parameters: 16 | joints: 17 | - panda_joint1 18 | - panda_joint2 19 | - panda_joint3 20 | - panda_joint4 21 | - panda_joint5 22 | - panda_joint6 23 | - panda_joint7 24 | command_interfaces: 25 | - position 26 | state_interfaces: 27 | - position 28 | - velocity 29 | 30 | gripper_trajectory_controller: 31 | ros__parameters: 32 | joints: 33 | - panda_finger_joint1 34 | - panda_finger_joint2 35 | command_interfaces: 36 | - position 37 | state_interfaces: 38 | - position 39 | - velocity 40 | -------------------------------------------------------------------------------- /panda_moveit_config/config/controllers_position,velocity.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 250 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | gripper_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | joint_trajectory_controller: 15 | ros__parameters: 16 | joints: 17 | - panda_joint1 18 | - panda_joint2 19 | - panda_joint3 20 | - panda_joint4 21 | - panda_joint5 22 | - panda_joint6 23 | - panda_joint7 24 | command_interfaces: 25 | - position 26 | - velocity 27 | state_interfaces: 28 | - position 29 | - velocity 30 | 31 | gripper_trajectory_controller: 32 | ros__parameters: 33 | joints: 34 | - panda_finger_joint1 35 | - panda_finger_joint2 36 | command_interfaces: 37 | - position 38 | - velocity 39 | state_interfaces: 40 | - position 41 | - velocity 42 | -------------------------------------------------------------------------------- /panda_description/scripts/xacro2sdf.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # This script converts xacro (URDF variant) into SDF for `panda_description` package 3 | 4 | SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)" 5 | XACRO_PATH="$(dirname "${SCRIPT_DIR}")/urdf/panda.urdf.xacro" 6 | SDF_PATH="$(dirname "${SCRIPT_DIR}")/panda/model.sdf" 7 | TMP_URDF_PATH="/tmp/panda_tmp.urdf" 8 | 9 | # Arguments for xacro 10 | XACRO_ARGS=( 11 | name:=panda 12 | gripper:=true 13 | collision_arm:=true 14 | collision_gripper:=true 15 | ros2_control:=true 16 | ros2_control_plugin:=ign 17 | ros2_control_command_interface:=effort 18 | gazebo_preserve_fixed_joint:=false 19 | ) 20 | 21 | # Remove old SDF file 22 | rm "${SDF_PATH}" 2>/dev/null 23 | 24 | # Process xacro into URDF, then convert URDF to SDF and edit the SDF to use relative paths for meshes 25 | xacro "${XACRO_PATH}" "${XACRO_ARGS[@]}" -o "${TMP_URDF_PATH}" && 26 | ign sdf -p "${TMP_URDF_PATH}" | sed "s/model:\/\/panda_description\///g" >"${SDF_PATH}" && 27 | echo "Created new ${SDF_PATH}" 28 | 29 | # Remove temporary URDF file 30 | rm "${TMP_URDF_PATH}" 2>/dev/null 31 | -------------------------------------------------------------------------------- /panda_moveit_config/srdf/panda_gripper.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /panda_moveit_config/config/moveit_controller_manager.yaml: -------------------------------------------------------------------------------- 1 | controller_names: 2 | - joint_trajectory_controller 3 | - gripper_trajectory_controller 4 | # - gripper_action_controller 5 | 6 | joint_trajectory_controller: 7 | action_ns: follow_joint_trajectory 8 | type: FollowJointTrajectory 9 | default: true 10 | joints: 11 | - panda_joint1 12 | - panda_joint2 13 | - panda_joint3 14 | - panda_joint4 15 | - panda_joint5 16 | - panda_joint6 17 | - panda_joint7 18 | 19 | gripper_trajectory_controller: 20 | action_ns: follow_joint_trajectory 21 | type: FollowJointTrajectory 22 | default: true 23 | joints: 24 | - panda_finger_joint1 25 | - panda_finger_joint2 26 | 27 | # ----- Gripper action controller (not yet functional) 28 | # gripper_action_controller: 29 | # action_ns: gripper_cmd 30 | # type: GripperCommand 31 | # default: true 32 | # joints: 33 | # - panda_finger_joint1 34 | # 35 | # Note: Once implemented, the following config needs to be within controller parameters 36 | # controller_manager: 37 | # ros__parameters: 38 | # gripper_action_controller: 39 | # type: position_controllers/GripperActionController 40 | # gripper_action_controller: 41 | # ros__parameters: 42 | # joint: panda_finger_joint1 43 | # 44 | # Node: Remember to enable mimicking of finger joints before attempting to use gripper action 45 | -------------------------------------------------------------------------------- /panda_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_moveit_config 4 | 3.0.0 5 | MoveIt configuration for Franka Emika Panda 6 | Andrej Orsula 7 | Andrej Orsula 8 | BSD 9 | 10 | ament_cmake 11 | 12 | panda_description 13 | 14 | 15 | backward_ros 16 | control_msgs 17 | controller_manager 18 | hardware_interface 19 | ign_ros2_control 20 | joint_state_broadcaster 21 | joint_trajectory_controller 22 | moveit_ros_move_group 23 | moveit_servo 24 | moveit 25 | robot_state_publisher 26 | ros_ign_bridge 27 | ros_ign_gazebo 28 | rviz2 29 | urdf 30 | xacro 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Andrej Orsula 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=galactic 2 | FROM ros:${ROS_DISTRO}-ros-base 3 | 4 | ### Use bash by default 5 | SHELL ["/bin/bash", "-c"] 6 | 7 | ### Define working directory 8 | ARG WS_DIR=/root/ws 9 | ENV WS_DIR=${WS_DIR} 10 | ENV WS_SRC_DIR=${WS_DIR}/src 11 | ENV WS_INSTALL_DIR=${WS_DIR}/install 12 | ENV WS_LOG_DIR=${WS_DIR}/log 13 | WORKDIR ${WS_DIR} 14 | 15 | ### Install Gazebo 16 | ARG IGNITION_VERSION=fortress 17 | ENV IGNITION_VERSION=${IGNITION_VERSION} 18 | RUN apt-get update && \ 19 | apt-get install -yq --no-install-recommends \ 20 | ignition-${IGNITION_VERSION} && \ 21 | rm -rf /var/lib/apt/lists/* 22 | 23 | ### Import and install dependencies, then build these dependencies (not panda_ign_moveit2 yet) 24 | COPY ./panda_ign_moveit2.repos ${WS_SRC_DIR}/panda_ign_moveit2/panda_ign_moveit2.repos 25 | RUN vcs import --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/panda_ign_moveit2/panda_ign_moveit2.repos && \ 26 | rosdep update && \ 27 | apt-get update && \ 28 | rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \ 29 | rm -rf /var/lib/apt/lists/* && \ 30 | source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ 31 | colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \ 32 | rm -rf ${WS_LOG_DIR} 33 | 34 | ### Copy over the rest of panda_ign_moveit2, then install dependencies and build 35 | COPY ./ ${WS_SRC_DIR}/panda_ign_moveit2/ 36 | RUN rosdep update && \ 37 | apt-get update && \ 38 | rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \ 39 | rm -rf /var/lib/apt/lists/* && \ 40 | source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ 41 | colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \ 42 | rm -rf ${WS_LOG_DIR} 43 | 44 | ### Add workspace to the ROS entrypoint 45 | ### Source ROS workspace inside `~/.bashrc` to enable autocompletion 46 | RUN sed -i '$i source "${WS_INSTALL_DIR}/local_setup.bash" --' /ros_entrypoint.sh && \ 47 | sed -i '$a source "/opt/ros/${ROS_DISTRO}/setup.bash"' ~/.bashrc 48 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # See https://pre-commit.com for more information 2 | # See https://pre-commit.com/hooks.html for more hooks 3 | 4 | ci: 5 | skip: [convert_xacros, hadolint-docker] 6 | 7 | exclude: \.(dae|stl|png)$ 8 | repos: 9 | - repo: https://github.com/pre-commit/pre-commit-hooks 10 | rev: v4.4.0 11 | hooks: 12 | - id: check-added-large-files 13 | args: ["--maxkb=2000"] 14 | - id: check-case-conflict 15 | - id: check-executables-have-shebangs 16 | - id: check-merge-conflict 17 | - id: check-shebang-scripts-are-executable 18 | - id: check-symlinks 19 | - id: check-xml 20 | - id: check-yaml 21 | - id: debug-statements 22 | - id: destroyed-symlinks 23 | - id: detect-private-key 24 | - id: end-of-file-fixer 25 | - id: mixed-line-ending 26 | - id: requirements-txt-fixer 27 | - id: trailing-whitespace 28 | 29 | - repo: https://github.com/pycqa/isort 30 | rev: 5.12.0 31 | hooks: 32 | - id: isort 33 | args: ["--profile", "black"] 34 | 35 | - repo: https://github.com/psf/black 36 | rev: 23.1.0 37 | hooks: 38 | - id: black 39 | 40 | - repo: https://github.com/lovesegfault/beautysh 41 | rev: v6.2.1 42 | hooks: 43 | - id: beautysh 44 | 45 | - repo: https://github.com/executablebooks/mdformat 46 | rev: 0.7.16 47 | hooks: 48 | - id: mdformat 49 | 50 | - repo: https://github.com/codespell-project/codespell 51 | rev: v2.2.4 52 | hooks: 53 | - id: codespell 54 | 55 | - repo: https://github.com/hadolint/hadolint 56 | rev: v2.12.1-beta 57 | hooks: 58 | - id: hadolint-docker 59 | args: [--ignore, DL3008] 60 | 61 | - repo: local 62 | hooks: 63 | - id: convert_xacros 64 | name: convert_xacros 65 | description: Pre-commit hook that convert xacros to URDF/SDF/SRDF. It makes sure that edits in xacros propagate to these files. 66 | entry: ./.git_hooks/pre-commit_convert_xacros.bash 67 | language: system 68 | files: \.(xacro)$|xacro2[a-zA-Z]+.bash$ 69 | pass_filenames: false 70 | -------------------------------------------------------------------------------- /panda_moveit_config/config/controllers_effort.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 250 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | gripper_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | joint_trajectory_controller: 15 | ros__parameters: 16 | joints: 17 | - panda_joint1 18 | - panda_joint2 19 | - panda_joint3 20 | - panda_joint4 21 | - panda_joint5 22 | - panda_joint6 23 | - panda_joint7 24 | command_interfaces: 25 | - effort 26 | state_interfaces: 27 | - position 28 | - velocity 29 | gains: 30 | panda_joint1: 31 | p: 4000.0 32 | d: 10.0 33 | i: 250.0 34 | i_clamp: 15.0 35 | panda_joint2: 36 | p: 10000.0 37 | d: 25.0 38 | i: 600.0 39 | i_clamp: 45.0 40 | panda_joint3: 41 | p: 8000.0 42 | d: 20.0 43 | i: 450.0 44 | i_clamp: 30.0 45 | panda_joint4: 46 | p: 6000.0 47 | d: 15.0 48 | i: 300.0 49 | i_clamp: 30.0 50 | panda_joint5: 51 | p: 3000.0 52 | d: 5.0 53 | i: 175.0 54 | i_clamp: 7.0 55 | panda_joint6: 56 | p: 2500.0 57 | d: 3.0 58 | i: 150.0 59 | i_clamp: 6.0 60 | panda_joint7: 61 | p: 2000.0 62 | d: 2.0 63 | i: 10.0 64 | i_clamp: 5.0 65 | 66 | gripper_trajectory_controller: 67 | ros__parameters: 68 | joints: 69 | - panda_finger_joint1 70 | - panda_finger_joint2 71 | command_interfaces: 72 | - effort 73 | state_interfaces: 74 | - position 75 | - velocity 76 | gains: 77 | panda_finger_joint1: 78 | p: 225.0 79 | d: 0.001 80 | i: 0.4 81 | i_clamp: 4.0 82 | panda_finger_joint2: 83 | p: 225.0 84 | d: 0.001 85 | i: 0.4 86 | i_clamp: 4.0 87 | -------------------------------------------------------------------------------- /panda_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | 5 | # As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee 6 | # that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel 7 | # to max accel in 1 ms) the acceleration limits are the ones that satisfy 8 | # max_jerk = (max_acceleration - min_acceleration) / 0.001 9 | 10 | joint_limits: 11 | panda_joint1: 12 | has_velocity_limits: true 13 | max_velocity: 2.175 14 | has_acceleration_limits: true 15 | max_acceleration: 3.75 16 | panda_joint2: 17 | has_velocity_limits: true 18 | max_velocity: 2.175 19 | has_acceleration_limits: true 20 | max_acceleration: 1.875 21 | panda_joint3: 22 | has_velocity_limits: true 23 | max_velocity: 2.175 24 | has_acceleration_limits: true 25 | max_acceleration: 2.5 26 | panda_joint4: 27 | has_velocity_limits: true 28 | max_velocity: 2.175 29 | has_acceleration_limits: true 30 | max_acceleration: 3.125 31 | panda_joint5: 32 | has_velocity_limits: true 33 | max_velocity: 2.61 34 | has_acceleration_limits: true 35 | max_acceleration: 3.75 36 | panda_joint6: 37 | has_velocity_limits: true 38 | max_velocity: 2.61 39 | has_acceleration_limits: true 40 | max_acceleration: 5.0 41 | panda_joint7: 42 | has_velocity_limits: true 43 | max_velocity: 2.61 44 | has_acceleration_limits: true 45 | max_acceleration: 5.0 46 | panda_finger_joint1: 47 | has_velocity_limits: true 48 | max_velocity: 0.1 49 | has_acceleration_limits: false 50 | max_acceleration: 0.0 51 | panda_finger_joint2: 52 | has_velocity_limits: true 53 | max_velocity: 0.1 54 | has_acceleration_limits: false 55 | max_acceleration: 0.0 56 | -------------------------------------------------------------------------------- /panda_moveit_config/config/controllers_velocity.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 250 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | gripper_trajectory_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | joint_trajectory_controller: 15 | ros__parameters: 16 | joints: 17 | - panda_joint1 18 | - panda_joint2 19 | - panda_joint3 20 | - panda_joint4 21 | - panda_joint5 22 | - panda_joint6 23 | - panda_joint7 24 | command_interfaces: 25 | - velocity 26 | state_interfaces: 27 | - position 28 | - velocity 29 | gains: 30 | panda_joint1: 31 | p: 4000.0 32 | d: 10.0 33 | i: 250.0 34 | i_clamp: 15.0 35 | panda_joint2: 36 | p: 10000.0 37 | d: 25.0 38 | i: 600.0 39 | i_clamp: 45.0 40 | panda_joint3: 41 | p: 8000.0 42 | d: 20.0 43 | i: 450.0 44 | i_clamp: 30.0 45 | panda_joint4: 46 | p: 6000.0 47 | d: 15.0 48 | i: 300.0 49 | i_clamp: 30.0 50 | panda_joint5: 51 | p: 3000.0 52 | d: 5.0 53 | i: 175.0 54 | i_clamp: 7.0 55 | panda_joint6: 56 | p: 2500.0 57 | d: 3.0 58 | i: 150.0 59 | i_clamp: 6.0 60 | panda_joint7: 61 | p: 2000.0 62 | d: 2.0 63 | i: 10.0 64 | i_clamp: 5.0 65 | 66 | gripper_trajectory_controller: 67 | ros__parameters: 68 | joints: 69 | - panda_finger_joint1 70 | - panda_finger_joint2 71 | command_interfaces: 72 | - velocity 73 | state_interfaces: 74 | - position 75 | - velocity 76 | gains: 77 | panda_finger_joint1: 78 | p: 225.0 79 | d: 0.001 80 | i: 0.4 81 | i_clamp: 4.0 82 | panda_finger_joint2: 83 | p: 225.0 84 | d: 0.001 85 | i: 0.4 86 | i_clamp: 4.0 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # panda_ign_moveit2 2 | 3 | Software packages for Franka Emika Panda that enable manipulation with MoveIt 2 inside ~~Ignition~~ Gazebo. For control, [gz_ros2_control](https://github.com/ros-controls/gz_ros2_control) is used. 4 | 5 |

6 | Animation of ex_follow_target 7 |

8 | 9 | ## Overview 10 | 11 | This branch targets ROS 2 `galactic` and Gazebo `fortress`. 12 | 13 | Below is an overview of the included packages, with a short description of their purpose. For more information, please see README.md of each individual package. 14 | 15 | - [**panda**](./panda) – Metapackage 16 | - [**panda_description**](./panda_description) – URDF and SDF description of the robot 17 | - [**panda_moveit_config**](./panda_moveit_config) – MoveIt 2 configuration for the robot 18 | 19 | ## Instructions 20 | 21 | ### Dependencies 22 | 23 | These are the primary dependencies required to use this project. 24 | 25 | - ROS 2 [Galactic](https://docs.ros.org/en/galactic/Installation.html) 26 | - Gazebo [Fortress](https://gazebosim.org/docs/fortress) 27 | 28 | All additional dependencies are either pulled via [vcstool](https://wiki.ros.org/vcstool) ([panda_ign_moveit2.repos](./panda_ign_moveit2.repos)) or installed via [rosdep](https://wiki.ros.org/rosdep) during the building process below. 29 | 30 | ### Building 31 | 32 | Clone this repository, import dependencies, install dependencies and build with [colcon](https://colcon.readthedocs.io). 33 | 34 | ```bash 35 | # Clone this repository into your favourite ROS 2 workspace 36 | git clone https://github.com/AndrejOrsula/panda_ign_moveit2.git 37 | # Import dependencies 38 | vcs import < panda_ign_moveit2/panda_ign_moveit2.repos 39 | # Install dependencies 40 | IGNITION_VERSION=fortress rosdep install -y -r -i --rosdistro ${ROS_DISTRO} --from-paths . 41 | # Build 42 | colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" 43 | ``` 44 | 45 | ### Sourcing 46 | 47 | Before utilising this package, remember to source the ROS 2 workspace. 48 | 49 | ```bash 50 | source install/local_setup.bash 51 | ``` 52 | 53 | This enables: 54 | 55 | - Execution of binaries, scripts and examples via `ros2 run panda_* ` 56 | - Launching of setup scripts via `ros2 launch panda_* ` 57 | - Discoverability of shared resources 58 | -------------------------------------------------------------------------------- /panda_moveit_config/srdf/panda.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 35 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /panda_moveit_config/launch/ex_fake_control.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S ros2 launch 2 | """Example of planning with MoveIt2 and executing motions using fake ROS 2 controllers within RViz2""" 3 | 4 | from os import path 5 | from typing import List 6 | 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch import LaunchDescription 9 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 12 | from launch_ros.substitutions import FindPackageShare 13 | 14 | 15 | def generate_launch_description() -> LaunchDescription: 16 | # Declare all launch arguments 17 | declared_arguments = generate_declared_arguments() 18 | 19 | # Get substitution for all arguments 20 | rviz_config = LaunchConfiguration("rviz_config") 21 | use_sim_time = LaunchConfiguration("use_sim_time") 22 | log_level = LaunchConfiguration("log_level") 23 | 24 | # List of included launch descriptions 25 | launch_descriptions = [ 26 | # Launch move_group of MoveIt 2 27 | IncludeLaunchDescription( 28 | PythonLaunchDescriptionSource( 29 | PathJoinSubstitution( 30 | [ 31 | FindPackageShare("panda_moveit_config"), 32 | "launch", 33 | "move_group.launch.py", 34 | ] 35 | ) 36 | ), 37 | launch_arguments=[ 38 | ("ros2_control_plugin", "fake"), 39 | ("ros2_control_command_interface", "position"), 40 | ("rviz_config", rviz_config), 41 | ("use_sim_time", use_sim_time), 42 | ("log_level", log_level), 43 | ], 44 | ), 45 | ] 46 | 47 | return LaunchDescription(declared_arguments + launch_descriptions) 48 | 49 | 50 | def generate_declared_arguments() -> List[DeclareLaunchArgument]: 51 | """ 52 | Generate list of all launch arguments that are declared for this launch script. 53 | """ 54 | 55 | return [ 56 | # Miscellaneous 57 | DeclareLaunchArgument( 58 | "rviz_config", 59 | default_value=path.join( 60 | get_package_share_directory("panda_moveit_config"), 61 | "rviz", 62 | "moveit.rviz", 63 | ), 64 | description="Path to configuration for RViz2.", 65 | ), 66 | DeclareLaunchArgument( 67 | "use_sim_time", 68 | default_value="false", 69 | description="If true, use simulated clock.", 70 | ), 71 | DeclareLaunchArgument( 72 | "log_level", 73 | default_value="warn", 74 | description="The level of logging that is applied to all ROS 2 nodes launched by this script.", 75 | ), 76 | ] 77 | -------------------------------------------------------------------------------- /panda_moveit_config/srdf/panda_arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /panda_moveit_config/README.md: -------------------------------------------------------------------------------- 1 | # panda_moveit_config 2 | 3 | MoveIt configuration for Franka Emika Panda. 4 | 5 | ## Instructions 6 | 7 | ### move_group 8 | 9 | In order to configure and setup `move_group` of MoveIt 2 to plan motions inside a simulation (real robot is not yet supported), [move_group.launch.py](./launch/move_group.launch.py) script can be launched or included in another launch script. 10 | 11 | ```bash 12 | ros2 launch panda_moveit_config move_group.launch.py := 13 | ``` 14 | 15 | To see all arguments, please use `ros2 launch --show-args panda_moveit_config move_group.launch.py`. 16 | 17 | ### SRDF 18 | 19 | For SRDF, [panda.srdf.xacro](./srdf/panda.srdf.xacro) is the primary descriptor that includes all other xacros and creates a configuration based on the passed arguments. To generate SRDF out of xacro, you can use the included [xacro2srdf.bash](./scripts/xacro2srdf.bash) script and modify its arguments as needed. Once executed, [panda.srdf](./srdf/panda.srdf) will automatically be replaced. Alternatively, `xacro panda.srdf.xacro name:="panda" := ...` can be executed directly, e.g. this is preferred within any launch script. 20 | 21 | ## Examples 22 | 23 | ### fake_control 24 | 25 | To see if everything works in an isolated environment, try using [ex_fake_control.launch.py](./launch/ex_fake_control.launch.py) script that allows planning motions with MoveIt 2 and executing them with fake controllers inside RViz2. 26 | 27 | ```bash 28 | ros2 launch panda_moveit_config ex_fake_control.launch.py 29 | ``` 30 | 31 | ### ign_control 32 | 33 | For example inside Gazebo, try using [ex_ign_control.launch.py](./launch/ex_ign_control.launch.py) script that allows planning motions with MoveIt 2 and executing them with simulated controllers. 34 | 35 | ```bash 36 | ros2 launch panda_moveit_config ex_ign_control.launch.py 37 | ``` 38 | 39 | ## Directory Structure 40 | 41 | The following directory structure is utilised for this package. 42 | 43 | ```bash 44 | . 45 | ├── config/ # [dir] Configuration files for MoveIt 2 46 | ├── controllers_*.yaml # Configuration of ROS 2 controllers for different command interfaces 47 | ├── joint_limits.yaml # List of velocity and acceleration joint limits 48 | ├── kinematics.yaml # Configuration for the kinematic solver 49 | ├── moveit_controller_manager.yaml # List of controllers with their type and action namespace for use with MoveIt 2 50 | ├── ompl_planning.yaml # Configuration of OMPL planning and specific planners 51 | └── servo.yaml # Configuration for moveit_servo 52 | ├── launch/ # [dir] ROS 2 launch scripts 53 | ├── ex_fake_control.launch.py # Launch script virtual motion planning and execution inside RViz2 54 | └── move_group.launch.py # Launch script for configuring and setting up move_group of MoveIt 2 55 | ├── rviz/moveit.rviz # RViz2 config for motion planning with MoveIt 2 56 | ├── scripts/ # [dir] Additional useful scripts 57 | ├── srdf/ # [dir] SRDF description (xacros) 58 | ├── panda_arm.xacro # Macro for SRDF portion of Franka Emika Panda arm 59 | ├── panda_gripper.xacro # Macro for SRDF portion of Franka Emika Panda gripper 60 | ├── panda.srdf # SRDF generated from `panda.srdf.xacro` 61 | └── panda.srdf.xacro # The primary xacro of the robot that combines both arm and gripper 62 | ├── CMakeLists.txt # Colcon-enabled CMake recipe 63 | └── package.xml # ROS 2 package metadata 64 | ``` 65 | -------------------------------------------------------------------------------- /panda_moveit_config/config/servo.yaml: -------------------------------------------------------------------------------- 1 | ## Properties of incoming commands 2 | command_in_type: "speed_units" # "unitless" - in range [-1:1], "speed_units" - in m/s and rad/s 3 | scale: 4 | # Scale parameters are only used if command_in_type=="unitless" 5 | linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. 6 | rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. 7 | joint: 0.5 # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. 8 | 9 | ## Properties of outgoing commands 10 | publish_period: 0.2 # 1/Nominal publish rate [seconds] 11 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 12 | 13 | # What type of topic does your robot driver expect? 14 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory 15 | command_out_type: trajectory_msgs/JointTrajectory 16 | 17 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 18 | publish_joint_positions: true 19 | publish_joint_velocities: true 20 | publish_joint_accelerations: false 21 | 22 | ## Plugins for smoothing outgoing commands 23 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" 24 | low_pass_filter_coeff: 1.05 25 | 26 | ## MoveIt properties 27 | move_group_name: arm # Often 'manipulator' or 'arm' 28 | planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' 29 | 30 | ## Other frames 31 | ee_frame_name: panda_hand_tcp # The name of the end effector link, used to return the EE pose 32 | robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector 33 | 34 | ## Stopping behaviour 35 | incoming_command_timeout: 0.25 # Stop servoing if X seconds elapse without a new command 36 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 37 | # Important because ROS may drop some messages and we need the robot to halt reliably. 38 | num_outgoing_halt_msgs_to_publish: 1 39 | 40 | ## Configure handling of singularities and joint limits 41 | lower_singularity_threshold: 20.0 # Start decelerating when the condition number hits this (close to singularity) 42 | hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this 43 | joint_limit_margin: 0.087267 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 44 | 45 | ## Topic names 46 | cartesian_command_in_topic: /delta_twist_cmds # Topic for incoming Cartesian twist commands 47 | joint_command_in_topic: /delta_joint_cmds # Topic for incoming joint angle commands 48 | joint_topic: /joint_states 49 | status_topic: ~/status # Publish status to this topic 50 | command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here 51 | 52 | ## Collision checking for the entire robot body 53 | check_collisions: true # Check collisions? 54 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. 55 | # Two collision check algorithms are available: 56 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 57 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 58 | collision_check_type: threshold_distance 59 | # Parameters for "threshold_distance"-type collision checking 60 | self_collision_proximity_threshold: 0.03 # Start decelerating when a self-collision is this far [m] 61 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 62 | # Parameters for "stop_distance"-type collision checking 63 | collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency 64 | min_allowable_collision_distance: 0.025 # Stop if a collision is closer than this [m] 65 | -------------------------------------------------------------------------------- /.docker/run.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | TAG="andrejorsula/panda_ign_moveit2" 4 | 5 | ## Forward custom volumes and environment variables 6 | CUSTOM_VOLUMES=() 7 | CUSTOM_ENVS=() 8 | while getopts ":v:e:" opt; do 9 | case "${opt}" in 10 | v) CUSTOM_VOLUMES+=("${OPTARG}") ;; 11 | e) CUSTOM_ENVS+=("${OPTARG}") ;; 12 | *) 13 | echo >&2 "Usage: ${0} [-v VOLUME] [-e ENV] [TAG] [CMD]" 14 | exit 2 15 | ;; 16 | esac 17 | done 18 | shift "$((OPTIND - 1))" 19 | 20 | ## Determine TAG and CMD positional arguments 21 | if [ "${#}" -gt "0" ]; then 22 | if [[ $(docker images --format "{{.Tag}}" "${TAG}") =~ (^|[[:space:]])${1}($|[[:space:]]) || $(wget -q https://registry.hub.docker.com/v2/repositories/${TAG}/tags -O - | grep -Poe '(?<=(\"name\":\")).*?(?=\")') =~ (^|[[:space:]])${1}($|[[:space:]]) ]]; then 23 | # Use the first argument as a tag is such tag exists either locally or on the remote registry 24 | TAG="${TAG}:${1}" 25 | CMD=${*:2} 26 | else 27 | CMD=${*:1} 28 | fi 29 | fi 30 | 31 | ## GPU 32 | LS_HW_DISPLAY=$(lshw -C display 2> /dev/null | grep vendor) 33 | if [[ ${LS_HW_DISPLAY^^} =~ NVIDIA ]]; then 34 | # Enable GPU either via NVIDIA Container Toolkit or NVIDIA Docker (depending on Docker version) 35 | if dpkg --compare-versions "$(docker version --format '{{.Server.Version}}')" gt "19.3"; then 36 | GPU_OPT="--gpus all" 37 | else 38 | GPU_OPT="--runtime nvidia" 39 | fi 40 | GPU_ENVS=( 41 | NVIDIA_VISIBLE_DEVICES="all" 42 | NVIDIA_DRIVER_CAPABILITIES="compute,utility,graphics" 43 | ) 44 | fi 45 | 46 | ## GUI 47 | # To enable GUI, make sure processes in the container can connect to the x server 48 | XAUTH=/tmp/.docker.xauth 49 | if [ ! -f ${XAUTH} ]; then 50 | touch ${XAUTH} 51 | chmod a+r ${XAUTH} 52 | 53 | XAUTH_LIST=$(xauth nlist "${DISPLAY}") 54 | if [ -n "${XAUTH_LIST}" ]; then 55 | # shellcheck disable=SC2001 56 | XAUTH_LIST=$(sed -e 's/^..../ffff/' <<<"${XAUTH_LIST}") 57 | echo "${XAUTH_LIST}" | xauth -f ${XAUTH} nmerge - 58 | fi 59 | fi 60 | # GUI-enabling volumes 61 | GUI_VOLUMES=( 62 | "${XAUTH}:${XAUTH}" 63 | "/tmp/.X11-unix:/tmp/.X11-unix" 64 | "/dev/input:/dev/input" 65 | ) 66 | # GUI-enabling environment variables 67 | GUI_ENVS=( 68 | XAUTHORITY="${XAUTH}" 69 | QT_X11_NO_MITSHM=1 70 | DISPLAY="${DISPLAY}" 71 | ) 72 | 73 | ## Additional volumes 74 | # Synchronize timezone with host 75 | CUSTOM_VOLUMES+=("/etc/localtime:/etc/localtime:ro") 76 | 77 | ## Additional environment variables 78 | # Synchronize ROS_DOMAIN_ID with host 79 | if [ -n "${ROS_DOMAIN_ID}" ]; then 80 | CUSTOM_ENVS+=("ROS_DOMAIN_ID=${ROS_DOMAIN_ID}") 81 | fi 82 | # Synchronize IGN_PARTITION with host 83 | if [ -n "${IGN_PARTITION}" ]; then 84 | CUSTOM_ENVS+=("IGN_PARTITION=${IGN_PARTITION}") 85 | fi 86 | # Synchronize RMW configuration with host 87 | if [ -n "${RMW_IMPLEMENTATION}" ]; then 88 | CUSTOM_ENVS+=("RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION}") 89 | fi 90 | if [ -n "${CYCLONEDDS_URI}" ]; then 91 | CUSTOM_ENVS+=("CYCLONEDDS_URI=${CYCLONEDDS_URI}") 92 | CUSTOM_VOLUMES+=("${CYCLONEDDS_URI//file:\/\//}:${CYCLONEDDS_URI//file:\/\//}:ro") 93 | fi 94 | if [ -n "${FASTRTPS_DEFAULT_PROFILES_FILE}" ]; then 95 | CUSTOM_ENVS+=("FASTRTPS_DEFAULT_PROFILES_FILE=${FASTRTPS_DEFAULT_PROFILES_FILE}") 96 | CUSTOM_VOLUMES+=("${FASTRTPS_DEFAULT_PROFILES_FILE}:${FASTRTPS_DEFAULT_PROFILES_FILE}:ro") 97 | fi 98 | 99 | DOCKER_RUN_CMD=( 100 | docker run 101 | --interactive 102 | --tty 103 | --rm 104 | --network host 105 | --ipc host 106 | --privileged 107 | --security-opt "seccomp=unconfined" 108 | "${GUI_VOLUMES[@]/#/"--volume "}" 109 | "${GUI_ENVS[@]/#/"--env "}" 110 | "${GPU_OPT}" 111 | "${GPU_ENVS[@]/#/"--env "}" 112 | "${CUSTOM_VOLUMES[@]/#/"--volume "}" 113 | "${CUSTOM_ENVS[@]/#/"--env "}" 114 | "${TAG}" 115 | "${CMD}" 116 | ) 117 | 118 | echo -e "\033[1;30m${DOCKER_RUN_CMD[*]}\033[0m" | xargs 119 | 120 | # shellcheck disable=SC2048 121 | exec ${DOCKER_RUN_CMD[*]} 122 | -------------------------------------------------------------------------------- /panda_description/README.md: -------------------------------------------------------------------------------- 1 | # panda_description 2 | 3 | URDF and SDF description of Franka Emika Panda. 4 | 5 |

6 | Visualisation of panda visual and collision geometry 7 |

8 | 9 | ## Instructions 10 | 11 | ### URDF 12 | 13 | For URDF, [panda.urdf.xacro](./urdf/panda.urdf.xacro) is the primary descriptor that includes all other xacros and creates a model based on the passed arguments. To generate URDF out of xacro, you can use the included [xacro2urdf.bash](./scripts/xacro2urdf.bash) script and modify its arguments as needed. Once executed, [panda.urdf](./urdf/panda.urdf) will automatically be replaced. Alternatively, `xacro panda.urdf.xacro name:="panda" := ...` can be executed directly, e.g. this is preferred within any launch script. 14 | 15 | In order to visualise URDF with RViz2, included [view.launch.py](./launch/view.launch.py) script can be used. 16 | 17 | ```bash 18 | ros2 launch panda_description view.launch.py 19 | ``` 20 | 21 | ### SDF 22 | 23 | For SDF, please use the included [xacro2sdf.bash](./scripts/xacro2sdf.bash) script with the desired arguments. This script makes sure that a correct relative path is used to locate all assets. 24 | 25 | To visualise SDF with Gazebo, included [view_ign.launch.py](./launch/view_ign.launch.py) script can be used. 26 | 27 | ```bash 28 | ros2 launch panda_description view_ign.launch.py 29 | ``` 30 | 31 | #### Fuel 32 | 33 | If you do not require URDF and other resources from this repository, the default model (without `ros2_control`) can also be included directly from [Fuel](https://app.gazebosim.org/AndrejOrsula/fuel/models/panda) if you do not require the URDF description. 34 | 35 | ```xml 36 | 37 | https://fuel.gazebosim.org/1.0/AndrejOrsula/models/panda 38 | 39 | ``` 40 | 41 | ## Disclaimer 42 | 43 | Several of the included xacros and meshes originated in [frankaemika/franka_ros](https://github.com/frankaemika/franka_ros/tree/develop/franka_description). These files were modified to fit the purpose of this repository, e.g. xacros were refactored, inertial properties were estimated, support for `ros2_control` was added, mesh geometry was remodelled to improve performance, ... 44 | 45 | ## Directory Structure 46 | 47 | The following directory structure is utilised for this package because it provides compatibility with Gazebo, including [Fuel](https://app.gazebosim.org). 48 | 49 | ```bash 50 | . 51 | ├── config/initial_joint_positions.yaml # List of initial joint positions for fake and simulated ROS 2 control 52 | ├── launch/ # [dir] ROS 2 launch scripts 53 | ├── view.launch.py # Launch script for visualising URDF with RViz2 54 | └── view_ign.launch.py # Launch script for visualising SDF with Gazebo 55 | ├── panda/ # [dir] Model directory compatible with Fuel 56 | ├── meshes/ # [dir] Meshes for both URDF and SDF 57 | ├── **/collision/*.stl # STL meshes for collision geometry 58 | └── **/visual/*.dae # COLLADA meshes for visuals 59 | ├── thumbnails/ # [dir] Thumbnails for Fuel 60 | ├── model.config # Model meta data 61 | └── model.sdf # SDF (generated from URDF) 62 | ├── rviz/view.rviz # RViz2 config for visualising URDF 63 | ├── scripts/ # [dir] Additional useful scripts 64 | ├── urdf/ # [dir] URDF description (xacros) 65 | ├── panda_arm.xacro # Xacro for Franka Emika Panda arm 66 | ├── panda_gripper.xacro # Xacro for Franka Emika Panda gripper 67 | ├── panda_inertial.xacro # Macro for inertial properties of Franka Emika Panda 68 | ├── panda_utils.Macro # Macros for general boilerplate 69 | ├── panda.gazebo # Macros that add Gazebo plugins for Franka Emika Panda 70 | ├── panda.ros2_control # Macros that add ros2 control for Franka Emika Panda 71 | ├── panda.urdf # URDF (generated from panda.urdf.xacro) 72 | └── panda.urdf.xacro # High-level xacro for Franka Emika Panda 73 | ├── CMakeLists.txt # Colcon-enabled CMake recipe 74 | └── package.xml # ROS 2 package metadata 75 | ``` 76 | -------------------------------------------------------------------------------- /panda_description/urdf/panda.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 77 | 78 | 79 | 94 | 95 | 96 | 97 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /panda_description/launch/view_ign.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S ros2 launch 2 | """Visualisation of SDF model for panda in Ignition Gazebo. Note that the generated model://panda/model.sdf descriptor is used.""" 3 | 4 | from os import path 5 | from typing import List 6 | 7 | from launch import LaunchDescription 8 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 9 | from launch.launch_description_sources import PythonLaunchDescriptionSource 10 | from launch.substitutions import ( 11 | Command, 12 | FindExecutable, 13 | LaunchConfiguration, 14 | PathJoinSubstitution, 15 | ) 16 | from launch_ros.actions import Node 17 | from launch_ros.substitutions import FindPackageShare 18 | 19 | 20 | def generate_launch_description() -> LaunchDescription: 21 | # Declare all launch arguments 22 | declared_arguments = generate_declared_arguments() 23 | 24 | # Get substitution for all arguments 25 | description_package = LaunchConfiguration("description_package") 26 | description_filepath = LaunchConfiguration("description_filepath") 27 | world = LaunchConfiguration("world") 28 | model = LaunchConfiguration("model") 29 | use_sim_time = LaunchConfiguration("use_sim_time") 30 | ign_verbosity = LaunchConfiguration("ign_verbosity") 31 | log_level = LaunchConfiguration("log_level") 32 | 33 | # URDF 34 | _robot_description_xml = Command( 35 | [ 36 | PathJoinSubstitution([FindExecutable(name="xacro")]), 37 | " ", 38 | PathJoinSubstitution( 39 | [FindPackageShare(description_package), description_filepath] 40 | ), 41 | " ", 42 | "name:=", 43 | model, 44 | ] 45 | ) 46 | robot_description = {"robot_description": _robot_description_xml} 47 | 48 | # List of included launch descriptions 49 | launch_descriptions = [ 50 | # Launch Ignition Gazebo 51 | IncludeLaunchDescription( 52 | PythonLaunchDescriptionSource( 53 | PathJoinSubstitution( 54 | [ 55 | FindPackageShare("ros_ign_gazebo"), 56 | "launch", 57 | "ign_gazebo.launch.py", 58 | ] 59 | ) 60 | ), 61 | launch_arguments=[("ign_args", [world, " -v ", ign_verbosity])], 62 | ), 63 | ] 64 | 65 | # List of nodes to be launched 66 | nodes = [ 67 | # robot_state_publisher 68 | Node( 69 | package="robot_state_publisher", 70 | executable="robot_state_publisher", 71 | output="log", 72 | arguments=["--ros-args", "--log-level", log_level], 73 | parameters=[ 74 | robot_description, 75 | { 76 | "publish_frequency": 50.0, 77 | "frame_prefix": "", 78 | "use_sim_time": use_sim_time, 79 | }, 80 | ], 81 | ), 82 | # ros_ign_gazebo_create 83 | Node( 84 | package="ros_ign_gazebo", 85 | executable="create", 86 | output="log", 87 | arguments=["-file", model, "--ros-args", "--log-level", log_level], 88 | parameters=[{"use_sim_time": use_sim_time}], 89 | ), 90 | ] 91 | 92 | return LaunchDescription(declared_arguments + launch_descriptions + nodes) 93 | 94 | 95 | def generate_declared_arguments() -> List[DeclareLaunchArgument]: 96 | """ 97 | Generate list of all launch arguments that are declared for this launch script. 98 | """ 99 | 100 | return [ 101 | # Locations of robot resources 102 | DeclareLaunchArgument( 103 | "description_package", 104 | default_value="panda_description", 105 | description="Custom package with robot description.", 106 | ), 107 | DeclareLaunchArgument( 108 | "description_filepath", 109 | default_value=path.join("urdf", "panda.urdf.xacro"), 110 | description="Path to xacro or URDF description of the robot, relative to share of `description_package`.", 111 | ), 112 | # World and model for Ignition Gazebo 113 | DeclareLaunchArgument( 114 | "world", 115 | default_value="default.sdf", 116 | description="Name or filepath of world to load.", 117 | ), 118 | DeclareLaunchArgument( 119 | "model", 120 | default_value="panda", 121 | description="Name or filepath of model to load.", 122 | ), 123 | # Miscellaneous 124 | DeclareLaunchArgument( 125 | "use_sim_time", 126 | default_value="true", 127 | description="If true, use simulated clock.", 128 | ), 129 | DeclareLaunchArgument( 130 | "ign_verbosity", 131 | default_value="3", 132 | description="Verbosity level for Ignition Gazebo (0~4).", 133 | ), 134 | DeclareLaunchArgument( 135 | "log_level", 136 | default_value="warn", 137 | description="The level of logging that is applied to all ROS 2 nodes launched by this script.", 138 | ), 139 | ] 140 | -------------------------------------------------------------------------------- /panda_moveit_config/launch/ex_ign_control.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S ros2 launch 2 | """Example of planning with MoveIt2 and executing motions using fake ROS 2 controllers within RViz2""" 3 | 4 | from os import path 5 | from typing import List 6 | 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch import LaunchDescription 9 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 12 | from launch_ros.actions import Node 13 | from launch_ros.substitutions import FindPackageShare 14 | 15 | 16 | def generate_launch_description() -> LaunchDescription: 17 | # Declare all launch arguments 18 | declared_arguments = generate_declared_arguments() 19 | 20 | # Get substitution for all arguments 21 | world = LaunchConfiguration("world") 22 | model = LaunchConfiguration("model") 23 | rviz_config = LaunchConfiguration("rviz_config") 24 | use_sim_time = LaunchConfiguration("use_sim_time") 25 | ign_verbosity = LaunchConfiguration("ign_verbosity") 26 | log_level = LaunchConfiguration("log_level") 27 | 28 | # List of included launch descriptions 29 | launch_descriptions = [ 30 | # Launch Ignition Gazebo 31 | IncludeLaunchDescription( 32 | PythonLaunchDescriptionSource( 33 | PathJoinSubstitution( 34 | [ 35 | FindPackageShare("ros_ign_gazebo"), 36 | "launch", 37 | "ign_gazebo.launch.py", 38 | ] 39 | ) 40 | ), 41 | launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], 42 | ), 43 | # Launch move_group of MoveIt 2 44 | IncludeLaunchDescription( 45 | PythonLaunchDescriptionSource( 46 | PathJoinSubstitution( 47 | [ 48 | FindPackageShare("panda_moveit_config"), 49 | "launch", 50 | "move_group.launch.py", 51 | ] 52 | ) 53 | ), 54 | launch_arguments=[ 55 | ("ros2_control_plugin", "ign"), 56 | ("ros2_control_command_interface", "effort"), 57 | # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled 58 | ("collision_arm", "false"), 59 | ("rviz_config", rviz_config), 60 | ("use_sim_time", use_sim_time), 61 | ("log_level", log_level), 62 | ], 63 | ), 64 | ] 65 | 66 | # List of nodes to be launched 67 | nodes = [ 68 | # ros_ign_gazebo_create 69 | Node( 70 | package="ros_ign_gazebo", 71 | executable="create", 72 | output="log", 73 | arguments=["-file", model, "--ros-args", "--log-level", log_level], 74 | parameters=[{"use_sim_time": use_sim_time}], 75 | ), 76 | # ros_ign_bridge (clock -> ROS 2) 77 | Node( 78 | package="ros_ign_bridge", 79 | executable="parameter_bridge", 80 | output="log", 81 | arguments=[ 82 | "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", 83 | "--ros-args", 84 | "--log-level", 85 | log_level, 86 | ], 87 | parameters=[{"use_sim_time": use_sim_time}], 88 | ), 89 | ] 90 | 91 | return LaunchDescription(declared_arguments + launch_descriptions + nodes) 92 | 93 | 94 | def generate_declared_arguments() -> List[DeclareLaunchArgument]: 95 | """ 96 | Generate list of all launch arguments that are declared for this launch script. 97 | """ 98 | 99 | return [ 100 | # World and model for Ignition Gazebo 101 | DeclareLaunchArgument( 102 | "world", 103 | default_value="default.sdf", 104 | description="Name or filepath of world to load.", 105 | ), 106 | DeclareLaunchArgument( 107 | "model", 108 | default_value="panda", 109 | description="Name or filepath of model to load.", 110 | ), 111 | # Miscellaneous 112 | DeclareLaunchArgument( 113 | "rviz_config", 114 | default_value=path.join( 115 | get_package_share_directory("panda_moveit_config"), 116 | "rviz", 117 | "moveit.rviz", 118 | ), 119 | description="Path to configuration for RViz2.", 120 | ), 121 | DeclareLaunchArgument( 122 | "use_sim_time", 123 | default_value="true", 124 | description="If true, use simulated clock.", 125 | ), 126 | DeclareLaunchArgument( 127 | "ign_verbosity", 128 | default_value="3", 129 | description="Verbosity level for Ignition Gazebo (0~4).", 130 | ), 131 | DeclareLaunchArgument( 132 | "log_level", 133 | default_value="warn", 134 | description="The level of logging that is applied to all ROS 2 nodes launched by this script.", 135 | ), 136 | ] 137 | -------------------------------------------------------------------------------- /panda_description/urdf/panda_inertial.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /panda_moveit_config/srdf/panda.srdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /panda_description/scripts/estimate_inertial_properties.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | from os import listdir, path 5 | 6 | import trimesh 7 | from pcg_gazebo.parsers.sdf import SDF, create_sdf_element 8 | 9 | # Note: Both `trimesh` and `pcg_gazebo` can be installed via `pip` 10 | # `trimesh` is used to estimate volume and inertial properties from meshes of links 11 | # `pcg_gazebo` is used for its SDF parser 12 | 13 | 14 | def main(): 15 | # Total mass taken from datasheet (given as ~18.0kg) 16 | # You can also use your own estimate of total mass if you managed to weigh Panda yourself :) 17 | total_mass = 18.0 18 | if len(sys.argv) > 1: 19 | if float(sys.argv[1]) > 0.0: 20 | total_mass = float(sys.argv[1]) 21 | else: 22 | print("Error: Total mass of Panda (first argument) must be positive.") 23 | exit(1) 24 | print( 25 | "Estimating inertial properties for each link to add up to %f kg" % total_mass 26 | ) 27 | 28 | # Percentage of mass to redistribute from hand to fingers due to internal mechanical coupling 29 | # Choose whatever feels right (default value here is guesstimated) 30 | pct_mass_of_hand = 0.75 31 | if len(sys.argv) > 2: 32 | if float(sys.argv[2]) >= 0.0 and float(sys.argv[2]) < 1.0: 33 | pct_mass_of_hand = float(sys.argv[2]) 34 | else: 35 | print( 36 | "Error: Percentage of hand's mass to redistribute (second argument) must be in range [0.0, 1.0)." 37 | ) 38 | exit(1) 39 | 40 | # Get path to all visual meshes 41 | visual_mesh_dir = path.join( 42 | path.dirname(path.dirname(path.realpath(__file__))), "panda", "meshes", "visual" 43 | ) 44 | visual_mesh_basenames = listdir(visual_mesh_dir) 45 | visual_mesh_basenames.sort() 46 | 47 | # Load all meshes 48 | meshes = {} 49 | for mesh_basename in visual_mesh_basenames: 50 | link_name = path.splitext(mesh_basename)[0] 51 | mesh_path = path.join(visual_mesh_dir, mesh_basename) 52 | meshes[link_name] = trimesh.load(mesh_path, force="mesh", ignore_materials=True) 53 | 54 | # Compute the total volume of the robot in order to estimate the required density 55 | total_volume = 0.0 56 | for link_name in meshes: 57 | mesh = meshes[link_name] 58 | print("Volume estimate of %s: %f m^3" % (link_name, mesh.volume)) 59 | if link_name == "finger": 60 | total_volume += 2 * mesh.volume 61 | print("Note: Finger volume added twice to the total volume") 62 | else: 63 | total_volume += mesh.volume 64 | 65 | # Compute average density 66 | average_density = total_mass / total_volume 67 | print("Average density estimate: %f kg/m^3" % average_density) 68 | 69 | # Estimate inertial properties for each link 70 | mass = {} 71 | inertia = {} 72 | centre_of_mass = {} 73 | for link_name in meshes: 74 | mesh = meshes[link_name] 75 | mesh.density = average_density 76 | mass[link_name] = mesh.mass 77 | inertia[link_name] = mesh.moment_inertia 78 | centre_of_mass[link_name] = mesh.center_mass 79 | 80 | # Redistribute X% of the hand's mass to the fingers due to internal mechanical coupling 81 | # This improves reliability of grasps and makes fingers less susceptible to disturbances 82 | print("Redistributing %f%% of hand's mass fingers" % (100 * pct_mass_of_hand)) 83 | old_mass_finger = mass["finger"] 84 | # Add half of hand's redistributed mass to each finger 85 | finger_extra_mass = (pct_mass_of_hand / 2) * mass["hand"] 86 | mass["finger"] += finger_extra_mass 87 | # Then, update inertial proportionally to mass increase ratio 88 | inertia["finger"] *= mass["finger"] / old_mass_finger 89 | # Recompute centre of finger's mass to account for this redistribution 90 | # TODO: Read translation directly from SDF (currently copied and hard-coded) 91 | translation_hand_finger = [0.0, 0.0, 0.0584] 92 | for i in range(3): 93 | centre_of_mass["finger"][i] = ( 94 | old_mass_finger * centre_of_mass["finger"][i] 95 | + finger_extra_mass 96 | * (centre_of_mass["hand"][i] - translation_hand_finger[i]) 97 | ) / mass["finger"] 98 | # Reduce mass and inertia of hand to (1.0-X)% in order to account for redistribution of its mass 99 | mass["hand"] *= 1.0 - pct_mass_of_hand 100 | inertia["hand"] *= 1.0 - pct_mass_of_hand 101 | 102 | # Create a new SDF with one model 103 | sdf = SDF() 104 | sdf.add_model(name="panda") 105 | model = sdf.models[0] 106 | 107 | # Set inertial properties for each link into the SDF 108 | for link_name in meshes: 109 | link = create_sdf_element("link") 110 | link.mass = mass[link_name] 111 | link.inertia.ixx = inertia[link_name][0][0] 112 | link.inertia.iyy = inertia[link_name][1][1] 113 | link.inertia.izz = inertia[link_name][2][2] 114 | link.inertia.ixy = inertia[link_name][0][1] 115 | link.inertia.ixz = inertia[link_name][0][2] 116 | link.inertia.iyz = inertia[link_name][1][2] 117 | link.inertial.pose = [ 118 | centre_of_mass[link_name][0], 119 | centre_of_mass[link_name][1], 120 | centre_of_mass[link_name][2], 121 | 0.0, 122 | 0.0, 123 | 0.0, 124 | ] 125 | model.add_link(link_name, link) 126 | 127 | # Write into output file 128 | output_file = "panda_inertial_out.sdf" 129 | sdf.export_xml(output_file) 130 | print('Results written into "%s"' % output_file) 131 | 132 | 133 | if __name__ == "__main__": 134 | main() 135 | -------------------------------------------------------------------------------- /panda_description/urdf/panda_gripper.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 71 | 72 | 81 | 87 | 88 | 89 | 105 | 112 | 113 | 114 | 130 | 137 | 138 | 139 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | -------------------------------------------------------------------------------- /panda_description/urdf/panda_utils.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | true 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | transmission_interface/SimpleTransmission 124 | 125 | hardware_interface/EffortJointInterface 126 | 127 | 128 | 1 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | true 145 | true 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 161 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | true 194 | true 195 | 196 | 197 | 198 | 199 | 200 | -------------------------------------------------------------------------------- /panda_description/rviz/view.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.7153699994087219 9 | Tree Height: 673 10 | - Class: rviz_common/Selection 11 | Name: Selection 12 | - Class: rviz_common/Tool Properties 13 | Expanded: 14 | - /2D Goal Pose1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz_common/Views 19 | Expanded: 20 | - /Current View1 21 | - /Current View1/Focal Point1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 1 33 | Class: rviz_default_plugins/RobotModel 34 | Collision Enabled: false 35 | Description File: "" 36 | Description Source: Topic 37 | Description Topic: 38 | Depth: 5 39 | Durability Policy: Volatile 40 | History Policy: Keep Last 41 | Reliability Policy: Reliable 42 | Value: robot_description 43 | Enabled: true 44 | Links: 45 | All Links Enabled: true 46 | Expand Joint Details: false 47 | Expand Link Details: false 48 | Expand Tree: false 49 | Link Tree Style: Links in Alphabetic Order 50 | panda_hand: 51 | Alpha: 1 52 | Show Axes: false 53 | Show Trail: false 54 | Value: true 55 | panda_hand_tcp: 56 | Alpha: 1 57 | Show Axes: false 58 | Show Trail: false 59 | panda_leftfinger: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Value: true 64 | panda_link0: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | panda_link1: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | panda_link2: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | panda_link3: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | panda_link4: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | panda_link5: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | panda_link6: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | panda_link7: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | panda_link8: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | panda_rightfinger: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | world: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Mass Properties: 118 | Inertia: false 119 | Mass: false 120 | Name: RobotModel 121 | TF Prefix: "" 122 | Update Interval: 0 123 | Value: true 124 | Visual Enabled: true 125 | Enabled: true 126 | Global Options: 127 | Background Color: 48; 48; 48 128 | Fixed Frame: panda_link0 129 | Frame Rate: 30 130 | Name: root 131 | Tools: 132 | - Class: rviz_default_plugins/Interact 133 | Hide Inactive Objects: true 134 | - Class: rviz_default_plugins/MoveCamera 135 | - Class: rviz_default_plugins/Select 136 | - Class: rviz_default_plugins/FocusCamera 137 | - Class: rviz_default_plugins/Measure 138 | Line color: 128; 128; 0 139 | - Class: rviz_default_plugins/SetInitialPose 140 | Covariance x: 0.25 141 | Covariance y: 0.25 142 | Covariance yaw: 0.06853891909122467 143 | Topic: 144 | Depth: 5 145 | Durability Policy: Volatile 146 | History Policy: Keep Last 147 | Reliability Policy: Reliable 148 | Value: /initialpose 149 | - Class: rviz_default_plugins/SetGoal 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /goal_pose 156 | - Class: rviz_default_plugins/PublishPoint 157 | Single click: true 158 | Topic: 159 | Depth: 5 160 | Durability Policy: Volatile 161 | History Policy: Keep Last 162 | Reliability Policy: Reliable 163 | Value: /clicked_point 164 | Transformation: 165 | Current: 166 | Class: rviz_default_plugins/TF 167 | Value: true 168 | Views: 169 | Current: 170 | Class: rviz_default_plugins/Orbit 171 | Distance: 1.5 172 | Enable Stereo Rendering: 173 | Stereo Eye Separation: 0.05999999865889549 174 | Stereo Focal Distance: 1 175 | Swap Stereo Eyes: false 176 | Value: false 177 | Focal Point: 178 | X: 0.3333333432674408 179 | Y: 0 180 | Z: 0.5 181 | Focal Shape Fixed Size: false 182 | Focal Shape Size: 0.05000000074505806 183 | Invert Z Axis: false 184 | Name: Current View 185 | Near Clip Distance: 0.009999999776482582 186 | Pitch: 0.39269909262657166 187 | Target Frame: 188 | Value: Orbit (rviz) 189 | Yaw: 0.7853981852531433 190 | Saved: ~ 191 | Window Geometry: 192 | Displays: 193 | collapsed: false 194 | Height: 720 195 | Hide Left Dock: false 196 | Hide Right Dock: true 197 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000002f0000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000002f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000002f0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000050fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000318000002f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 198 | Selection: 199 | collapsed: false 200 | Time: 201 | collapsed: false 202 | Tool Properties: 203 | collapsed: false 204 | Views: 205 | collapsed: true 206 | Width: 1280 207 | X: 0 208 | Y: 0 209 | -------------------------------------------------------------------------------- /panda_description/urdf/panda.ros2_control: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | fake_components/GenericSystem 27 | 28 | 29 | ign_ros2_control/IgnitionSystem 30 | 31 | 32 | 33 | 34 | 35 | ${plugin} 36 | 37 | 38 | 39 | ${initial_positions['joint1']} 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | ${initial_positions['joint2']} 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | ${initial_positions['joint3']} 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | ${initial_positions['joint4']} 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | ${initial_positions['joint5']} 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | ${initial_positions['joint6']} 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | ${initial_positions['joint7']} 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 158 | 159 | 160 | 161 | 162 | 163 | fake_components/GenericSystem 164 | 165 | 166 | ign_ros2_control/IgnitionSystem 167 | 168 | 169 | 170 | 171 | 172 | ${plugin} 173 | 174 | 175 | 176 | ${initial_positions['joint1']} 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | ${initial_positions['joint2']} 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | -------------------------------------------------------------------------------- /panda_description/launch/view.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S ros2 launch 2 | """Visualisation of URDF model for panda in RViz2""" 3 | 4 | from os import path 5 | from typing import List 6 | 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch import LaunchDescription 9 | from launch.actions import DeclareLaunchArgument 10 | from launch.substitutions import ( 11 | Command, 12 | FindExecutable, 13 | LaunchConfiguration, 14 | PathJoinSubstitution, 15 | ) 16 | from launch_ros.actions import Node 17 | from launch_ros.substitutions import FindPackageShare 18 | 19 | 20 | def generate_launch_description() -> LaunchDescription: 21 | # Declare all launch arguments 22 | declared_arguments = generate_declared_arguments() 23 | 24 | # Get substitution for all arguments 25 | description_package = LaunchConfiguration("description_package") 26 | description_filepath = LaunchConfiguration("description_filepath") 27 | name = LaunchConfiguration("name") 28 | prefix = LaunchConfiguration("prefix") 29 | gripper = LaunchConfiguration("gripper") 30 | collision_arm = LaunchConfiguration("collision_arm") 31 | collision_gripper = LaunchConfiguration("collision_gripper") 32 | safety_limits = LaunchConfiguration("safety_limits") 33 | safety_position_margin = LaunchConfiguration("safety_position_margin") 34 | safety_k_position = LaunchConfiguration("safety_k_position") 35 | safety_k_velocity = LaunchConfiguration("safety_k_velocity") 36 | ros2_control = LaunchConfiguration("ros2_control") 37 | ros2_control_plugin = LaunchConfiguration("ros2_control_plugin") 38 | ros2_control_command_interface = LaunchConfiguration( 39 | "ros2_control_command_interface" 40 | ) 41 | gazebo_preserve_fixed_joint = LaunchConfiguration("gazebo_preserve_fixed_joint") 42 | rviz_config = LaunchConfiguration("rviz_config") 43 | use_sim_time = LaunchConfiguration("use_sim_time") 44 | log_level = LaunchConfiguration("log_level") 45 | 46 | # Extract URDF from description file 47 | robot_description_content = Command( 48 | [ 49 | PathJoinSubstitution([FindExecutable(name="xacro")]), 50 | " ", 51 | PathJoinSubstitution( 52 | [FindPackageShare(description_package), description_filepath] 53 | ), 54 | " ", 55 | "name:=", 56 | name, 57 | " ", 58 | "prefix:=", 59 | prefix, 60 | " ", 61 | "gripper:=", 62 | gripper, 63 | " ", 64 | "collision_arm:=", 65 | collision_arm, 66 | " ", 67 | "collision_gripper:=", 68 | collision_gripper, 69 | " ", 70 | "safety_limits:=", 71 | safety_limits, 72 | " ", 73 | "safety_position_margin:=", 74 | safety_position_margin, 75 | " ", 76 | "safety_k_position:=", 77 | safety_k_position, 78 | " ", 79 | "safety_k_velocity:=", 80 | safety_k_velocity, 81 | " ", 82 | "ros2_control:=", 83 | ros2_control, 84 | " ", 85 | "ros2_control_plugin:=", 86 | ros2_control_plugin, 87 | " ", 88 | "ros2_control_command_interface:=", 89 | ros2_control_command_interface, 90 | " ", 91 | "gazebo_preserve_fixed_joint:=", 92 | gazebo_preserve_fixed_joint, 93 | ] 94 | ) 95 | robot_description = {"robot_description": robot_description_content} 96 | 97 | # List of nodes to be launched 98 | nodes = [ 99 | # robot_state_publisher 100 | Node( 101 | package="robot_state_publisher", 102 | executable="robot_state_publisher", 103 | output="log", 104 | arguments=["--ros-args", "--log-level", log_level], 105 | parameters=[robot_description, {"use_sim_time": use_sim_time}], 106 | ), 107 | # rviz2 108 | Node( 109 | package="rviz2", 110 | executable="rviz2", 111 | output="log", 112 | arguments=[ 113 | "--display-config", 114 | rviz_config, 115 | "--ros-args", 116 | "--log-level", 117 | log_level, 118 | ], 119 | parameters=[{"use_sim_time": use_sim_time}], 120 | ), 121 | # joint_state_publisher_gui 122 | Node( 123 | package="joint_state_publisher_gui", 124 | executable="joint_state_publisher_gui", 125 | output="log", 126 | arguments=["--ros-args", "--log-level", log_level], 127 | parameters=[{"use_sim_time": use_sim_time}], 128 | ), 129 | ] 130 | 131 | return LaunchDescription(declared_arguments + nodes) 132 | 133 | 134 | def generate_declared_arguments() -> List[DeclareLaunchArgument]: 135 | """ 136 | Generate list of all launch arguments that are declared for this launch script. 137 | """ 138 | 139 | return [ 140 | # Location of xacro/URDF to visualise 141 | DeclareLaunchArgument( 142 | "description_package", 143 | default_value="panda_description", 144 | description="Custom package with robot description.", 145 | ), 146 | DeclareLaunchArgument( 147 | "description_filepath", 148 | default_value=path.join("urdf", "panda.urdf.xacro"), 149 | description="Path to xacro or URDF description of the robot, relative to share of `description_package`.", 150 | ), 151 | # Naming of the robot 152 | DeclareLaunchArgument( 153 | "name", 154 | default_value="panda", 155 | description="Name of the robot.", 156 | ), 157 | DeclareLaunchArgument( 158 | "prefix", 159 | default_value="panda_", 160 | description="Prefix for all robot entities. If modified, then joint names in the configuration of controllers must also be updated.", 161 | ), 162 | # Gripper 163 | DeclareLaunchArgument( 164 | "gripper", 165 | default_value="true", 166 | description="Flag to enable default gripper.", 167 | ), 168 | # Collision geometry 169 | DeclareLaunchArgument( 170 | "collision_arm", 171 | default_value="true", 172 | description="Flag to enable collision geometry for manipulator's arm.", 173 | ), 174 | DeclareLaunchArgument( 175 | "collision_gripper", 176 | default_value="true", 177 | description="Flag to enable collision geometry for manipulator's gripper (hand and fingers).", 178 | ), 179 | # Safety controller 180 | DeclareLaunchArgument( 181 | "safety_limits", 182 | default_value="true", 183 | description="Flag to enable safety limits controllers on manipulator joints.", 184 | ), 185 | DeclareLaunchArgument( 186 | "safety_position_margin", 187 | default_value="0.15", 188 | description="Lower and upper margin for position limits of all safety controllers.", 189 | ), 190 | DeclareLaunchArgument( 191 | "safety_k_position", 192 | default_value="100.0", 193 | description="Parametric k-position factor of all safety controllers.", 194 | ), 195 | DeclareLaunchArgument( 196 | "safety_k_velocity", 197 | default_value="40.0", 198 | description="Parametric k-velocity factor of all safety controllers.", 199 | ), 200 | # ROS 2 control 201 | DeclareLaunchArgument( 202 | "ros2_control", 203 | default_value="true", 204 | description="Flag to enable ros2 controllers for manipulator.", 205 | ), 206 | DeclareLaunchArgument( 207 | "ros2_control_plugin", 208 | default_value="ign", 209 | description="The ros2_control plugin that should be loaded for the manipulator ('fake', 'ign', 'real' or custom).", 210 | ), 211 | DeclareLaunchArgument( 212 | "ros2_control_command_interface", 213 | default_value="effort", 214 | description="The output control command interface provided by ros2_control ('position', 'velocity', 'effort' or certain combinations 'position,velocity').", 215 | ), 216 | # Gazebo 217 | DeclareLaunchArgument( 218 | "gazebo_preserve_fixed_joint", 219 | default_value="false", 220 | description="Flag to preserve fixed joints and prevent lumping when generating SDF for Gazebo.", 221 | ), 222 | # Miscellaneous 223 | DeclareLaunchArgument( 224 | "rviz_config", 225 | default_value=path.join( 226 | get_package_share_directory("panda_description"), 227 | "rviz", 228 | "view.rviz", 229 | ), 230 | description="Path to configuration for RViz2.", 231 | ), 232 | DeclareLaunchArgument( 233 | "use_sim_time", 234 | default_value="false", 235 | description="If true, use simulated clock.", 236 | ), 237 | DeclareLaunchArgument( 238 | "log_level", 239 | default_value="warn", 240 | description="The level of logging that is applied to all ROS 2 nodes launched by this script.", 241 | ), 242 | ] 243 | -------------------------------------------------------------------------------- /panda_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | FMTkConfigDefault: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMTkConfigDefault: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | 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 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDSTkConfigDefault: 74 | type: geometric::PDST 75 | STRIDEkConfigDefault: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | 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 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRTkConfigDefault: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRTkConfigDefault: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiESTkConfigDefault: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjESTkConfigDefault: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRMkConfigDefault: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstarkConfigDefault: 110 | type: geometric::LazyPRMstar 111 | SPARSkConfigDefault: 112 | type: geometric::SPARS 113 | 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 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwokConfigDefault: 118 | type: geometric::SPARStwo 119 | 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 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | TrajOptDefault: 124 | type: geometric::TrajOpt 125 | 126 | arm: 127 | enforce_constrained_state_space: true 128 | projection_evaluator: joints(panda_joint1,panda_joint2) 129 | planner_configs: 130 | - SBLkConfigDefault 131 | - ESTkConfigDefault 132 | - LBKPIECEkConfigDefault 133 | - BKPIECEkConfigDefault 134 | - KPIECEkConfigDefault 135 | - RRTkConfigDefault 136 | - RRTConnectkConfigDefault 137 | - RRTstarkConfigDefault 138 | - TRRTkConfigDefault 139 | - PRMkConfigDefault 140 | - PRMstarkConfigDefault 141 | - FMTkConfigDefault 142 | - BFMTkConfigDefault 143 | - PDSTkConfigDefault 144 | - STRIDEkConfigDefault 145 | - BiTRRTkConfigDefault 146 | - LBTRRTkConfigDefault 147 | - BiESTkConfigDefault 148 | - ProjESTkConfigDefault 149 | - LazyPRMkConfigDefault 150 | - LazyPRMstarkConfigDefault 151 | - SPARSkConfigDefault 152 | - SPARStwokConfigDefault 153 | - TrajOptDefault 154 | longest_valid_segment_fraction: 0.005 155 | -------------------------------------------------------------------------------- /panda_moveit_config/rviz/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /MotionPlanning1 9 | Splitter Ratio: 0.6278260946273804 10 | Tree Height: 412 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | - /Current View1/Focal Point1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Acceleration_Scaling_Factor: 1 34 | Class: moveit_rviz_plugin/MotionPlanning 35 | Enabled: true 36 | Move Group Namespace: "" 37 | MoveIt_Allow_Approximate_IK: false 38 | MoveIt_Allow_External_Program: false 39 | MoveIt_Allow_Replanning: false 40 | MoveIt_Allow_Sensor_Positioning: false 41 | MoveIt_Planning_Attempts: 10 42 | MoveIt_Planning_Time: 5 43 | MoveIt_Use_Cartesian_Path: false 44 | MoveIt_Use_Constraint_Aware_IK: false 45 | MoveIt_Warehouse_Host: 127.0.0.1 46 | MoveIt_Warehouse_Port: 33829 47 | MoveIt_Workspace: 48 | Center: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Size: 53 | X: 2 54 | Y: 2 55 | Z: 2 56 | Name: MotionPlanning 57 | Planned Path: 58 | Color Enabled: false 59 | Interrupt Display: false 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | panda_hand: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | panda_hand_tcp: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | panda_leftfinger: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | panda_link0: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | panda_link1: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | panda_link2: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | panda_link3: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | panda_link4: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | panda_link5: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | panda_link6: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | panda_link7: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | panda_link8: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | panda_rightfinger: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | world: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Loop Animation: true 134 | Robot Alpha: 0.25 135 | Robot Color: 150; 50; 150 136 | Show Robot Collision: false 137 | Show Robot Visual: true 138 | Show Trail: false 139 | State Display Time: 0.1s 140 | Trail Step Size: 1 141 | Trajectory Topic: /display_planned_path 142 | Planning Metrics: 143 | Payload: 1 144 | Show Joint Torques: false 145 | Show Manipulability: false 146 | Show Manipulability Index: false 147 | Show Weight Limit: false 148 | TextHeight: 0.07999999821186066 149 | Planning Request: 150 | Colliding Link Color: 255; 0; 0 151 | Goal State Alpha: 0.5 152 | Goal State Color: 250; 128; 0 153 | Interactive Marker Size: 0.10000000149011612 154 | Joint Violation Color: 255; 0; 255 155 | Planning Group: arm 156 | Query Goal State: true 157 | Query Start State: false 158 | Show Workspace: false 159 | Start State Alpha: 0.5 160 | Start State Color: 0; 255; 0 161 | Planning Scene Topic: monitored_planning_scene 162 | Robot Description: robot_description 163 | Scene Geometry: 164 | Scene Alpha: 0.8999999761581421 165 | Scene Color: 50; 230; 50 166 | Scene Display Time: 0.009999999776482582 167 | Show Scene Geometry: true 168 | Voxel Coloring: Z-Axis 169 | Voxel Rendering: Occupied Voxels 170 | Scene Robot: 171 | Attached Body Color: 150; 50; 150 172 | Links: 173 | All Links Enabled: true 174 | Expand Joint Details: false 175 | Expand Link Details: false 176 | Expand Tree: false 177 | Link Tree Style: Links in Alphabetic Order 178 | panda_hand: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | Value: true 183 | panda_hand_tcp: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | panda_leftfinger: 188 | Alpha: 1 189 | Show Axes: false 190 | Show Trail: false 191 | Value: true 192 | panda_link0: 193 | Alpha: 1 194 | Show Axes: false 195 | Show Trail: false 196 | Value: true 197 | panda_link1: 198 | Alpha: 1 199 | Show Axes: false 200 | Show Trail: false 201 | Value: true 202 | panda_link2: 203 | Alpha: 1 204 | Show Axes: false 205 | Show Trail: false 206 | Value: true 207 | panda_link3: 208 | Alpha: 1 209 | Show Axes: false 210 | Show Trail: false 211 | Value: true 212 | panda_link4: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | Value: true 217 | panda_link5: 218 | Alpha: 1 219 | Show Axes: false 220 | Show Trail: false 221 | Value: true 222 | panda_link6: 223 | Alpha: 1 224 | Show Axes: false 225 | Show Trail: false 226 | Value: true 227 | panda_link7: 228 | Alpha: 1 229 | Show Axes: false 230 | Show Trail: false 231 | Value: true 232 | panda_link8: 233 | Alpha: 1 234 | Show Axes: false 235 | Show Trail: false 236 | panda_rightfinger: 237 | Alpha: 1 238 | Show Axes: false 239 | Show Trail: false 240 | Value: true 241 | world: 242 | Alpha: 1 243 | Show Axes: false 244 | Show Trail: false 245 | Robot Alpha: 1 246 | Show Robot Collision: false 247 | Show Robot Visual: true 248 | Value: true 249 | Velocity_Scaling_Factor: 1 250 | Enabled: true 251 | Global Options: 252 | Background Color: 48; 48; 48 253 | Fixed Frame: panda_link0 254 | Frame Rate: 30 255 | Name: root 256 | Tools: 257 | - Class: rviz_default_plugins/Interact 258 | Hide Inactive Objects: true 259 | - Class: rviz_default_plugins/MoveCamera 260 | - Class: rviz_default_plugins/Select 261 | - Class: rviz_default_plugins/FocusCamera 262 | - Class: rviz_default_plugins/Measure 263 | Line color: 128; 128; 0 264 | - Class: rviz_default_plugins/SetInitialPose 265 | Covariance x: 0.25 266 | Covariance y: 0.25 267 | Covariance yaw: 0.06853891909122467 268 | Topic: 269 | Depth: 5 270 | Durability Policy: Volatile 271 | History Policy: Keep Last 272 | Reliability Policy: Reliable 273 | Value: /initialpose 274 | - Class: rviz_default_plugins/SetGoal 275 | Topic: 276 | Depth: 5 277 | Durability Policy: Volatile 278 | History Policy: Keep Last 279 | Reliability Policy: Reliable 280 | Value: /goal_pose 281 | - Class: rviz_default_plugins/PublishPoint 282 | Single click: true 283 | Topic: 284 | Depth: 5 285 | Durability Policy: Volatile 286 | History Policy: Keep Last 287 | Reliability Policy: Reliable 288 | Value: /clicked_point 289 | Transformation: 290 | Current: 291 | Class: rviz_default_plugins/TF 292 | Value: true 293 | Views: 294 | Current: 295 | Class: rviz_default_plugins/Orbit 296 | Distance: 1.5 297 | Enable Stereo Rendering: 298 | Stereo Eye Separation: 0.05999999865889549 299 | Stereo Focal Distance: 1 300 | Swap Stereo Eyes: false 301 | Value: false 302 | Focal Point: 303 | X: 0.3333333432674408 304 | Y: 0 305 | Z: 0.5 306 | Focal Shape Fixed Size: false 307 | Focal Shape Size: 0.05000000074505806 308 | Invert Z Axis: false 309 | Name: Current View 310 | Near Clip Distance: 0.009999999776482582 311 | Pitch: 0.39269909262657166 312 | Target Frame: 313 | Value: Orbit (rviz) 314 | Yaw: 0.7853981852531433 315 | Saved: ~ 316 | Window Geometry: 317 | Displays: 318 | collapsed: false 319 | Height: 720 320 | Hide Left Dock: false 321 | Hide Right Dock: true 322 | MotionPlanning: 323 | collapsed: false 324 | MotionPlanning - Trajectory Slider: 325 | collapsed: false 326 | QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 327 | Selection: 328 | collapsed: false 329 | Time: 330 | collapsed: false 331 | Tool Properties: 332 | collapsed: false 333 | Views: 334 | collapsed: true 335 | Width: 1280 336 | X: 0 337 | Y: 0 338 | -------------------------------------------------------------------------------- /panda_description/urdf/panda_arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 143 | 144 | 153 | 159 | 160 | 161 | 181 | 187 | 188 | 189 | 209 | 215 | 216 | 217 | 237 | 243 | 244 | 245 | 265 | 271 | 272 | 273 | 293 | 299 | 300 | 301 | 321 | 327 | 328 | 329 | 349 | 355 | 356 | 357 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | -------------------------------------------------------------------------------- /panda_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env -S ros2 launch 2 | """Configure and setup move group for planning with MoveIt 2""" 3 | 4 | from os import path 5 | from typing import List 6 | 7 | import yaml 8 | from ament_index_python.packages import get_package_share_directory 9 | from launch import LaunchDescription 10 | from launch.actions import DeclareLaunchArgument 11 | from launch.conditions import IfCondition 12 | from launch.substitutions import ( 13 | Command, 14 | FindExecutable, 15 | LaunchConfiguration, 16 | PathJoinSubstitution, 17 | PythonExpression, 18 | ) 19 | from launch_ros.actions import Node 20 | from launch_ros.substitutions import FindPackageShare 21 | 22 | 23 | def generate_launch_description(): 24 | # Declare all launch arguments 25 | declared_arguments = generate_declared_arguments() 26 | 27 | # Get substitution for all arguments 28 | description_package = LaunchConfiguration("description_package") 29 | description_filepath = LaunchConfiguration("description_filepath") 30 | moveit_config_package = "panda_moveit_config" 31 | name = LaunchConfiguration("name") 32 | prefix = LaunchConfiguration("prefix") 33 | gripper = LaunchConfiguration("gripper") 34 | collision_arm = LaunchConfiguration("collision_arm") 35 | collision_gripper = LaunchConfiguration("collision_gripper") 36 | safety_limits = LaunchConfiguration("safety_limits") 37 | safety_position_margin = LaunchConfiguration("safety_position_margin") 38 | safety_k_position = LaunchConfiguration("safety_k_position") 39 | safety_k_velocity = LaunchConfiguration("safety_k_velocity") 40 | ros2_control = LaunchConfiguration("ros2_control") 41 | ros2_control_plugin = LaunchConfiguration("ros2_control_plugin") 42 | ros2_control_command_interface = LaunchConfiguration( 43 | "ros2_control_command_interface" 44 | ) 45 | gazebo_preserve_fixed_joint = LaunchConfiguration("gazebo_preserve_fixed_joint") 46 | enable_servo = LaunchConfiguration("enable_servo") 47 | enable_rviz = LaunchConfiguration("enable_rviz") 48 | rviz_config = LaunchConfiguration("rviz_config") 49 | use_sim_time = LaunchConfiguration("use_sim_time") 50 | log_level = LaunchConfiguration("log_level") 51 | 52 | # URDF 53 | _robot_description_xml = Command( 54 | [ 55 | PathJoinSubstitution([FindExecutable(name="xacro")]), 56 | " ", 57 | PathJoinSubstitution( 58 | [FindPackageShare(description_package), description_filepath] 59 | ), 60 | " ", 61 | "name:=", 62 | name, 63 | " ", 64 | "prefix:=", 65 | prefix, 66 | " ", 67 | "gripper:=", 68 | gripper, 69 | " ", 70 | "collision_arm:=", 71 | collision_arm, 72 | " ", 73 | "collision_gripper:=", 74 | collision_gripper, 75 | " ", 76 | "safety_limits:=", 77 | safety_limits, 78 | " ", 79 | "safety_position_margin:=", 80 | safety_position_margin, 81 | " ", 82 | "safety_k_position:=", 83 | safety_k_position, 84 | " ", 85 | "safety_k_velocity:=", 86 | safety_k_velocity, 87 | " ", 88 | "ros2_control:=", 89 | ros2_control, 90 | " ", 91 | "ros2_control_plugin:=", 92 | ros2_control_plugin, 93 | " ", 94 | "ros2_control_command_interface:=", 95 | ros2_control_command_interface, 96 | " ", 97 | "gazebo_preserve_fixed_joint:=", 98 | gazebo_preserve_fixed_joint, 99 | ] 100 | ) 101 | robot_description = {"robot_description": _robot_description_xml} 102 | 103 | # SRDF 104 | _robot_description_semantic_xml = Command( 105 | [ 106 | PathJoinSubstitution([FindExecutable(name="xacro")]), 107 | " ", 108 | PathJoinSubstitution( 109 | [ 110 | FindPackageShare(moveit_config_package), 111 | "srdf", 112 | "panda.srdf.xacro", 113 | ] 114 | ), 115 | " ", 116 | "name:=", 117 | name, 118 | " ", 119 | "prefix:=", 120 | prefix, 121 | ] 122 | ) 123 | robot_description_semantic = { 124 | "robot_description_semantic": _robot_description_semantic_xml 125 | } 126 | 127 | # Kinematics 128 | kinematics = load_yaml( 129 | moveit_config_package, path.join("config", "kinematics.yaml") 130 | ) 131 | 132 | # Joint limits 133 | joint_limits = { 134 | "robot_description_planning": load_yaml( 135 | moveit_config_package, path.join("config", "joint_limits.yaml") 136 | ) 137 | } 138 | 139 | # Servo 140 | servo_params = { 141 | "moveit_servo": load_yaml( 142 | moveit_config_package, path.join("config", "servo.yaml") 143 | ) 144 | } 145 | servo_params["moveit_servo"].update({"use_gazebo": use_sim_time}) 146 | 147 | # Planning pipeline 148 | planning_pipeline = { 149 | "planning_pipelines": ["ompl"], 150 | "default_planning_pipeline": "ompl", 151 | "ompl": { 152 | "planning_plugin": "ompl_interface/OMPLPlanner", 153 | # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved 154 | "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", 155 | # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled 156 | "start_state_max_bounds_error": 0.31416, 157 | }, 158 | } 159 | _ompl_yaml = load_yaml( 160 | moveit_config_package, path.join("config", "ompl_planning.yaml") 161 | ) 162 | planning_pipeline["ompl"].update(_ompl_yaml) 163 | 164 | # Planning scene 165 | planning_scene_monitor_parameters = { 166 | "publish_planning_scene": True, 167 | "publish_geometry_updates": True, 168 | "publish_state_updates": True, 169 | "publish_transforms_updates": True, 170 | } 171 | 172 | # MoveIt controller manager 173 | moveit_controller_manager_yaml = load_yaml( 174 | moveit_config_package, path.join("config", "moveit_controller_manager.yaml") 175 | ) 176 | moveit_controller_manager = { 177 | "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", 178 | "moveit_simple_controller_manager": moveit_controller_manager_yaml, 179 | } 180 | 181 | # Trajectory execution 182 | trajectory_execution = { 183 | "allow_trajectory_execution": True, 184 | "moveit_manage_controllers": False, 185 | "trajectory_execution.allowed_execution_duration_scaling": 1.2, 186 | "trajectory_execution.allowed_goal_duration_margin": 0.5, 187 | "trajectory_execution.allowed_start_tolerance": 0.01, 188 | } 189 | 190 | # Controller parameters 191 | declared_arguments.append( 192 | DeclareLaunchArgument( 193 | "__controller_parameters_basename", 194 | default_value=["controllers_", ros2_control_command_interface, ".yaml"], 195 | ) 196 | ) 197 | controller_parameters = PathJoinSubstitution( 198 | [ 199 | FindPackageShare(moveit_config_package), 200 | "config", 201 | LaunchConfiguration("__controller_parameters_basename"), 202 | ] 203 | ) 204 | 205 | # List of nodes to be launched 206 | nodes = [ 207 | # robot_state_publisher 208 | Node( 209 | package="robot_state_publisher", 210 | executable="robot_state_publisher", 211 | output="log", 212 | arguments=["--ros-args", "--log-level", log_level], 213 | parameters=[ 214 | robot_description, 215 | { 216 | "publish_frequency": 50.0, 217 | "frame_prefix": "", 218 | "use_sim_time": use_sim_time, 219 | }, 220 | ], 221 | ), 222 | # ros2_control_node (only for fake controller) 223 | Node( 224 | package="controller_manager", 225 | executable="ros2_control_node", 226 | output="log", 227 | arguments=["--ros-args", "--log-level", log_level], 228 | parameters=[ 229 | robot_description, 230 | controller_parameters, 231 | {"use_sim_time": use_sim_time}, 232 | ], 233 | condition=( 234 | IfCondition( 235 | PythonExpression( 236 | [ 237 | "'", 238 | ros2_control_plugin, 239 | "'", 240 | " == ", 241 | "'fake'", 242 | ] 243 | ) 244 | ) 245 | ), 246 | ), 247 | # move_group (with execution) 248 | Node( 249 | package="moveit_ros_move_group", 250 | executable="move_group", 251 | output="log", 252 | arguments=["--ros-args", "--log-level", log_level], 253 | parameters=[ 254 | robot_description, 255 | robot_description_semantic, 256 | kinematics, 257 | joint_limits, 258 | planning_pipeline, 259 | trajectory_execution, 260 | planning_scene_monitor_parameters, 261 | moveit_controller_manager, 262 | {"use_sim_time": use_sim_time}, 263 | ], 264 | ), 265 | # move_servo 266 | Node( 267 | package="moveit_servo", 268 | executable="servo_node_main", 269 | output="log", 270 | arguments=["--ros-args", "--log-level", log_level], 271 | parameters=[ 272 | robot_description, 273 | robot_description_semantic, 274 | kinematics, 275 | joint_limits, 276 | planning_pipeline, 277 | trajectory_execution, 278 | planning_scene_monitor_parameters, 279 | servo_params, 280 | {"use_sim_time": use_sim_time}, 281 | ], 282 | condition=IfCondition(enable_servo), 283 | ), 284 | # rviz2 285 | Node( 286 | package="rviz2", 287 | executable="rviz2", 288 | output="log", 289 | arguments=[ 290 | "--display-config", 291 | rviz_config, 292 | "--ros-args", 293 | "--log-level", 294 | log_level, 295 | ], 296 | parameters=[ 297 | robot_description, 298 | robot_description_semantic, 299 | kinematics, 300 | planning_pipeline, 301 | joint_limits, 302 | {"use_sim_time": use_sim_time}, 303 | ], 304 | condition=IfCondition(enable_rviz), 305 | ), 306 | ] 307 | 308 | # Add nodes for loading controllers 309 | for controller in moveit_controller_manager_yaml["controller_names"] + [ 310 | "joint_state_broadcaster" 311 | ]: 312 | nodes.append( 313 | # controller_manager_spawner 314 | Node( 315 | package="controller_manager", 316 | executable="spawner", 317 | output="log", 318 | arguments=[controller, "--ros-args", "--log-level", log_level], 319 | parameters=[{"use_sim_time": use_sim_time}], 320 | ), 321 | ) 322 | 323 | return LaunchDescription(declared_arguments + nodes) 324 | 325 | 326 | def load_yaml(package_name: str, file_path: str): 327 | """ 328 | Load yaml configuration based on package name and file path relative to its share. 329 | """ 330 | 331 | package_path = get_package_share_directory(package_name) 332 | absolute_file_path = path.join(package_path, file_path) 333 | return parse_yaml(absolute_file_path) 334 | 335 | 336 | def parse_yaml(absolute_file_path: str): 337 | """ 338 | Parse yaml from file, given its absolute file path. 339 | """ 340 | 341 | try: 342 | with open(absolute_file_path, "r") as file: 343 | return yaml.safe_load(file) 344 | except EnvironmentError: 345 | return None 346 | 347 | 348 | def generate_declared_arguments() -> List[DeclareLaunchArgument]: 349 | """ 350 | Generate list of all launch arguments that are declared for this launch script. 351 | """ 352 | 353 | return [ 354 | # Locations of robot resources 355 | DeclareLaunchArgument( 356 | "description_package", 357 | default_value="panda_description", 358 | description="Custom package with robot description.", 359 | ), 360 | DeclareLaunchArgument( 361 | "description_filepath", 362 | default_value=path.join("urdf", "panda.urdf.xacro"), 363 | description="Path to xacro or URDF description of the robot, relative to share of `description_package`.", 364 | ), 365 | # Naming of the robot 366 | DeclareLaunchArgument( 367 | "name", 368 | default_value="panda", 369 | description="Name of the robot.", 370 | ), 371 | DeclareLaunchArgument( 372 | "prefix", 373 | default_value="panda_", 374 | description="Prefix for all robot entities. If modified, then joint names in the configuration of controllers must also be updated.", 375 | ), 376 | # Gripper 377 | DeclareLaunchArgument( 378 | "gripper", 379 | default_value="true", 380 | description="Flag to enable default gripper.", 381 | ), 382 | # Collision geometry 383 | DeclareLaunchArgument( 384 | "collision_arm", 385 | default_value="true", 386 | description="Flag to enable collision geometry for manipulator's arm.", 387 | ), 388 | DeclareLaunchArgument( 389 | "collision_gripper", 390 | default_value="true", 391 | description="Flag to enable collision geometry for manipulator's gripper (hand and fingers).", 392 | ), 393 | # Safety controller 394 | DeclareLaunchArgument( 395 | "safety_limits", 396 | default_value="true", 397 | description="Flag to enable safety limits controllers on manipulator joints.", 398 | ), 399 | DeclareLaunchArgument( 400 | "safety_position_margin", 401 | default_value="0.15", 402 | description="Lower and upper margin for position limits of all safety controllers.", 403 | ), 404 | DeclareLaunchArgument( 405 | "safety_k_position", 406 | default_value="100.0", 407 | description="Parametric k-position factor of all safety controllers.", 408 | ), 409 | DeclareLaunchArgument( 410 | "safety_k_velocity", 411 | default_value="40.0", 412 | description="Parametric k-velocity factor of all safety controllers.", 413 | ), 414 | # ROS 2 control 415 | DeclareLaunchArgument( 416 | "ros2_control", 417 | default_value="true", 418 | description="Flag to enable ros2 controllers for manipulator.", 419 | ), 420 | DeclareLaunchArgument( 421 | "ros2_control_plugin", 422 | default_value="ign", 423 | description="The ros2_control plugin that should be loaded for the manipulator ('fake', 'ign', 'real' or custom).", 424 | ), 425 | DeclareLaunchArgument( 426 | "ros2_control_command_interface", 427 | default_value="effort", 428 | description="The output control command interface provided by ros2_control ('position', 'velocity', 'effort' or certain combinations 'position,velocity').", 429 | ), 430 | # Gazebo 431 | DeclareLaunchArgument( 432 | "gazebo_preserve_fixed_joint", 433 | default_value="false", 434 | description="Flag to preserve fixed joints and prevent lumping when generating SDF for Gazebo.", 435 | ), 436 | # Servo 437 | DeclareLaunchArgument( 438 | "enable_servo", 439 | default_value="true", 440 | description="Flag to enable MoveIt2 Servo for manipulator.", 441 | ), 442 | # Miscellaneous 443 | DeclareLaunchArgument( 444 | "enable_rviz", default_value="true", description="Flag to enable RViz2." 445 | ), 446 | DeclareLaunchArgument( 447 | "rviz_config", 448 | default_value=path.join( 449 | get_package_share_directory("panda_moveit_config"), 450 | "rviz", 451 | "moveit.rviz", 452 | ), 453 | description="Path to configuration for RViz2.", 454 | ), 455 | DeclareLaunchArgument( 456 | "use_sim_time", 457 | default_value="false", 458 | description="If true, use simulated clock.", 459 | ), 460 | DeclareLaunchArgument( 461 | "log_level", 462 | default_value="warn", 463 | description="The level of logging that is applied to all ROS 2 nodes launched by this script.", 464 | ), 465 | ] 466 | -------------------------------------------------------------------------------- /panda_description/urdf/panda.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | transmission_interface/SimpleTransmission 48 | 49 | hardware_interface/EffortJointInterface 50 | 51 | 52 | 1 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | transmission_interface/SimpleTransmission 92 | 93 | hardware_interface/EffortJointInterface 94 | 95 | 96 | 1 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | transmission_interface/SimpleTransmission 136 | 137 | hardware_interface/EffortJointInterface 138 | 139 | 140 | 1 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | transmission_interface/SimpleTransmission 180 | 181 | hardware_interface/EffortJointInterface 182 | 183 | 184 | 1 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | transmission_interface/SimpleTransmission 224 | 225 | hardware_interface/EffortJointInterface 226 | 227 | 228 | 1 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | transmission_interface/SimpleTransmission 268 | 269 | hardware_interface/EffortJointInterface 270 | 271 | 272 | 1 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | transmission_interface/SimpleTransmission 312 | 313 | hardware_interface/EffortJointInterface 314 | 315 | 316 | 1 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | ign_ros2_control/IgnitionSystem 356 | 357 | 358 | 0.0 359 | 360 | 361 | 362 | 363 | 364 | 365 | -0.7853981633974483 366 | 367 | 368 | 369 | 370 | 371 | 372 | 0.0 373 | 374 | 375 | 376 | 377 | 378 | 379 | -2.356194490192345 380 | 381 | 382 | 383 | 384 | 385 | 386 | 0.0 387 | 388 | 389 | 390 | 391 | 392 | 393 | 1.5707963267948966 394 | 395 | 396 | 397 | 398 | 399 | 400 | 0.7853981633974483 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | transmission_interface/SimpleTransmission 445 | 446 | hardware_interface/EffortJointInterface 447 | 448 | 449 | 1 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | transmission_interface/SimpleTransmission 489 | 490 | hardware_interface/EffortJointInterface 491 | 492 | 493 | 1 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | ign_ros2_control/IgnitionSystem 533 | 534 | 535 | 0.0 536 | 537 | 538 | 539 | 540 | 541 | 542 | 0.0 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | /home/andrej/ws/code/repos/panda_ign_moveit2/install/share/panda_moveit_config/config/controllers_effort.yaml 552 | 553 | 554 | 555 | --------------------------------------------------------------------------------