├── .gitignore ├── .gitmodules ├── .vscode ├── c_cpp_properties.json └── settings.json ├── LICENSE ├── README.md └── src ├── robot_setup ├── dual_ur_robotiq_rs_description │ ├── CMakeLists.txt │ ├── config │ │ ├── base_frame.yaml │ │ ├── controllers.yaml │ │ ├── left_initial_positions.yaml │ │ └── right_initial_positions.yaml │ ├── gazebo │ │ ├── empty.world │ │ └── sim_env.world │ ├── include │ │ └── dual_ur_robotiq_rs_description │ │ │ └── dual_ur_robotiq_rs_node.h │ ├── launch │ │ ├── dual_ur_robotiq_rs.launch.py │ │ ├── dual_ur_robotiq_rs_planning.launch.py │ │ └── view_dual_ur_robotiq_rs.launch.py │ ├── moveit2 │ │ ├── cartesian_limits.yaml │ │ ├── joint_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit_controllers.yaml │ │ ├── ompl_planning.yaml │ │ └── planning_pipelines_config.yaml │ ├── package.xml │ ├── rviz │ │ ├── dual_ur_robotiq_rs.rviz │ │ └── view_urdf.rviz │ ├── src │ │ └── dual_ur_robotiq_rs_node.cpp │ ├── srdf │ │ ├── dual_ur_robotiq_rs.srdf.xacro │ │ └── test.srdf │ └── urdf │ │ ├── dual_ur_robotiq_rs.urdf.xacro │ │ └── test.urdf ├── ur_ft_description │ ├── CMakeLists.txt │ ├── config │ │ ├── base_frame.yaml │ │ ├── controllers.yaml │ │ └── initial_positions.yaml │ ├── gazebo │ │ └── empty.world │ ├── include │ │ └── ur_ft_description │ │ │ └── ur_ft_node.h │ ├── launch │ │ ├── remove_comments.py │ │ ├── ur_ft.launch.py │ │ └── ur_ft_planning.launch.py │ ├── moveit2 │ │ ├── cartesian_limits.yaml │ │ ├── joint_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit_controllers.yaml │ │ ├── ompl_planning.yaml │ │ └── planning_pipelines_config.yaml │ ├── package.xml │ ├── rviz │ │ └── ur_ft.rviz │ ├── src │ │ └── ur_ft_node.cpp │ ├── srdf │ │ ├── test.srdf │ │ ├── ur_ft.srdf.xacro │ │ └── ur_ft_macro.srdf.xacro │ └── urdf │ │ ├── test.urdf │ │ ├── ur_ft.urdf.xacro │ │ └── ur_ft_macro.urdf.xacro └── ur_robotiq_rs_description │ ├── CMakeLists.txt │ ├── config │ ├── base_frame.yaml │ ├── controllers.yaml │ └── initial_positions.yaml │ ├── gazebo │ └── empty.world │ ├── include │ └── ur_robotiq_rs_description │ │ └── ur_robotiq_rs_node.h │ ├── launch │ ├── ur_robotiq_rs.launch.py │ ├── ur_robotiq_rs_planning.launch.py │ └── view_ur_robotiq_rs.launch.py │ ├── moveit2 │ ├── cartesian_limits.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── moveit_controllers.yaml │ ├── ompl_planning.yaml │ └── planning_pipelines_config.yaml │ ├── package.xml │ ├── rviz │ ├── ur_robotiq_rs.rviz │ └── view_urdf.rviz │ ├── src │ └── ur_robotiq_rs_node.cpp │ ├── srdf │ ├── test.srdf │ ├── ur_robotiq_rs.srdf.xacro │ └── ur_robotiq_rs_macro.srdf.xacro │ └── urdf │ ├── realsense_d435.gazebo.xacro │ ├── realsense_d435.urdf.xacro │ ├── test.urdf │ ├── ur_robotiq_rs.urdf.xacro │ └── ur_robotiq_rs_macro.urdf.xacro └── tasks ├── dual_plan ├── CMakeLists.txt ├── config │ ├── base_frame.yaml │ ├── left_initial_positions.yaml │ └── right_initial_positions.yaml ├── gazebo │ └── sim_env.world ├── launch │ ├── bringup.launch.py │ └── dual_plan.launch.py ├── package.xml ├── rviz │ └── dual_plan.rviz └── src │ └── dual_plan.cpp ├── find_hole ├── CMakeLists.txt ├── config │ ├── base_frame.yaml │ └── initial_positions.yaml ├── gazebo │ ├── sim_env.world │ └── table_with_hole │ │ ├── meshes │ │ └── table_with_hole.stl │ │ ├── model.config │ │ └── model.sdf ├── include │ └── find_hole │ │ └── temp.h ├── launch │ ├── bringup.launch.py │ └── find_hole.launch.py ├── package.xml ├── rviz │ └── find_hole.rviz └── src │ └── find_hole.cpp └── pick_and_place ├── CMakeLists.txt ├── config ├── base_frame.yaml ├── initial_positions.yaml └── target_pose_list.yaml ├── gazebo └── sim_env.world ├── launch ├── bringup.launch.py └── pick_and_place.launch.py ├── package.xml ├── rviz └── pick_and_place.rviz └── src └── pick_and_place.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | install/ 19 | log/ 20 | 21 | # Generated by dynamic reconfigure 22 | *.cfgc 23 | /cfg/cpp/ 24 | /cfg/*.py 25 | 26 | # Ignore generated docs 27 | *.dox 28 | *.wikidoc 29 | 30 | # eclipse stuff 31 | .project 32 | .cproject 33 | 34 | # qcreator stuff 35 | CMakeLists.txt.user 36 | 37 | srv/_*.py 38 | *.pcd 39 | *.pyc 40 | qtcreator-* 41 | *.user 42 | 43 | /planning/cfg 44 | /planning/docs 45 | /planning/src 46 | 47 | *~ 48 | 49 | # Emacs 50 | .#* 51 | 52 | # Catkin custom files 53 | CATKIN_IGNORE 54 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/libs/ros2_robotiq_gripper"] 2 | path = src/libs/ros2_robotiq_gripper 3 | url = git@github.com:zitongbai/ros2_robotiq_gripper.git 4 | [submodule "src/libs/serial"] 5 | path = src/libs/serial 6 | url = git@github.com:tylerjw/serial.git 7 | branch = ros2 8 | [submodule "src/libs/gazebo-pkgs"] 9 | path = src/libs/gazebo-pkgs 10 | url = git@github.com:zitongbai/gazebo-pkgs.git 11 | branch = humble 12 | [submodule "src/libs/realsense_gazebo_plugin"] 13 | path = src/libs/realsense_gazebo_plugin 14 | url = git@github.com:zitongbai/realsense_gazebo_plugin.git 15 | branch = foxy-devel 16 | [submodule "src/libs/ros2_robotiq_ft_sensor"] 17 | path = src/libs/ros2_robotiq_ft_sensor 18 | url = git@github.com:zitongbai/ros2_robotiq_ft_sensor.git 19 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "${workspaceFolder}/**", 10 | "/opt/ros/humble/include/**", 11 | "/usr/include/**", 12 | "${workspaceFolder}/src/libs/moveit_task_constructor/core/include" 13 | ], 14 | "name": "ROS", 15 | "intelliSenseMode": "gcc-x64", 16 | "compilerPath": "/usr/bin/gcc", 17 | "cStandard": "gnu11", 18 | "cppStandard": "c++14" 19 | } 20 | ], 21 | "version": 4 22 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/xiaobaige/ros2_ws/install/moveit_task_constructor_msgs/local/lib/python3.10/dist-packages", 4 | "/home/xiaobaige/UR5e_Vision_Assemble/build/vision", 5 | "/home/xiaobaige/UR5e_Vision_Assemble/install/vision/lib/python3.10/site-packages", 6 | "/home/xiaobaige/UR5e_Vision_Assemble/install/ur_moveit_config/local/lib/python3.10/dist-packages", 7 | "/home/xiaobaige/UR5e_Vision_Assemble/install/realsense2_camera_msgs/local/lib/python3.10/dist-packages", 8 | "/opt/ros/humble/lib/python3.10/site-packages", 9 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 10 | ], 11 | "python.analysis.extraPaths": [ 12 | "/home/xiaobaige/ros2_ws/install/moveit_task_constructor_msgs/local/lib/python3.10/dist-packages", 13 | "/home/xiaobaige/UR5e_Vision_Assemble/build/vision", 14 | "/home/xiaobaige/UR5e_Vision_Assemble/install/vision/lib/python3.10/site-packages", 15 | "/home/xiaobaige/UR5e_Vision_Assemble/install/ur_moveit_config/local/lib/python3.10/dist-packages", 16 | "/home/xiaobaige/UR5e_Vision_Assemble/install/realsense2_camera_msgs/local/lib/python3.10/dist-packages", 17 | "/opt/ros/humble/lib/python3.10/site-packages", 18 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 19 | ], 20 | "files.associations": { 21 | "cctype": "cpp", 22 | "clocale": "cpp", 23 | "cmath": "cpp", 24 | "csignal": "cpp", 25 | "cstdarg": "cpp", 26 | "cstddef": "cpp", 27 | "cstdio": "cpp", 28 | "cstdlib": "cpp", 29 | "cstring": "cpp", 30 | "ctime": "cpp", 31 | "cwchar": "cpp", 32 | "cwctype": "cpp", 33 | "any": "cpp", 34 | "array": "cpp", 35 | "atomic": "cpp", 36 | "strstream": "cpp", 37 | "bit": "cpp", 38 | "*.tcc": "cpp", 39 | "bitset": "cpp", 40 | "chrono": "cpp", 41 | "codecvt": "cpp", 42 | "compare": "cpp", 43 | "complex": "cpp", 44 | "concepts": "cpp", 45 | "condition_variable": "cpp", 46 | "coroutine": "cpp", 47 | "cstdint": "cpp", 48 | "deque": "cpp", 49 | "forward_list": "cpp", 50 | "list": "cpp", 51 | "map": "cpp", 52 | "set": "cpp", 53 | "string": "cpp", 54 | "unordered_map": "cpp", 55 | "unordered_set": "cpp", 56 | "vector": "cpp", 57 | "exception": "cpp", 58 | "algorithm": "cpp", 59 | "functional": "cpp", 60 | "iterator": "cpp", 61 | "memory": "cpp", 62 | "memory_resource": "cpp", 63 | "numeric": "cpp", 64 | "optional": "cpp", 65 | "random": "cpp", 66 | "ratio": "cpp", 67 | "regex": "cpp", 68 | "source_location": "cpp", 69 | "string_view": "cpp", 70 | "system_error": "cpp", 71 | "tuple": "cpp", 72 | "type_traits": "cpp", 73 | "utility": "cpp", 74 | "fstream": "cpp", 75 | "future": "cpp", 76 | "initializer_list": "cpp", 77 | "iomanip": "cpp", 78 | "iosfwd": "cpp", 79 | "iostream": "cpp", 80 | "istream": "cpp", 81 | "limits": "cpp", 82 | "mutex": "cpp", 83 | "new": "cpp", 84 | "numbers": "cpp", 85 | "ostream": "cpp", 86 | "semaphore": "cpp", 87 | "shared_mutex": "cpp", 88 | "sstream": "cpp", 89 | "stdexcept": "cpp", 90 | "stop_token": "cpp", 91 | "streambuf": "cpp", 92 | "thread": "cpp", 93 | "cfenv": "cpp", 94 | "cinttypes": "cpp", 95 | "typeindex": "cpp", 96 | "typeinfo": "cpp", 97 | "variant": "cpp", 98 | "__nullptr": "cpp", 99 | "*.ipp": "cpp", 100 | "__functional_base": "cpp", 101 | "locale": "cpp", 102 | "__node_handle": "cpp", 103 | "hash_map": "cpp", 104 | "hash_set": "cpp", 105 | "valarray": "cpp" 106 | } 107 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # THIS REPO IS STILL UNDER DEVEL AND UNSTABLE 2 | # 项目仍在开发中，不保证稳定性 3 | # ur_project_ros2 4 | The application of ur robots, robotiq gripper, realsense camera, etc. 5 | 6 | To find more infomation (installation, usage), visit [wiki](https://github.com/zitongbai/ur_project_ros2/wiki) 7 | 8 | # Structure 9 | 10 | 11 | * `robot_setup`: robot description, moveit config, simulation env setup, etc. 12 | * `ur_robotiq_rs`: ur manipulator with a robotiq gripper and a realsense camera in its end effector 13 | * `dual_ur_robotiq_rs`: dual ur_robotiq_rs 14 | * `vision`: vision tools to detect objects, estimate pose, etc. 15 | * `tasks`: 16 | * `pick_and_place` use moveit to plan a path to the given pose, grasp the obj. 17 | * `find_hole` use ft sensor. 18 | * `dual_plan` plan and execute dual arms at the same time. 19 | * `libs`: third-party tools, plugins, etc. 20 | 21 | # Temp note 22 | 23 | create new task package: 24 | ```shell 25 | ros2 pkg create ${task_name} --build-type ament_cmake --node-name ${task_name} --dependencies rclcpp moveit_core moveit_ros_planning_interface moveit_common moveit_ros_planning tf2 rclcpp_action rclcpp_components control_msgs ${robot_pkg} 26 | ``` -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(dual_ur_robotiq_rs_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(moveit_common REQUIRED) 15 | find_package(moveit_core REQUIRED) 16 | find_package(moveit_ros_planning_interface REQUIRED) 17 | find_package(moveit_ros_planning REQUIRED) 18 | find_package(moveit_visual_tools REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(rclcpp_action REQUIRED) 21 | find_package(control_msgs REQUIRED) 22 | 23 | add_library(dual_ur_robotiq_rs_node src/dual_ur_robotiq_rs_node.cpp) 24 | target_compile_features(dual_ur_robotiq_rs_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 25 | target_include_directories(dual_ur_robotiq_rs_node PUBLIC 26 | $ 27 | $) 28 | ament_target_dependencies( 29 | dual_ur_robotiq_rs_node 30 | rclcpp moveit_ros_planning_interface tf2 31 | moveit_core moveit_ros_planning control_msgs 32 | ) 33 | 34 | # Causes the visibility macros to use dllexport rather than dllimport, 35 | # which is appropriate when building the dll but not consuming it. 36 | target_compile_definitions(dual_ur_robotiq_rs_node PRIVATE "TEST_CREATING_BUILDING_LIBRARY") 37 | 38 | install( 39 | DIRECTORY include/ 40 | DESTINATION include 41 | ) 42 | install( 43 | TARGETS dual_ur_robotiq_rs_node 44 | EXPORT export_${PROJECT_NAME} 45 | ARCHIVE DESTINATION lib 46 | LIBRARY DESTINATION lib 47 | RUNTIME DESTINATION bin 48 | ) 49 | 50 | 51 | install( 52 | DIRECTORY 53 | config 54 | launch 55 | rviz 56 | urdf 57 | gazebo 58 | srdf 59 | moveit2 60 | # src 61 | DESTINATION 62 | share/${PROJECT_NAME} 63 | ) 64 | 65 | if(BUILD_TESTING) 66 | find_package(ament_lint_auto REQUIRED) 67 | # the following line skips the linter which checks for copyrights 68 | # comment the line when a copyright and license is added to all source files 69 | set(ament_cmake_copyright_FOUND TRUE) 70 | # the following line skips cpplint (only works in a git repo) 71 | # comment the line when this package is in a git repo and when 72 | # a copyright and license is added to all source files 73 | set(ament_cmake_cpplint_FOUND TRUE) 74 | ament_lint_auto_find_test_dependencies() 75 | endif() 76 | 77 | 78 | ament_export_include_directories( 79 | include 80 | ) 81 | ament_export_libraries( 82 | dual_ur_robotiq_rs_node 83 | ) 84 | ament_export_targets( 85 | export_${PROJECT_NAME} 86 | ) 87 | 88 | ament_package() 89 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | # it will be parsed in dual_ur_robotiq_rs.urdf.xacro 3 | left_base_frame: 4 | x: 0.0 5 | y: 0.5 6 | z: 1.02 7 | roll: 0.0 8 | pitch: 0.0 9 | yaw: 0.0 10 | right_base_frame: 11 | x: 0.0 12 | y: -0.5 13 | z: 1.02 14 | roll: 0.0 15 | pitch: 0.0 16 | yaw: 0.0 -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | # ros2_control uses this configuration for controller management 2 | # This file is only for simulation 3 | controller_manager: 4 | ros__parameters: 5 | 6 | update_rate: 100 # Hz 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | left_ur_arm_controller: 12 | type: joint_trajectory_controller/JointTrajectoryController 13 | 14 | left_forward_velocity_controller: 15 | type: velocity_controllers/JointGroupVelocityController 16 | 17 | left_forward_position_controller: 18 | type: position_controllers/JointGroupPositionController 19 | 20 | left_gripper_controller: 21 | type: position_controllers/GripperActionController 22 | 23 | right_ur_arm_controller: 24 | type: joint_trajectory_controller/JointTrajectoryController 25 | 26 | right_forward_velocity_controller: 27 | type: velocity_controllers/JointGroupVelocityController 28 | 29 | right_forward_position_controller: 30 | type: position_controllers/JointGroupPositionController 31 | 32 | right_gripper_controller: 33 | type: position_controllers/GripperActionController 34 | 35 | # ----------------------------------------------------------------- 36 | left_ur_arm_controller: 37 | ros__parameters: 38 | joints: 39 | - left_shoulder_pan_joint 40 | - left_shoulder_lift_joint 41 | - left_elbow_joint 42 | - left_wrist_1_joint 43 | - left_wrist_2_joint 44 | - left_wrist_3_joint 45 | command_interfaces: 46 | - position 47 | state_interfaces: 48 | - position 49 | - velocity 50 | state_publish_rate: 100.0 51 | action_monitor_rate: 20.0 52 | allow_partial_joints_goal: false 53 | constraints: 54 | stopped_velocity_tolerance: 0.2 55 | goal_time: 0.0 56 | left_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 57 | left_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 58 | left_elbow_joint: { trajectory: 0.2, goal: 0.1 } 59 | left_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 60 | left_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 61 | left_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 62 | 63 | left_forward_velocity_controller: 64 | ros__parameters: 65 | joints: 66 | - left_shoulder_pan_joint 67 | - left_shoulder_lift_joint 68 | - left_elbow_joint 69 | - left_wrist_1_joint 70 | - left_wrist_2_joint 71 | - left_wrist_3_joint 72 | interface_name: velocity 73 | 74 | left_forward_position_controller: 75 | ros__parameters: 76 | joints: 77 | - left_shoulder_pan_joint 78 | - left_shoulder_lift_joint 79 | - left_elbow_joint 80 | - left_wrist_1_joint 81 | - left_wrist_2_joint 82 | - left_wrist_3_joint 83 | 84 | left_gripper_controller: 85 | ros__parameters: 86 | joint: left_robotiq_85_left_knuckle_joint 87 | 88 | # ----------------------------------------------------------------- 89 | right_ur_arm_controller: 90 | ros__parameters: 91 | joints: 92 | - right_shoulder_pan_joint 93 | - right_shoulder_lift_joint 94 | - right_elbow_joint 95 | - right_wrist_1_joint 96 | - right_wrist_2_joint 97 | - right_wrist_3_joint 98 | command_interfaces: 99 | - position 100 | state_interfaces: 101 | - position 102 | - velocity 103 | state_publish_rate: 100.0 104 | action_monitor_rate: 20.0 105 | allow_partial_joints_goal: false 106 | constraints: 107 | stopped_velocity_tolerance: 0.2 108 | goal_time: 0.0 109 | right_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 110 | right_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 111 | right_elbow_joint: { trajectory: 0.2, goal: 0.1 } 112 | right_wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 113 | right_wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 114 | right_wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 115 | 116 | right_forward_velocity_controller: 117 | ros__parameters: 118 | joints: 119 | - right_shoulder_pan_joint 120 | - right_shoulder_lift_joint 121 | - right_elbow_joint 122 | - right_wrist_1_joint 123 | - right_wrist_2_joint 124 | - right_wrist_3_joint 125 | interface_name: velocity 126 | 127 | right_forward_position_controller: 128 | ros__parameters: 129 | joints: 130 | - right_shoulder_pan_joint 131 | - right_shoulder_lift_joint 132 | - right_elbow_joint 133 | - right_wrist_1_joint 134 | - right_wrist_2_joint 135 | - right_wrist_3_joint 136 | 137 | right_gripper_controller: 138 | ros__parameters: 139 | joint: right_robotiq_85_left_knuckle_joint -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/config/left_initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/config/right_initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/gazebo/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 0 0 -9.8 11 | 12 | 13 | 14 | 15 | model://sun 16 | 17 | 18 | 19 | 20 | 21 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 22 | orbit 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/include/dual_ur_robotiq_rs_description/dual_ur_robotiq_rs_node.h: -------------------------------------------------------------------------------- 1 | #ifndef DUAL_UR_ROBOTIQ_RS_DESCRIPTION_NODE_H_ 2 | #define DUAL_UR_ROBOTIQ_RS_DESCRIPTION_NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class DualUrRobotiqRsNode : public rclcpp::Node{ 20 | public: 21 | using GripperCommand = control_msgs::action::GripperCommand; 22 | using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle; 23 | 24 | explicit DualUrRobotiqRsNode(const std::string & node_name, 25 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); 26 | 27 | void init(); 28 | 29 | bool go_to_ready_position(); 30 | 31 | bool plan_and_execute( 32 | const geometry_msgs::msg::Pose & left_target_pose, 33 | const geometry_msgs::msg::Pose & right_target_pose, 34 | const std::string & left_reference_frame = "left_base_link", 35 | const std::string & right_reference_frame = "right_base_link", 36 | const std::string & left_end_effector_link = "", 37 | const std::string & right_end_effector_link = ""); 38 | 39 | bool plan_single_arm( 40 | bool left, 41 | const geometry_msgs::msg::Pose & target_pose, 42 | const std::string & reference_frame, 43 | const std::string & end_effector_link, 44 | moveit::planning_interface::MoveGroupInterface::Plan & plan 45 | ); 46 | 47 | void add_collision_objects(std::vector & collision_objects){ 48 | planning_scene_interface_.addCollisionObjects(collision_objects); 49 | } 50 | 51 | // Helper functions 52 | 53 | /** 54 | * @brief Set the both planning group name object 55 | * 56 | * @param planning_group_name 57 | * @return DualUrRobotiqRsNode& 58 | */ 59 | DualUrRobotiqRsNode & set_both_planning_group_name(const std::string & planning_group_name){ 60 | both_planning_group_name_ = planning_group_name; 61 | return *this; 62 | } 63 | /** 64 | * @brief Set the left planning group name object 65 | * 66 | * @param planning_group_name 67 | * @return DualUrRobotiqRsNode& 68 | */ 69 | DualUrRobotiqRsNode & set_left_planning_group_name(const std::string & planning_group_name){ 70 | left_planning_group_name_ = planning_group_name; 71 | return *this; 72 | } 73 | /** 74 | * @brief Set the right planning group name object 75 | * 76 | * @param planning_group_name 77 | * @return DualUrRobotiqRsNode& 78 | */ 79 | DualUrRobotiqRsNode & set_right_planning_group_name(const std::string & planning_group_name){ 80 | right_planning_group_name_ = planning_group_name; 81 | return *this; 82 | } 83 | 84 | moveit::planning_interface::MoveGroupInterfacePtr get_both_move_group(){ 85 | return both_move_group_; 86 | } 87 | moveit::planning_interface::MoveGroupInterfacePtr get_left_move_group(){ 88 | return left_move_group_; 89 | } 90 | moveit::planning_interface::MoveGroupInterfacePtr get_right_move_group(){ 91 | return right_move_group_; 92 | } 93 | 94 | 95 | private: 96 | // moveit 97 | std::string both_planning_group_name_ = "both_manipulators"; 98 | std::string left_planning_group_name_ = "left_ur_manipulator"; 99 | std::string right_planning_group_name_ = "right_ur_manipulator"; 100 | moveit::planning_interface::MoveGroupInterfacePtr both_move_group_; 101 | moveit::planning_interface::MoveGroupInterfacePtr left_move_group_; 102 | moveit::planning_interface::MoveGroupInterfacePtr right_move_group_; 103 | moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; 104 | 105 | std::vector left_target_joint_; 106 | std::vector right_target_joint_; 107 | std::vector both_target_joint_; 108 | 109 | // robotiq gripper 110 | std::string left_gripper_action_name_ = "/left_gripper_controller/gripper_cmd"; 111 | std::string right_gripper_action_name_ = "/right_gripper_controller/gripper_cmd"; 112 | rclcpp_action::Client::SharedPtr left_gripper_action_client_; 113 | rclcpp_action::Client::SharedPtr right_gripper_action_client_; 114 | void goal_response_callback(const GoalHandleGripperCommand::SharedPtr & goal_handle); 115 | void feedback_callback(GoalHandleGripperCommand::SharedPtr, const std::shared_ptr feedback); 116 | void result_callback(const GoalHandleGripperCommand::WrappedResult & result); 117 | rclcpp_action::Client::SendGoalOptions send_goal_options_; 118 | 119 | }; 120 | 121 | 122 | 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/launch/view_dual_ur_robotiq_rs.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch.substitutions import ( 3 | Command, 4 | FindExecutable, 5 | LaunchConfiguration, 6 | PathJoinSubstitution, 7 | ) 8 | import launch_ros 9 | import os 10 | 11 | 12 | def generate_launch_description(): 13 | pkg_share = launch_ros.substitutions.FindPackageShare( 14 | package="dual_ur_robotiq_rs_description" 15 | ).find("dual_ur_robotiq_rs_description") 16 | default_model_path = os.path.join( 17 | pkg_share, "urdf", "dual_ur_robotiq_rs.urdf.xacro" 18 | ) 19 | default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz") 20 | 21 | args = [] 22 | args.append( 23 | launch.actions.DeclareLaunchArgument( 24 | name="model", 25 | default_value=default_model_path, 26 | description="Absolute path to robot URDF file", 27 | ) 28 | ) 29 | args.append( 30 | launch.actions.DeclareLaunchArgument( 31 | name="rvizconfig", 32 | default_value=default_rviz_config_path, 33 | description="Absolute path to rviz config file", 34 | ) 35 | ) 36 | 37 | robot_description_content = Command( 38 | [ 39 | PathJoinSubstitution([FindExecutable(name="xacro")]), 40 | " ", 41 | LaunchConfiguration("model"), 42 | " " 43 | "ur_type:=", 44 | "ur5e", 45 | " ", 46 | "sim_gazebo:=", 47 | "false" 48 | ] 49 | ) 50 | robot_description_param = { 51 | "robot_description": launch_ros.descriptions.ParameterValue( 52 | robot_description_content, value_type=str 53 | ) 54 | } 55 | 56 | robot_state_publisher_node = launch_ros.actions.Node( 57 | package="robot_state_publisher", 58 | executable="robot_state_publisher", 59 | parameters=[robot_description_param], 60 | ) 61 | 62 | joint_state_publisher_node = launch_ros.actions.Node( 63 | package="joint_state_publisher_gui", 64 | executable="joint_state_publisher_gui", 65 | ) 66 | 67 | rviz_node = launch_ros.actions.Node( 68 | package="rviz2", 69 | executable="rviz2", 70 | name="rviz2", 71 | output="screen", 72 | arguments=["-d", LaunchConfiguration("rvizconfig")], 73 | ) 74 | 75 | nodes = [ 76 | robot_state_publisher_node, 77 | joint_state_publisher_node, 78 | rviz_node, 79 | ] 80 | 81 | return launch.LaunchDescription(args + nodes) 82 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_planning: 4 | cartesian_limits: 5 | max_trans_vel: 1.0 6 | max_trans_acc: 2.25 7 | max_trans_dec: 1.0 8 | max_rot_vel: 100.57 9 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # These limits are used by MoveIt and augment/override the definitions in ur_description. 2 | # 3 | # While the robot does not inherently have any limits on joint accelerations (only on torques), 4 | # MoveIt needs them for time parametrization. They were chosen conservatively to work in most use 5 | # cases. For specific applications, higher values might lead to better execution performance. 6 | 7 | /**: 8 | ros__parameters: 9 | robot_description_planning: 10 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 11 | 12 | # For beginners, we downscale velocity and acceleration limits. 13 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 14 | default_velocity_scaling_factor: 0.1 15 | default_acceleration_scaling_factor: 0.1 16 | 17 | # As MoveIt does not support jerk limits, the acceleration limits provided here are the highest values that guarantee 18 | # that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel 19 | # to max accel in 1 ms) the acceleration limits are the ones that satisfy 20 | # max_jerk = (max_acceleration - min_acceleration) / 0.001 21 | 22 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 23 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 24 | joint_limits: 25 | left_shoulder_pan_joint: 26 | has_acceleration_limits: true 27 | max_acceleration: 5.0 28 | left_shoulder_lift_joint: 29 | has_acceleration_limits: true 30 | max_acceleration: 5.0 31 | left_elbow_joint: 32 | has_acceleration_limits: true 33 | max_acceleration: 5.0 34 | left_wrist_1_joint: 35 | has_acceleration_limits: true 36 | max_acceleration: 5.0 37 | left_wrist_2_joint: 38 | has_acceleration_limits: true 39 | max_acceleration: 5.0 40 | left_wrist_3_joint: 41 | has_acceleration_limits: true 42 | max_acceleration: 5.0 43 | left_robotiq_85_left_knuckle_joint: 44 | has_velocity_limits: true 45 | max_velocity: 0.5 46 | has_acceleration_limits: false 47 | max_acceleration: 0 48 | right_shoulder_pan_joint: 49 | has_acceleration_limits: true 50 | max_acceleration: 5.0 51 | right_shoulder_lift_joint: 52 | has_acceleration_limits: true 53 | max_acceleration: 5.0 54 | right_elbow_joint: 55 | has_acceleration_limits: true 56 | max_acceleration: 5.0 57 | right_wrist_1_joint: 58 | has_acceleration_limits: true 59 | max_acceleration: 5.0 60 | right_wrist_2_joint: 61 | has_acceleration_limits: true 62 | max_acceleration: 5.0 63 | right_wrist_3_joint: 64 | has_acceleration_limits: true 65 | max_acceleration: 5.0 66 | right_robotiq_85_left_knuckle_joint: 67 | has_velocity_limits: true 68 | max_velocity: 0.5 69 | has_acceleration_limits: false 70 | max_acceleration: 0 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/kinematics.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_kinematics: 4 | left_ur_manipulator: 5 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 6 | kinematics_solver_search_resolution: 0.005 7 | kinematics_solver_timeout: 0.005 8 | kinematics_solver_attempts: 3 9 | right_ur_manipulator: 10 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 11 | kinematics_solver_search_resolution: 0.005 12 | kinematics_solver_timeout: 0.005 13 | kinematics_solver_attempts: 3 14 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | moveit_controller_manager: 4 | moveit_simple_controller_manager/MoveItSimpleControllerManager 5 | 6 | moveit_simple_controller_manager: 7 | controller_names: 8 | - left_ur_arm_controller 9 | - left_gripper_controller 10 | - right_ur_arm_controller 11 | - right_gripper_controller 12 | 13 | left_ur_arm_controller: 14 | type: FollowJointTrajectory 15 | action_ns: follow_joint_trajectory 16 | default: true 17 | joints: 18 | - left_shoulder_pan_joint 19 | - left_shoulder_lift_joint 20 | - left_elbow_joint 21 | - left_wrist_1_joint 22 | - left_wrist_2_joint 23 | - left_wrist_3_joint 24 | action_ns: follow_joint_trajectory 25 | default: true 26 | left_gripper_controller: 27 | type: GripperCommand 28 | joints: 29 | - left_robotiq_85_left_knuckle_joint 30 | action_ns: gripper_cmd 31 | default: true 32 | 33 | right_ur_arm_controller: 34 | type: FollowJointTrajectory 35 | action_ns: follow_joint_trajectory 36 | default: true 37 | joints: 38 | - right_shoulder_pan_joint 39 | - right_shoulder_lift_joint 40 | - right_elbow_joint 41 | - right_wrist_1_joint 42 | - right_wrist_2_joint 43 | - right_wrist_3_joint 44 | action_ns: follow_joint_trajectory 45 | default: true 46 | right_gripper_controller: 47 | type: GripperCommand 48 | joints: 49 | - right_robotiq_85_left_knuckle_joint 50 | action_ns: gripper_cmd 51 | default: true 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | ompl: 4 | planner_configs: 5 | SBLkConfigDefault: 6 | type: geometric::SBL 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 8 | ESTkConfigDefault: 9 | type: geometric::EST 10 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 11 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 12 | LBKPIECEkConfigDefault: 13 | type: geometric::LBKPIECE 14 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 15 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 16 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 17 | BKPIECEkConfigDefault: 18 | type: geometric::BKPIECE 19 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 20 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 21 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 22 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 23 | KPIECEkConfigDefault: 24 | type: geometric::KPIECE 25 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 26 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 27 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 28 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 29 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 30 | RRTkConfigDefault: 31 | type: geometric::RRT 32 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 33 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 34 | RRTConnectkConfigDefault: 35 | type: geometric::RRTConnect 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | RRTstarkConfigDefault: 38 | type: geometric::RRTstar 39 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 40 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 41 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 42 | TRRTkConfigDefault: 43 | type: geometric::TRRT 44 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 45 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 46 | max_states_failed: 10 # when to start increasing temp. default: 10 47 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 48 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 49 | init_temperature: 10e-6 # initial temperature. default: 10e-6 50 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 51 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 52 | k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() 53 | PRMkConfigDefault: 54 | type: geometric::PRM 55 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 56 | PRMstarkConfigDefault: 57 | type: geometric::PRMstar 58 | left_ur_manipulator: 59 | planner_configs: 60 | - SBLkConfigDefault 61 | - ESTkConfigDefault 62 | - LBKPIECEkConfigDefault 63 | - BKPIECEkConfigDefault 64 | - KPIECEkConfigDefault 65 | - RRTkConfigDefault 66 | - RRTConnectkConfigDefault 67 | - RRTstarkConfigDefault 68 | - TRRTkConfigDefault 69 | - PRMkConfigDefault 70 | - PRMstarkConfigDefault 71 | ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE 72 | projection_evaluator: joints(left_shoulder_pan_joint,left_shoulder_lift_joint) 73 | longest_valid_segment_fraction: 0.01 74 | right_ur_manipulator: 75 | planner_configs: 76 | - SBLkConfigDefault 77 | - ESTkConfigDefault 78 | - LBKPIECEkConfigDefault 79 | - BKPIECEkConfigDefault 80 | - KPIECEkConfigDefault 81 | - RRTkConfigDefault 82 | - RRTConnectkConfigDefault 83 | - RRTstarkConfigDefault 84 | - TRRTkConfigDefault 85 | - PRMkConfigDefault 86 | - PRMstarkConfigDefault 87 | ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE 88 | projection_evaluator: joints(right_shoulder_pan_joint,right_shoulder_lift_joint) 89 | longest_valid_segment_fraction: 0.01 90 | both_manipulators: 91 | planner_configs: 92 | - SBLkConfigDefault 93 | - ESTkConfigDefault 94 | - LBKPIECEkConfigDefault 95 | - BKPIECEkConfigDefault 96 | - KPIECEkConfigDefault 97 | - RRTkConfigDefault 98 | - RRTConnectkConfigDefault 99 | - RRTstarkConfigDefault 100 | - TRRTkConfigDefault 101 | - PRMkConfigDefault 102 | - PRMstarkConfigDefault 103 | ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE 104 | # projection_evaluator: joints(right_shoulder_pan_joint,right_shoulder_lift_joint) 105 | longest_valid_segment_fraction: 0.01 106 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/moveit2/planning_pipelines_config.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | default_planning_pipeline: ompl 4 | planning_pipelines: [pilz, ompl] 5 | pilz: 6 | planning_plugin: pilz_industrial_motion_planner/CommandPlanner 7 | request_adapters: "" 8 | start_state_max_bounds_error: 0.1 9 | ompl: 10 | planning_plugin: ompl_interface/OMPLPlanner 11 | request_adapters: 12 | default_planner_request_adapters/AddTimeOptimalParameterization 13 | default_planner_request_adapters/FixWorkspaceBounds 14 | default_planner_request_adapters/FixStartStateBounds 15 | default_planner_request_adapters/FixStartStateCollision 16 | default_planner_request_adapters/FixStartStatePathConstraints 17 | start_state_max_bounds_error: 0.1 18 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dual_ur_robotiq_rs_description 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | moveit_core 14 | moveit_ros_planning_interface 15 | moveit_common 16 | moveit_ros_planning 17 | tf2 18 | rclcpp_action 19 | rclcpp_components 20 | control_msgs 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/rviz/dual_ur_robotiq_rs.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /MotionPlanning1 9 | Splitter Ratio: 0.6264705657958984 10 | Tree Height: 789 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 | 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: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Acceleration_Scaling_Factor: 0.1 51 | Class: moveit_rviz_plugin/MotionPlanning 52 | Enabled: true 53 | Move Group Namespace: "" 54 | MoveIt_Allow_Approximate_IK: false 55 | MoveIt_Allow_External_Program: false 56 | MoveIt_Allow_Replanning: false 57 | MoveIt_Allow_Sensor_Positioning: false 58 | MoveIt_Planning_Attempts: 10 59 | MoveIt_Planning_Time: 5 60 | MoveIt_Use_Cartesian_Path: false 61 | MoveIt_Use_Constraint_Aware_IK: false 62 | MoveIt_Workspace: 63 | Center: 64 | X: 0 65 | Y: 0 66 | Z: 0 67 | Size: 68 | X: 2 69 | Y: 2 70 | Z: 2 71 | Name: MotionPlanning 72 | Planned Path: 73 | Color Enabled: false 74 | Interrupt Display: false 75 | Links: 76 | All Links Enabled: true 77 | Expand Joint Details: false 78 | Expand Link Details: false 79 | Expand Tree: false 80 | Link Tree Style: "" 81 | Loop Animation: false 82 | Robot Alpha: 0.5 83 | Robot Color: 150; 50; 150 84 | Show Robot Collision: false 85 | Show Robot Visual: true 86 | Show Trail: false 87 | State Display Time: 3x 88 | Trail Step Size: 1 89 | Trajectory Topic: /display_planned_path 90 | Use Sim Time: false 91 | Planning Metrics: 92 | Payload: 1 93 | Show Joint Torques: false 94 | Show Manipulability: false 95 | Show Manipulability Index: false 96 | Show Weight Limit: false 97 | TextHeight: 0.07999999821186066 98 | Planning Request: 99 | Colliding Link Color: 255; 0; 0 100 | Goal State Alpha: 1 101 | Goal State Color: 250; 128; 0 102 | Interactive Marker Size: 0 103 | Joint Violation Color: 255; 0; 255 104 | Planning Group: "" 105 | Query Goal State: true 106 | Query Start State: false 107 | Show Workspace: false 108 | Start State Alpha: 1 109 | Start State Color: 0; 255; 0 110 | Planning Scene Topic: /monitored_planning_scene 111 | Robot Description: robot_description 112 | Scene Geometry: 113 | Scene Alpha: 0.8999999761581421 114 | Scene Color: 50; 230; 50 115 | Scene Display Time: 0.009999999776482582 116 | Show Scene Geometry: true 117 | Voxel Coloring: Z-Axis 118 | Voxel Rendering: Occupied Voxels 119 | Scene Robot: 120 | Attached Body Color: 150; 50; 150 121 | Links: 122 | All Links Enabled: true 123 | Expand Joint Details: false 124 | Expand Link Details: false 125 | Expand Tree: false 126 | Link Tree Style: "" 127 | Robot Alpha: 1 128 | Show Robot Collision: false 129 | Show Robot Visual: true 130 | Value: true 131 | Velocity_Scaling_Factor: 0.1 132 | Enabled: true 133 | Global Options: 134 | Background Color: 48; 48; 48 135 | Fixed Frame: world 136 | Frame Rate: 30 137 | Name: root 138 | Tools: 139 | - Class: rviz_default_plugins/Interact 140 | Hide Inactive Objects: true 141 | - Class: rviz_default_plugins/MoveCamera 142 | - Class: rviz_default_plugins/Select 143 | - Class: rviz_default_plugins/FocusCamera 144 | - Class: rviz_default_plugins/Measure 145 | Line color: 128; 128; 0 146 | - Class: rviz_default_plugins/SetInitialPose 147 | Covariance x: 0.25 148 | Covariance y: 0.25 149 | Covariance yaw: 0.06853891909122467 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /initialpose 156 | - Class: rviz_default_plugins/SetGoal 157 | Topic: 158 | Depth: 5 159 | Durability Policy: Volatile 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /goal_pose 163 | - Class: rviz_default_plugins/PublishPoint 164 | Single click: true 165 | Topic: 166 | Depth: 5 167 | Durability Policy: Volatile 168 | History Policy: Keep Last 169 | Reliability Policy: Reliable 170 | Value: /clicked_point 171 | Transformation: 172 | Current: 173 | Class: rviz_default_plugins/TF 174 | Value: true 175 | Views: 176 | Current: 177 | Class: rviz_default_plugins/Orbit 178 | Distance: 14.638081550598145 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.05999999865889549 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Focal Point: 185 | X: -0.09681572020053864 186 | Y: -0.10843408107757568 187 | Z: 0.1451336145401001 188 | Focal Shape Fixed Size: true 189 | Focal Shape Size: 0.05000000074505806 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.009999999776482582 193 | Pitch: 0.785398006439209 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 0.785398006439209 197 | Saved: ~ 198 | Window Geometry: 199 | "": 200 | collapsed: false 201 | " - Trajectory Slider": 202 | collapsed: false 203 | Displays: 204 | collapsed: false 205 | Height: 2002 206 | Hide Left Dock: false 207 | Hide Right Dock: true 208 | QMainWindow State: 000000ff00000000fd0000000400000000000003a3000006d0fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000040d0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffbffffffff0100000487000002b7000002b700ffffff000000010000015f000006d0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000006d00000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006620000005afc0100000002fb0000000800540069006d00650100000000000006620000058100fffffffb0000000800540069006d00650100000000000004500000000000000000000002b3000006d000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 209 | Selection: 210 | collapsed: false 211 | Time: 212 | collapsed: false 213 | Tool Properties: 214 | collapsed: false 215 | Views: 216 | collapsed: true 217 | Width: 1634 218 | X: 1774 219 | Y: 54 220 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/rviz/view_urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | Splitter Ratio: 0.6264705657958984 9 | Tree Height: 555 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 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz_default_plugins/RobotModel 51 | Collision Enabled: false 52 | Description File: "" 53 | Description Source: Topic 54 | Description Topic: 55 | Depth: 5 56 | Durability Policy: Volatile 57 | History Policy: Keep Last 58 | Reliability Policy: Reliable 59 | Value: robot_description 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | arm_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | base_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | dummy_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | end_effector_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | forearm_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | gripper_base_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | left_finger_dist_link: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | left_finger_prox_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | lower_wrist_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | right_finger_dist_link: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | right_finger_prox_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | shoulder_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | table: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | tool_frame: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | upper_wrist_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | world: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Mass Properties: 144 | Inertia: false 145 | Mass: false 146 | Name: RobotModel 147 | TF Prefix: "" 148 | Update Interval: 0 149 | Value: true 150 | Visual Enabled: true 151 | Enabled: true 152 | Global Options: 153 | Background Color: 48; 48; 48 154 | Fixed Frame: world 155 | Frame Rate: 30 156 | Name: root 157 | Tools: 158 | - Class: rviz_default_plugins/Interact 159 | Hide Inactive Objects: true 160 | - Class: rviz_default_plugins/MoveCamera 161 | - Class: rviz_default_plugins/Select 162 | - Class: rviz_default_plugins/FocusCamera 163 | - Class: rviz_default_plugins/Measure 164 | Line color: 128; 128; 0 165 | - Class: rviz_default_plugins/SetInitialPose 166 | Covariance x: 0.25 167 | Covariance y: 0.25 168 | Covariance yaw: 0.06853891909122467 169 | Topic: 170 | Depth: 5 171 | Durability Policy: Volatile 172 | History Policy: Keep Last 173 | Reliability Policy: Reliable 174 | Value: /initialpose 175 | - Class: rviz_default_plugins/SetGoal 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /goal_pose 182 | - Class: rviz_default_plugins/PublishPoint 183 | Single click: true 184 | Topic: 185 | Depth: 5 186 | Durability Policy: Volatile 187 | History Policy: Keep Last 188 | Reliability Policy: Reliable 189 | Value: /clicked_point 190 | Transformation: 191 | Current: 192 | Class: rviz_default_plugins/TF 193 | Value: true 194 | Views: 195 | Current: 196 | Class: rviz_default_plugins/Orbit 197 | Distance: 2.1567115783691406 198 | Enable Stereo Rendering: 199 | Stereo Eye Separation: 0.05999999865889549 200 | Stereo Focal Distance: 1 201 | Swap Stereo Eyes: false 202 | Value: false 203 | Focal Point: 204 | X: -0.09681572020053864 205 | Y: -0.10843408107757568 206 | Z: 0.1451336145401001 207 | Focal Shape Fixed Size: true 208 | Focal Shape Size: 0.05000000074505806 209 | Invert Z Axis: false 210 | Name: Current View 211 | Near Clip Distance: 0.009999999776482582 212 | Pitch: 0.785398006439209 213 | Target Frame: 214 | Value: Orbit (rviz) 215 | Yaw: 0.785398006439209 216 | Saved: ~ 217 | Window Geometry: 218 | Displays: 219 | collapsed: false 220 | Height: 846 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Time: 227 | collapsed: false 228 | Tool Properties: 229 | collapsed: false 230 | Views: 231 | collapsed: false 232 | Width: 1200 233 | X: 1989 234 | Y: 261 235 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/src/dual_ur_robotiq_rs_node.cpp: -------------------------------------------------------------------------------- 1 | #include "dual_ur_robotiq_rs_description/dual_ur_robotiq_rs_node.h" 2 | 3 | DualUrRobotiqRsNode::DualUrRobotiqRsNode(const std::string & node_name, const rclcpp::NodeOptions &options) 4 | : Node(node_name, options){ 5 | // create action client 6 | left_gripper_action_client_ = 7 | rclcpp_action::create_client(this, left_gripper_action_name_); 8 | right_gripper_action_client_ = 9 | rclcpp_action::create_client(this, right_gripper_action_name_); 10 | send_goal_options_.goal_response_callback = 11 | std::bind(&DualUrRobotiqRsNode::goal_response_callback, this, std::placeholders::_1); 12 | send_goal_options_.feedback_callback = 13 | std::bind(&DualUrRobotiqRsNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); 14 | send_goal_options_.result_callback = 15 | std::bind(&DualUrRobotiqRsNode::result_callback, this, std::placeholders::_1); 16 | // wait for action server to come up 17 | RCLCPP_INFO(this->get_logger(), "Waiting for action server"); 18 | left_gripper_action_client_->wait_for_action_server(std::chrono::seconds(5)); 19 | right_gripper_action_client_->wait_for_action_server(std::chrono::seconds(5)); 20 | if (!left_gripper_action_client_->action_server_is_ready() || 21 | !right_gripper_action_client_->action_server_is_ready()) { 22 | RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 23 | throw std::runtime_error("Action server not available"); 24 | } 25 | } 26 | 27 | void DualUrRobotiqRsNode::init(){ 28 | // moveit 29 | both_move_group_ = 30 | std::make_shared( 31 | shared_from_this(), both_planning_group_name_); 32 | left_move_group_ = 33 | std::make_shared( 34 | shared_from_this(), left_planning_group_name_); 35 | right_move_group_ = 36 | std::make_shared( 37 | shared_from_this(), right_planning_group_name_); 38 | 39 | // for fast demo 40 | both_move_group_->setMaxAccelerationScalingFactor(0.3); 41 | both_move_group_->setMaxVelocityScalingFactor(0.3); 42 | 43 | both_move_group_->setPlanningPipelineId("ompl"); // refer to the planning_pipeline_config.yaml 44 | both_move_group_->setPlannerId("RRTConnectkConfigDefault"); 45 | left_move_group_->setPlanningPipelineId("ompl"); // refer to the planning_pipeline_config.yaml 46 | left_move_group_->setPlannerId("RRTConnectkConfigDefault"); 47 | right_move_group_->setPlanningPipelineId("ompl"); // refer to the planning_pipeline_config.yaml 48 | right_move_group_->setPlannerId("RRTConnectkConfigDefault"); 49 | 50 | // left_move_group_->setStartStateToCurrentState(); 51 | // left_move_group_->setNamedTarget("left_ready"); 52 | // left_move_group_->move(); 53 | // right_move_group_->setStartStateToCurrentState(); 54 | // right_move_group_->setNamedTarget("right_ready"); 55 | // right_move_group_->move(); 56 | } 57 | 58 | bool DualUrRobotiqRsNode::go_to_ready_position(){ 59 | // std::vector left_ready_joint = {M_PI, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, 0.0}; 60 | // std::vector right_ready_joint = {0.0, -M_PI_2, M_PI_2, -M_PI_2, -M_PI_2, 0.0}; 61 | std::vector both_ready_joint = { 62 | M_PI, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, 0.0, 63 | 0.0, -M_PI_2, M_PI_2, -M_PI_2, -M_PI_2, 0.0, 64 | }; 65 | both_move_group_->setJointValueTarget(both_ready_joint); 66 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 67 | bool success = (both_move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 68 | if(success){ 69 | both_move_group_->execute(my_plan); 70 | RCLCPP_INFO(this->get_logger(), "Go to ready position successfully"); 71 | } else { 72 | RCLCPP_ERROR(this->get_logger(), "Go to ready position failed"); 73 | } 74 | return success; 75 | } 76 | 77 | bool DualUrRobotiqRsNode::plan_and_execute( 78 | const geometry_msgs::msg::Pose & left_target_pose, 79 | const geometry_msgs::msg::Pose & right_target_pose, 80 | const std::string & left_reference_frame, 81 | const std::string & right_reference_frame, 82 | const std::string & left_end_effector_link, 83 | const std::string & right_end_effector_link){ 84 | 85 | auto const left_target_pose_stamped = [&left_target_pose, &left_reference_frame]{ 86 | geometry_msgs::msg::PoseStamped msg; 87 | msg.header.frame_id = left_reference_frame; 88 | msg.pose = left_target_pose; 89 | return msg; 90 | }(); 91 | auto const right_target_pose_stamped = [&right_target_pose, &right_reference_frame]{ 92 | geometry_msgs::msg::PoseStamped msg; 93 | msg.header.frame_id = right_reference_frame; 94 | msg.pose = right_target_pose; 95 | return msg; 96 | }(); 97 | 98 | left_target_joint_.clear(); 99 | right_target_joint_.clear(); 100 | both_target_joint_.clear(); 101 | 102 | left_move_group_->setStartStateToCurrentState(); 103 | left_move_group_->setJointValueTarget(left_target_pose_stamped, left_end_effector_link); 104 | left_move_group_->getJointValueTarget(left_target_joint_); // use ik to get joint target from target pose 105 | right_move_group_->setStartStateToCurrentState(); 106 | right_move_group_->setJointValueTarget(right_target_pose_stamped, right_end_effector_link); 107 | right_move_group_->getJointValueTarget(right_target_joint_); // use ik to get joint target from target pose 108 | 109 | // combine both target joint 110 | both_target_joint_.insert(both_target_joint_.end(), left_target_joint_.begin(), left_target_joint_.end()); 111 | both_target_joint_.insert(both_target_joint_.end(), right_target_joint_.begin(), right_target_joint_.end()); 112 | 113 | // set the target positions for both manipulators in joint space 114 | both_move_group_->setStartStateToCurrentState(); 115 | both_move_group_->setJointValueTarget(both_target_joint_); 116 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 117 | bool success = (both_move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 118 | if(success){ 119 | both_move_group_->execute(my_plan); 120 | RCLCPP_INFO(this->get_logger(), "Plan and execute successfully"); 121 | } else { 122 | RCLCPP_ERROR(this->get_logger(), "Plan and execute failed"); 123 | } 124 | return success; 125 | } 126 | 127 | bool DualUrRobotiqRsNode::plan_single_arm( 128 | bool left, 129 | const geometry_msgs::msg::Pose & target_pose, 130 | const std::string & reference_frame, 131 | const std::string & end_effector_link, 132 | moveit::planning_interface::MoveGroupInterface::Plan & plan){ 133 | 134 | auto move_group = left ? left_move_group_ : right_move_group_; 135 | 136 | auto const target_pose_stamped = [&target_pose, &reference_frame]{ 137 | geometry_msgs::msg::PoseStamped msg; 138 | msg.header.frame_id = reference_frame; 139 | msg.pose = target_pose; 140 | return msg; 141 | }(); 142 | 143 | move_group->setStartStateToCurrentState(); 144 | move_group->setJointValueTarget(target_pose_stamped, end_effector_link); 145 | bool success = (move_group->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); 146 | return success; 147 | } 148 | 149 | void DualUrRobotiqRsNode::goal_response_callback(const GoalHandleGripperCommand::SharedPtr & goal_handle){ 150 | if(!goal_handle){ 151 | RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); 152 | } else { 153 | RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); 154 | } 155 | } 156 | void DualUrRobotiqRsNode::feedback_callback(GoalHandleGripperCommand::SharedPtr, const std::shared_ptr feedback){ 157 | RCLCPP_INFO(this->get_logger(), "Got Feedback: Current position is %f", feedback->position); 158 | } 159 | void DualUrRobotiqRsNode::result_callback(const GoalHandleGripperCommand::WrappedResult & result){ 160 | switch (result.code) 161 | { 162 | case rclcpp_action::ResultCode::SUCCEEDED: 163 | break; 164 | case rclcpp_action::ResultCode::ABORTED: 165 | RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 166 | return; 167 | case rclcpp_action::ResultCode::CANCELED: 168 | RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 169 | return; 170 | default: 171 | RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 172 | return; 173 | } 174 | RCLCPP_INFO(this->get_logger(), "Goal is completed, current position is %f", result.result->position); 175 | } -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/srdf/dual_ur_robotiq_rs.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/robot_setup/dual_ur_robotiq_rs_description/urdf/dual_ur_robotiq_rs.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 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | $(arg simulation_controllers) 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | left_robotiq_85_gripper_arm 66 | left_wrist_3_link 67 | left_robotiq_85_left_finger_tip_link 68 | left_robotiq_85_right_finger_tip_link 69 | left_robotiq_85_left_knuckle_link 70 | left_robotiq_85_right_knuckle_link 71 | left_robotiq_85_left_inner_knuckle_link 72 | left_robotiq_85_right_inner_knuckle_link 73 | 74 | 75 | right_robotiq_85_gripper_arm 76 | right_wrist_3_link 77 | right_robotiq_85_left_finger_tip_link 78 | right_robotiq_85_right_finger_tip_link 79 | right_robotiq_85_left_knuckle_link 80 | right_robotiq_85_right_knuckle_link 81 | right_robotiq_85_left_inner_knuckle_link 82 | right_robotiq_85_right_inner_knuckle_link 83 | 84 | 150 85 | 30 86 | 1 87 | 2 88 | 0.01 89 | false 90 | __default_topic__ 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ur_ft_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(moveit_common REQUIRED) 15 | find_package(moveit_core REQUIRED) 16 | find_package(moveit_ros_planning_interface REQUIRED) 17 | find_package(moveit_ros_planning REQUIRED) 18 | find_package(moveit_visual_tools REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(rclcpp_action REQUIRED) 21 | find_package(control_msgs REQUIRED) 22 | 23 | add_library(ur_ft_node src/ur_ft_node.cpp) 24 | target_compile_features(ur_ft_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 25 | target_include_directories(ur_ft_node PUBLIC 26 | $ 27 | $) 28 | ament_target_dependencies( 29 | ur_ft_node 30 | rclcpp moveit_ros_planning_interface tf2 31 | moveit_core moveit_ros_planning control_msgs 32 | ) 33 | 34 | # Causes the visibility macros to use dllexport rather than dllimport, 35 | # which is appropriate when building the dll but not consuming it. 36 | target_compile_definitions(ur_ft_node PRIVATE "TEST_CREATING_BUILDING_LIBRARY") 37 | 38 | install( 39 | DIRECTORY include/ 40 | DESTINATION include 41 | ) 42 | install( 43 | TARGETS ur_ft_node 44 | EXPORT export_${PROJECT_NAME} 45 | ARCHIVE DESTINATION lib 46 | LIBRARY DESTINATION lib 47 | RUNTIME DESTINATION bin 48 | ) 49 | 50 | 51 | install( 52 | DIRECTORY 53 | config 54 | launch 55 | rviz 56 | urdf 57 | gazebo 58 | srdf 59 | moveit2 60 | # src 61 | DESTINATION 62 | share/${PROJECT_NAME} 63 | ) 64 | 65 | if(BUILD_TESTING) 66 | find_package(ament_lint_auto REQUIRED) 67 | # the following line skips the linter which checks for copyrights 68 | # comment the line when a copyright and license is added to all source files 69 | set(ament_cmake_copyright_FOUND TRUE) 70 | # the following line skips cpplint (only works in a git repo) 71 | # comment the line when this package is in a git repo and when 72 | # a copyright and license is added to all source files 73 | set(ament_cmake_cpplint_FOUND TRUE) 74 | ament_lint_auto_find_test_dependencies() 75 | endif() 76 | 77 | ament_export_include_directories( 78 | include 79 | ) 80 | ament_export_libraries( 81 | ur_ft_node 82 | ) 83 | ament_export_targets( 84 | export_${PROJECT_NAME} 85 | ) 86 | 87 | 88 | ament_package() 89 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | base_frame: 3 | x: 0.0 4 | y: 0.0 5 | z: 1.0 6 | roll: 0.0 7 | pitch: 0.0 8 | yaw: -1.5707 -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | 4 | update_rate: 100 # Hz 5 | 6 | joint_state_broadcaster: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | 9 | io_and_status_controller: 10 | type: ur_controllers/GPIOController 11 | 12 | speed_scaling_state_broadcaster: 13 | type: ur_controllers/SpeedScalingStateBroadcaster 14 | 15 | force_torque_sensor_broadcaster: 16 | type: ur_controllers/ForceTorqueStateBroadcaster 17 | 18 | joint_trajectory_controller: 19 | type: joint_trajectory_controller/JointTrajectoryController 20 | 21 | scaled_joint_trajectory_controller: 22 | type: ur_controllers/ScaledJointTrajectoryController 23 | 24 | forward_velocity_controller: 25 | type: velocity_controllers/JointGroupVelocityController 26 | 27 | forward_position_controller: 28 | type: position_controllers/JointGroupPositionController 29 | 30 | speed_scaling_state_broadcaster: 31 | ros__parameters: 32 | state_publish_rate: 100.0 33 | 34 | 35 | force_torque_sensor_broadcaster: 36 | ros__parameters: 37 | sensor_name: tcp_fts_sensor 38 | state_interface_names: 39 | - force.x 40 | - force.y 41 | - force.z 42 | - torque.x 43 | - torque.y 44 | - torque.z 45 | frame_id: tool0 46 | topic_name: ft_data 47 | 48 | 49 | joint_trajectory_controller: 50 | ros__parameters: 51 | joints: 52 | - shoulder_pan_joint 53 | - shoulder_lift_joint 54 | - elbow_joint 55 | - wrist_1_joint 56 | - wrist_2_joint 57 | - wrist_3_joint 58 | command_interfaces: 59 | - position 60 | state_interfaces: 61 | - position 62 | - velocity 63 | state_publish_rate: 100.0 64 | action_monitor_rate: 20.0 65 | allow_partial_joints_goal: false 66 | constraints: 67 | stopped_velocity_tolerance: 0.2 68 | goal_time: 0.0 69 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 70 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 71 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 72 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 73 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 74 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 75 | 76 | 77 | scaled_joint_trajectory_controller: 78 | ros__parameters: 79 | joints: 80 | - shoulder_pan_joint 81 | - shoulder_lift_joint 82 | - elbow_joint 83 | - wrist_1_joint 84 | - wrist_2_joint 85 | - wrist_3_joint 86 | command_interfaces: 87 | - position 88 | state_interfaces: 89 | - position 90 | - velocity 91 | state_publish_rate: 100.0 92 | action_monitor_rate: 20.0 93 | allow_partial_joints_goal: false 94 | constraints: 95 | stopped_velocity_tolerance: 0.2 96 | goal_time: 0.0 97 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 98 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 99 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 100 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 101 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 102 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 103 | 104 | forward_velocity_controller: 105 | ros__parameters: 106 | joints: 107 | - shoulder_pan_joint 108 | - shoulder_lift_joint 109 | - elbow_joint 110 | - wrist_1_joint 111 | - wrist_2_joint 112 | - wrist_3_joint 113 | interface_name: velocity 114 | 115 | forward_position_controller: 116 | ros__parameters: 117 | joints: 118 | - shoulder_pan_joint 119 | - shoulder_lift_joint 120 | - elbow_joint 121 | - wrist_1_joint 122 | - wrist_2_joint 123 | - wrist_3_joint 124 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/gazebo/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 0 0 -9.8 11 | 12 | 13 | 14 | 15 | model://sun 16 | 17 | 18 | 19 | 20 | 21 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 22 | orbit 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/include/ur_ft_description/ur_ft_node.h: -------------------------------------------------------------------------------- 1 | #ifndef UR_FT_DESCRIPTION_INCLUDE_UR_FT_NODE_H_ 2 | #define UR_FT_DESCRIPTION_INCLUDE_UR_FT_NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | class UrFTNode : public rclcpp::Node{ 21 | public: 22 | /** 23 | * @brief Construct a new Ur Robotiq Rs Node object 24 | * 25 | * @param options 26 | */ 27 | explicit UrFTNode(const std::string & node_name, 28 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); 29 | 30 | /** 31 | * @brief initialize move group in init function 32 | * 33 | * MoveGroupInterface needs the shared point of the Node, but shared_ptr won't be created until after the constructor returns 34 | * so we need to create the move group interface in a separate function 35 | * @ref https://robotics.stackexchange.com/questions/96027/getting-a-nodesharedptr-from-this 36 | */ 37 | void init(); 38 | 39 | /** 40 | * @brief 链式构造 41 | */ 42 | UrFTNode & set_planning_group_name(const std::string & planning_group_name){ 43 | PLANNING_GROUP = planning_group_name; 44 | return *this; 45 | } 46 | 47 | UrFTNode & set_wrench_topic_name(const std::string & wrench_topic_name){ 48 | wrench_topic_name_ = wrench_topic_name; 49 | return *this; 50 | } 51 | 52 | /** 53 | * @brief Plan the path to the target pose and execute it 54 | * 55 | * @param target_pose 56 | * @return true 57 | * @return false 58 | */ 59 | bool plan_and_execute(const geometry_msgs::msg::Pose & target_pose, 60 | const std::string & reference_frame = "base_link", 61 | const std::string & end_effector_link = "tip_link_end"); 62 | 63 | bool plan_and_execute_cartesian(const geometry_msgs::msg::Pose & target_pose, 64 | const std::string & reference_frame = "base_link", 65 | const std::string & end_effector_link = "tip_link_end"); 66 | 67 | void add_collision_objects(std::vector & collision_objects); 68 | 69 | moveit::planning_interface::MoveGroupInterfacePtr & get_move_group(){ 70 | return move_group_; 71 | } 72 | 73 | geometry_msgs::msg::Wrench & get_current_wrench(){ 74 | return current_wrench_; 75 | } 76 | 77 | private: 78 | // moveit 79 | std::string PLANNING_GROUP = "ur_manipulator"; 80 | moveit::planning_interface::MoveGroupInterfacePtr move_group_; 81 | moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; 82 | /** 83 | * @brief convert cartesian target to joint target 84 | * 85 | * @param target_pose cartesian target pose 86 | * @param joint_target joint target 87 | * @param reference_frame reference frame 88 | */ 89 | void cartesian_target_2_joint_target(const geometry_msgs::msg::Pose & target_pose, 90 | const std::string & reference_frame); 91 | 92 | 93 | // robotiq ft sensor wrench msg subscribe 94 | std::string wrench_topic_name_ = "/robotiq_ft300/wrench"; 95 | rclcpp::Subscription::SharedPtr wrench_sub_; 96 | geometry_msgs::msg::Wrench current_wrench_; 97 | void wrench_topic_callback(const geometry_msgs::msg::Wrench & msg){ 98 | current_wrench_ = msg; 99 | } 100 | }; 101 | 102 | 103 | 104 | #endif -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/launch/remove_comments.py: -------------------------------------------------------------------------------- 1 | import re 2 | import xacro 3 | import sys 4 | import argparse 5 | def remove_comments(text): 6 | pattern = r'' 7 | return re.sub(pattern, '', text, flags=re.DOTALL) 8 | 9 | if __name__ == '__main__': 10 | # 接收来自命令行的参数，参数格式与xacro命令行工具相同 11 | # 例如：python remove_comments.py ur_ft_description.urdf.xacro xx:=yy ... 12 | # 创建 ArgumentParser 对象 13 | parser = argparse.ArgumentParser(description='Process some command line arguments.') 14 | 15 | # 添加文件名参数 16 | parser.add_argument('filename', type=str, help='The filename to process.') 17 | 18 | # 添加可变参数 19 | parser.add_argument('pairs', nargs='*', type=str, help='The key-value pairs in the format xx:=yy.') 20 | 21 | # 解析命令行参数 22 | args = parser.parse_args() 23 | 24 | # 调用 xacro.process_file() 函数 25 | urdf = xacro.process_file(args.filename, mappings=dict([pair.split(':=') for pair in args.pairs])) 26 | urdf_string = urdf.toxml() 27 | urdf_string = remove_comments(urdf_string) 28 | print(urdf_string) 29 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_planning: 4 | cartesian_limits: 5 | max_trans_vel: 1.0 6 | max_trans_acc: 2.25 7 | max_trans_dec: 1.0 8 | max_rot_vel: 100.57 9 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # These limits are used by MoveIt and augment/override the definitions in ur_description. 2 | # 3 | # While the robot does not inherently have any limits on joint accelerations (only on torques), 4 | # MoveIt needs them for time parametrization. They were chosen conservatively to work in most use 5 | # cases. For specific applications, higher values might lead to better execution performance. 6 | 7 | /**: 8 | ros__parameters: 9 | robot_description_planning: 10 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 11 | 12 | # For beginners, we downscale velocity and acceleration limits. 13 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 14 | default_velocity_scaling_factor: 0.1 15 | default_acceleration_scaling_factor: 0.1 16 | 17 | # As MoveIt does not support jerk limits, the acceleration limits provided here are the highest values that guarantee 18 | # that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel 19 | # to max accel in 1 ms) the acceleration limits are the ones that satisfy 20 | # max_jerk = (max_acceleration - min_acceleration) / 0.001 21 | 22 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 23 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 24 | joint_limits: 25 | shoulder_pan_joint: 26 | has_acceleration_limits: true 27 | max_acceleration: 5.0 28 | shoulder_lift_joint: 29 | has_acceleration_limits: true 30 | max_acceleration: 5.0 31 | elbow_joint: 32 | has_acceleration_limits: true 33 | max_acceleration: 5.0 34 | wrist_1_joint: 35 | has_acceleration_limits: true 36 | max_acceleration: 5.0 37 | wrist_2_joint: 38 | has_acceleration_limits: true 39 | max_acceleration: 5.0 40 | wrist_3_joint: 41 | has_acceleration_limits: true 42 | max_acceleration: 5.0 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/kinematics.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_kinematics: 4 | ur_manipulator: 5 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 6 | kinematics_solver_search_resolution: 0.005 7 | kinematics_solver_timeout: 0.005 8 | kinematics_solver_attempts: 3 9 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | moveit_controller_manager: 4 | moveit_simple_controller_manager/MoveItSimpleControllerManager 5 | 6 | moveit_simple_controller_manager: 7 | controller_names: 8 | - scaled_joint_trajectory_controller 9 | - joint_trajectory_controller 10 | - robotiq_gripper_controller 11 | 12 | 13 | scaled_joint_trajectory_controller: 14 | action_ns: follow_joint_trajectory 15 | type: FollowJointTrajectory 16 | default: false 17 | joints: 18 | - shoulder_pan_joint 19 | - shoulder_lift_joint 20 | - elbow_joint 21 | - wrist_1_joint 22 | - wrist_2_joint 23 | - wrist_3_joint 24 | 25 | joint_trajectory_controller: 26 | action_ns: follow_joint_trajectory 27 | type: FollowJointTrajectory 28 | default: true 29 | joints: 30 | - shoulder_pan_joint 31 | - shoulder_lift_joint 32 | - elbow_joint 33 | - wrist_1_joint 34 | - wrist_2_joint 35 | - wrist_3_joint 36 | 37 | robotiq_gripper_controller: 38 | action_ns: gripper_cmd 39 | type: GripperCommand 40 | default: true 41 | joints: 42 | - robotiq_85_left_knuckle_joint 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | ompl: 4 | planner_configs: 5 | SBLkConfigDefault: 6 | type: geometric::SBL 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 8 | ESTkConfigDefault: 9 | type: geometric::EST 10 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 11 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 12 | LBKPIECEkConfigDefault: 13 | type: geometric::LBKPIECE 14 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 15 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 16 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 17 | BKPIECEkConfigDefault: 18 | type: geometric::BKPIECE 19 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 20 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 21 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 22 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 23 | KPIECEkConfigDefault: 24 | type: geometric::KPIECE 25 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 26 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 27 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 28 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 29 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 30 | RRTkConfigDefault: 31 | type: geometric::RRT 32 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 33 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 34 | RRTConnectkConfigDefault: 35 | type: geometric::RRTConnect 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | RRTstarkConfigDefault: 38 | type: geometric::RRTstar 39 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 40 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 41 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 42 | TRRTkConfigDefault: 43 | type: geometric::TRRT 44 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 45 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 46 | max_states_failed: 10 # when to start increasing temp. default: 10 47 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 48 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 49 | init_temperature: 10e-6 # initial temperature. default: 10e-6 50 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 51 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 52 | k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() 53 | PRMkConfigDefault: 54 | type: geometric::PRM 55 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 56 | PRMstarkConfigDefault: 57 | type: geometric::PRMstar 58 | ur_manipulator: 59 | planner_configs: 60 | - SBLkConfigDefault 61 | - ESTkConfigDefault 62 | - LBKPIECEkConfigDefault 63 | - BKPIECEkConfigDefault 64 | - KPIECEkConfigDefault 65 | - RRTkConfigDefault 66 | - RRTConnectkConfigDefault 67 | - RRTstarkConfigDefault 68 | - TRRTkConfigDefault 69 | - PRMkConfigDefault 70 | - PRMstarkConfigDefault 71 | ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE 72 | #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) 73 | longest_valid_segment_fraction: 0.01 74 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/moveit2/planning_pipelines_config.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | default_planning_pipeline: ompl 4 | planning_pipelines: [pilz, ompl] 5 | pilz: 6 | planning_plugin: pilz_industrial_motion_planner/CommandPlanner 7 | request_adapters: "" 8 | start_state_max_bounds_error: 0.1 9 | ompl: 10 | planning_plugin: ompl_interface/OMPLPlanner 11 | request_adapters: 12 | default_planner_request_adapters/AddTimeOptimalParameterization 13 | default_planner_request_adapters/FixWorkspaceBounds 14 | default_planner_request_adapters/FixStartStateBounds 15 | default_planner_request_adapters/FixStartStateCollision 16 | default_planner_request_adapters/FixStartStatePathConstraints 17 | start_state_max_bounds_error: 0.1 18 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ur_ft_description 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/src/ur_ft_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ur_ft_description/ur_ft_node.h" 2 | 3 | UrFTNode::UrFTNode(const std::string & node_name, const rclcpp::NodeOptions &options) 4 | : Node(node_name, options){ 5 | move_group_ = nullptr; 6 | 7 | // wrench subscription 8 | wrench_sub_ = this->create_subscription( 9 | wrench_topic_name_, 10, std::bind(&UrFTNode::wrench_topic_callback, this, std::placeholders::_1) 10 | ); 11 | } 12 | 13 | void UrFTNode::init(){ 14 | // moveit 15 | move_group_ = std::make_shared(shared_from_this(), PLANNING_GROUP); 16 | RCLCPP_INFO(this->get_logger(), "Reference frame: %s", move_group_->getPlanningFrame().c_str()); 17 | 18 | // move_group_->setPlanningTime(5.0); 19 | // move_group_->setNumPlanningAttempts(5); 20 | move_group_->setMaxVelocityScalingFactor(0.5); 21 | move_group_->setMaxAccelerationScalingFactor(0.5); 22 | // move_group_->setGoalTolerance(0.01); 23 | // move_group_->setGoalJointTolerance(0.01); 24 | // move_group_->setGoalOrientationTolerance(0.01); 25 | // move_group_->setGoalPositionTolerance(0.01); 26 | // move_group_->setPlannerId("RRTConnectkConfigDefault"); 27 | 28 | // move to ready configuration 29 | move_group_->setStartStateToCurrentState(); 30 | move_group_->allowReplanning(true); 31 | move_group_->setNamedTarget("ready"); 32 | move_group_->move(); 33 | 34 | } 35 | 36 | void UrFTNode::cartesian_target_2_joint_target( 37 | const geometry_msgs::msg::Pose & target_pose, 38 | const std::string & reference_frame){ 39 | geometry_msgs::msg::PoseStamped target_pose_stamped; 40 | target_pose_stamped.header.frame_id = reference_frame; 41 | target_pose_stamped.pose = target_pose; 42 | 43 | // Set the joint state goal for a particular joint by computing IK. 44 | move_group_->setJointValueTarget(target_pose_stamped); 45 | } 46 | 47 | 48 | bool UrFTNode::plan_and_execute(const geometry_msgs::msg::Pose & target_pose, 49 | const std::string & reference_frame, 50 | const std::string & end_effector_link){ 51 | move_group_->setPlanningPipelineId("ompl"); // refer to the planning_pipeline_config.yaml 52 | move_group_->setPlannerId("RRTConnectkConfigDefault"); 53 | move_group_->setMaxVelocityScalingFactor(0.03); 54 | move_group_->setMaxAccelerationScalingFactor(0.03); 55 | 56 | auto const target_pose_stamped = [&target_pose, &reference_frame]{ 57 | geometry_msgs::msg::PoseStamped msg; 58 | msg.header.frame_id = reference_frame; 59 | msg.pose = target_pose; 60 | return msg; 61 | }(); 62 | 63 | move_group_->setStartStateToCurrentState(); 64 | move_group_->setJointValueTarget(target_pose_stamped, end_effector_link); 65 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 66 | bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 67 | if(success){ 68 | move_group_->execute(my_plan); 69 | RCLCPP_INFO(this->get_logger(), "Plan and execute successfully"); 70 | } 71 | return success; 72 | } 73 | 74 | bool UrFTNode::plan_and_execute_cartesian(const geometry_msgs::msg::Pose & target_pose, 75 | const std::string & reference_frame, 76 | const std::string & end_effector_link){ 77 | move_group_->setPlanningPipelineId("pilz"); 78 | move_group_->setPlannerId("PTP"); 79 | move_group_->setMaxVelocityScalingFactor(0.01); 80 | move_group_->setMaxAccelerationScalingFactor(0.01); 81 | 82 | auto const target_pose_stamped = [&target_pose, &reference_frame]{ 83 | geometry_msgs::msg::PoseStamped msg; 84 | msg.header.frame_id = reference_frame; 85 | msg.pose = target_pose; 86 | return msg; 87 | }(); 88 | 89 | move_group_->setStartStateToCurrentState(); 90 | move_group_->setPoseTarget(target_pose_stamped, end_effector_link); 91 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 92 | bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 93 | if(success){ 94 | move_group_->execute(my_plan); 95 | RCLCPP_INFO(this->get_logger(), "Plan and execute successfully"); 96 | } 97 | return success; 98 | } 99 | 100 | 101 | void UrFTNode::add_collision_objects(std::vector & collision_objects){ 102 | planning_scene_interface_.addCollisionObjects(collision_objects); 103 | } 104 | 105 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/srdf/test.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 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/srdf/ur_ft.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/srdf/ur_ft_macro.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 | -------------------------------------------------------------------------------- /src/robot_setup/ur_ft_description/urdf/ur_ft.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | $(arg simulation_controllers) 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ur_robotiq_rs_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | find_package(rclcpp REQUIRED) 13 | find_package(moveit_common REQUIRED) 14 | find_package(moveit_core REQUIRED) 15 | find_package(moveit_ros_planning_interface REQUIRED) 16 | find_package(moveit_ros_planning REQUIRED) 17 | find_package(moveit_visual_tools REQUIRED) 18 | find_package(rclcpp_components REQUIRED) 19 | find_package(rclcpp_action REQUIRED) 20 | find_package(control_msgs REQUIRED) 21 | 22 | include_directories(include) 23 | 24 | 25 | add_library(ur_robotiq_rs_node src/ur_robotiq_rs_node.cpp) 26 | target_compile_features(ur_robotiq_rs_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 27 | target_include_directories(ur_robotiq_rs_node PUBLIC 28 | $ 29 | $) 30 | ament_target_dependencies( 31 | ur_robotiq_rs_node 32 | rclcpp moveit_ros_planning_interface tf2 33 | moveit_core moveit_ros_planning control_msgs 34 | ) 35 | 36 | # Causes the visibility macros to use dllexport rather than dllimport, 37 | # which is appropriate when building the dll but not consuming it. 38 | target_compile_definitions(ur_robotiq_rs_node PRIVATE "TEST_CREATING_BUILDING_LIBRARY") 39 | 40 | install( 41 | DIRECTORY include/ 42 | DESTINATION include 43 | ) 44 | install( 45 | TARGETS ur_robotiq_rs_node 46 | EXPORT export_${PROJECT_NAME} 47 | ARCHIVE DESTINATION lib 48 | LIBRARY DESTINATION lib 49 | RUNTIME DESTINATION bin 50 | ) 51 | 52 | install( 53 | DIRECTORY 54 | config 55 | launch 56 | rviz 57 | urdf 58 | gazebo 59 | srdf 60 | moveit2 61 | # src 62 | DESTINATION 63 | share/${PROJECT_NAME} 64 | ) 65 | 66 | if(BUILD_TESTING) 67 | find_package(ament_lint_auto REQUIRED) 68 | # the following line skips the linter which checks for copyrights 69 | # comment the line when a copyright and license is added to all source files 70 | set(ament_cmake_copyright_FOUND TRUE) 71 | # the following line skips cpplint (only works in a git repo) 72 | # comment the line when this package is in a git repo and when 73 | # a copyright and license is added to all source files 74 | set(ament_cmake_cpplint_FOUND TRUE) 75 | ament_lint_auto_find_test_dependencies() 76 | endif() 77 | 78 | ament_export_include_directories( 79 | include 80 | ) 81 | ament_export_libraries( 82 | ur_robotiq_rs_node 83 | ) 84 | ament_export_targets( 85 | export_${PROJECT_NAME} 86 | ) 87 | 88 | ament_package() 89 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | base_frame: 3 | x: 0.0 4 | y: 0.0 5 | z: 1.0 6 | roll: 0.0 7 | pitch: 0.0 8 | yaw: 1.5707 -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | 4 | update_rate: 100 # Hz 5 | 6 | joint_state_broadcaster: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | 9 | io_and_status_controller: 10 | type: ur_controllers/GPIOController 11 | 12 | speed_scaling_state_broadcaster: 13 | type: ur_controllers/SpeedScalingStateBroadcaster 14 | 15 | force_torque_sensor_broadcaster: 16 | type: ur_controllers/ForceTorqueStateBroadcaster 17 | 18 | joint_trajectory_controller: 19 | type: joint_trajectory_controller/JointTrajectoryController 20 | 21 | scaled_joint_trajectory_controller: 22 | type: ur_controllers/ScaledJointTrajectoryController 23 | 24 | forward_velocity_controller: 25 | type: velocity_controllers/JointGroupVelocityController 26 | 27 | forward_position_controller: 28 | type: position_controllers/JointGroupPositionController 29 | 30 | robotiq_gripper_controller: 31 | type: position_controllers/GripperActionController 32 | 33 | robotiq_activation_controller: 34 | type: robotiq_controllers/RobotiqActivationController 35 | 36 | speed_scaling_state_broadcaster: 37 | ros__parameters: 38 | state_publish_rate: 100.0 39 | 40 | 41 | force_torque_sensor_broadcaster: 42 | ros__parameters: 43 | sensor_name: tcp_fts_sensor 44 | state_interface_names: 45 | - force.x 46 | - force.y 47 | - force.z 48 | - torque.x 49 | - torque.y 50 | - torque.z 51 | frame_id: tool0 52 | topic_name: ft_data 53 | 54 | 55 | joint_trajectory_controller: 56 | ros__parameters: 57 | joints: 58 | - shoulder_pan_joint 59 | - shoulder_lift_joint 60 | - elbow_joint 61 | - wrist_1_joint 62 | - wrist_2_joint 63 | - wrist_3_joint 64 | command_interfaces: 65 | - position 66 | state_interfaces: 67 | - position 68 | - velocity 69 | state_publish_rate: 100.0 70 | action_monitor_rate: 20.0 71 | allow_partial_joints_goal: false 72 | constraints: 73 | stopped_velocity_tolerance: 0.2 74 | goal_time: 0.0 75 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 76 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 77 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 78 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 79 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 80 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 81 | 82 | 83 | scaled_joint_trajectory_controller: 84 | ros__parameters: 85 | joints: 86 | - shoulder_pan_joint 87 | - shoulder_lift_joint 88 | - elbow_joint 89 | - wrist_1_joint 90 | - wrist_2_joint 91 | - wrist_3_joint 92 | command_interfaces: 93 | - position 94 | state_interfaces: 95 | - position 96 | - velocity 97 | state_publish_rate: 100.0 98 | action_monitor_rate: 20.0 99 | allow_partial_joints_goal: false 100 | constraints: 101 | stopped_velocity_tolerance: 0.2 102 | goal_time: 0.0 103 | shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } 104 | shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } 105 | elbow_joint: { trajectory: 0.2, goal: 0.1 } 106 | wrist_1_joint: { trajectory: 0.2, goal: 0.1 } 107 | wrist_2_joint: { trajectory: 0.2, goal: 0.1 } 108 | wrist_3_joint: { trajectory: 0.2, goal: 0.1 } 109 | 110 | forward_velocity_controller: 111 | ros__parameters: 112 | joints: 113 | - shoulder_pan_joint 114 | - shoulder_lift_joint 115 | - elbow_joint 116 | - wrist_1_joint 117 | - wrist_2_joint 118 | - wrist_3_joint 119 | interface_name: velocity 120 | 121 | forward_position_controller: 122 | ros__parameters: 123 | joints: 124 | - shoulder_pan_joint 125 | - shoulder_lift_joint 126 | - elbow_joint 127 | - wrist_1_joint 128 | - wrist_2_joint 129 | - wrist_3_joint 130 | 131 | robotiq_gripper_controller: 132 | ros__parameters: 133 | default: true 134 | joint: robotiq_85_left_knuckle_joint 135 | 136 | robotiq_activation_controller: 137 | ros__parameters: 138 | default: true -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | # you can use `ros2 launch ur_robotiq_rs_description view_ur_robotiq_rs.launch.py` 2 | # to help you find the desired joint angles 3 | shoulder_pan_joint: 0.0 4 | shoulder_lift_joint: -1.57 5 | elbow_joint: 1.57 6 | wrist_1_joint: -1.57 7 | wrist_2_joint: -1.57 8 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/gazebo/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 0 0 -9.8 11 | 12 | 13 | 14 | 15 | model://sun 16 | 17 | 18 | 19 | 20 | 21 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 22 | orbit 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/include/ur_robotiq_rs_description/ur_robotiq_rs_node.h: -------------------------------------------------------------------------------- 1 | #ifndef UR_ROBOTIQ_RS_DESCRIPTION_INCLUDE_UR_ROBOTIQ_RS_NODE_H_ 2 | #define UR_ROBOTIQ_RS_DESCRIPTION_INCLUDE_UR_ROBOTIQ_RS_NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class UrRobotiqRsNode : public rclcpp::Node{ 20 | public: 21 | using GripperCommand = control_msgs::action::GripperCommand; 22 | using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle; 23 | 24 | /** 25 | * @brief Construct a new Ur Robotiq Rs Node object 26 | * 27 | * @param options 28 | */ 29 | explicit UrRobotiqRsNode(const std::string & node_name, 30 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); 31 | 32 | /** 33 | * @brief initialize move group in init function 34 | * 35 | * MoveGroupInterface needs the shared point of the Node, but shared_ptr won't be created until after the constructor returns 36 | * so we need to create the move group interface in a separate function 37 | * @ref https://robotics.stackexchange.com/questions/96027/getting-a-nodesharedptr-from-this 38 | */ 39 | void init(); 40 | 41 | /** 42 | * @brief 链式构造 43 | */ 44 | UrRobotiqRsNode & set_planning_group_name(const std::string & planning_group_name){ 45 | PLANNING_GROUP = planning_group_name; 46 | return *this; 47 | } 48 | UrRobotiqRsNode & set_gripper_action_name(const std::string & gripper_action_name){ 49 | gripper_action_name_ = gripper_action_name; 50 | return *this; 51 | } 52 | 53 | /** 54 | * @brief Plan the path to the target pose and execute it 55 | * 56 | * @param target_pose 57 | * @return true 58 | * @return false 59 | */ 60 | bool plan_and_execute(const geometry_msgs::msg::Pose & target_pose); 61 | 62 | /** 63 | * @brief grasp the object 64 | * 65 | * @param gripper_position target position for the robotiq gripper 66 | */ 67 | void grasp(double gripper_position); 68 | 69 | private: 70 | // moveit 71 | std::string PLANNING_GROUP = "ur_manipulator"; 72 | moveit::planning_interface::MoveGroupInterfacePtr move_group_; 73 | moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; 74 | /** 75 | * @brief convert cartesian target to joint target 76 | * 77 | * @param target_pose cartesian target pose 78 | * @param joint_target joint target 79 | * @param reference_frame reference frame 80 | */ 81 | void cartesian_target_2_joint_target(const geometry_msgs::msg::Pose & target_pose, 82 | const std::string & reference_frame); 83 | 84 | // robotiq gripper 85 | std::string gripper_action_name_ = "/robotiq_gripper_controller/gripper_cmd"; 86 | rclcpp_action::Client::SharedPtr gripper_action_client_; 87 | void goal_response_callback(const GoalHandleGripperCommand::SharedPtr & goal_handle); 88 | void feedback_callback(GoalHandleGripperCommand::SharedPtr, const std::shared_ptr feedback); 89 | void result_callback(const GoalHandleGripperCommand::WrappedResult & result); 90 | rclcpp_action::Client::SendGoalOptions send_goal_options_; 91 | 92 | 93 | 94 | }; 95 | 96 | 97 | 98 | #endif -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/launch/view_ur_robotiq_rs.launch.py: -------------------------------------------------------------------------------- 1 | import launch 2 | from launch.substitutions import ( 3 | Command, 4 | FindExecutable, 5 | LaunchConfiguration, 6 | PathJoinSubstitution, 7 | ) 8 | import launch_ros 9 | import os 10 | 11 | 12 | def generate_launch_description(): 13 | pkg_share = launch_ros.substitutions.FindPackageShare( 14 | package="ur_robotiq_rs_description" 15 | ).find("ur_robotiq_rs_description") 16 | default_model_path = os.path.join( 17 | pkg_share, "urdf", "ur_robotiq_rs.urdf.xacro" 18 | ) 19 | default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz") 20 | 21 | args = [] 22 | args.append( 23 | launch.actions.DeclareLaunchArgument( 24 | name="model", 25 | default_value=default_model_path, 26 | description="Absolute path to robot URDF file", 27 | ) 28 | ) 29 | args.append( 30 | launch.actions.DeclareLaunchArgument( 31 | name="rvizconfig", 32 | default_value=default_rviz_config_path, 33 | description="Absolute path to rviz config file", 34 | ) 35 | ) 36 | 37 | robot_description_content = Command( 38 | [ 39 | PathJoinSubstitution([FindExecutable(name="xacro")]), 40 | " ", 41 | LaunchConfiguration("model"), 42 | " " 43 | "ur_type:=", 44 | "ur5e", 45 | " ", 46 | "sim_gazebo:=", 47 | "true" 48 | ] 49 | ) 50 | robot_description_param = { 51 | "robot_description": launch_ros.parameter_descriptions.ParameterValue( 52 | robot_description_content, value_type=str 53 | ) 54 | } 55 | 56 | robot_state_publisher_node = launch_ros.actions.Node( 57 | package="robot_state_publisher", 58 | executable="robot_state_publisher", 59 | parameters=[robot_description_param], 60 | ) 61 | 62 | joint_state_publisher_node = launch_ros.actions.Node( 63 | package="joint_state_publisher_gui", 64 | executable="joint_state_publisher_gui", 65 | ) 66 | 67 | rviz_node = launch_ros.actions.Node( 68 | package="rviz2", 69 | executable="rviz2", 70 | name="rviz2", 71 | output="screen", 72 | arguments=["-d", LaunchConfiguration("rvizconfig")], 73 | ) 74 | 75 | nodes = [ 76 | robot_state_publisher_node, 77 | joint_state_publisher_node, 78 | rviz_node, 79 | ] 80 | 81 | return launch.LaunchDescription(args + nodes) 82 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_planning: 4 | cartesian_limits: 5 | max_trans_vel: 1.0 6 | max_trans_acc: 2.25 7 | max_trans_dec: 1.0 8 | max_rot_vel: 100.57 9 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # These limits are used by MoveIt and augment/override the definitions in ur_description. 2 | # 3 | # While the robot does not inherently have any limits on joint accelerations (only on torques), 4 | # MoveIt needs them for time parametrization. They were chosen conservatively to work in most use 5 | # cases. For specific applications, higher values might lead to better execution performance. 6 | 7 | /**: 8 | ros__parameters: 9 | robot_description_planning: 10 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 11 | 12 | # For beginners, we downscale velocity and acceleration limits. 13 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 14 | default_velocity_scaling_factor: 0.1 15 | default_acceleration_scaling_factor: 0.1 16 | 17 | # As MoveIt does not support jerk limits, the acceleration limits provided here are the highest values that guarantee 18 | # that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel 19 | # to max accel in 1 ms) the acceleration limits are the ones that satisfy 20 | # max_jerk = (max_acceleration - min_acceleration) / 0.001 21 | 22 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 23 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 24 | joint_limits: 25 | shoulder_pan_joint: 26 | has_acceleration_limits: true 27 | max_acceleration: 5.0 28 | shoulder_lift_joint: 29 | has_acceleration_limits: true 30 | max_acceleration: 5.0 31 | elbow_joint: 32 | has_acceleration_limits: true 33 | max_acceleration: 5.0 34 | wrist_1_joint: 35 | has_acceleration_limits: true 36 | max_acceleration: 5.0 37 | wrist_2_joint: 38 | has_acceleration_limits: true 39 | max_acceleration: 5.0 40 | wrist_3_joint: 41 | has_acceleration_limits: true 42 | max_acceleration: 5.0 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/kinematics.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description_kinematics: 4 | ur_manipulator: 5 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 6 | kinematics_solver_search_resolution: 0.005 7 | kinematics_solver_timeout: 0.005 8 | kinematics_solver_attempts: 3 9 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | moveit_controller_manager: 4 | moveit_simple_controller_manager/MoveItSimpleControllerManager 5 | 6 | moveit_simple_controller_manager: 7 | controller_names: 8 | - scaled_joint_trajectory_controller 9 | - joint_trajectory_controller 10 | - robotiq_gripper_controller 11 | 12 | 13 | scaled_joint_trajectory_controller: 14 | action_ns: follow_joint_trajectory 15 | type: FollowJointTrajectory 16 | default: false 17 | joints: 18 | - shoulder_pan_joint 19 | - shoulder_lift_joint 20 | - elbow_joint 21 | - wrist_1_joint 22 | - wrist_2_joint 23 | - wrist_3_joint 24 | 25 | joint_trajectory_controller: 26 | action_ns: follow_joint_trajectory 27 | type: FollowJointTrajectory 28 | default: true 29 | joints: 30 | - shoulder_pan_joint 31 | - shoulder_lift_joint 32 | - elbow_joint 33 | - wrist_1_joint 34 | - wrist_2_joint 35 | - wrist_3_joint 36 | 37 | robotiq_gripper_controller: 38 | action_ns: gripper_cmd 39 | type: GripperCommand 40 | default: true 41 | joints: 42 | - robotiq_85_left_knuckle_joint 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | ompl: 4 | planner_configs: 5 | SBLkConfigDefault: 6 | type: geometric::SBL 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 8 | ESTkConfigDefault: 9 | type: geometric::EST 10 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 11 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 12 | LBKPIECEkConfigDefault: 13 | type: geometric::LBKPIECE 14 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 15 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 16 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 17 | BKPIECEkConfigDefault: 18 | type: geometric::BKPIECE 19 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 20 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 21 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 22 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 23 | KPIECEkConfigDefault: 24 | type: geometric::KPIECE 25 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 26 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 27 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 28 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 29 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 30 | RRTkConfigDefault: 31 | type: geometric::RRT 32 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 33 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 34 | RRTConnectkConfigDefault: 35 | type: geometric::RRTConnect 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | RRTstarkConfigDefault: 38 | type: geometric::RRTstar 39 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 40 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 41 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 42 | TRRTkConfigDefault: 43 | type: geometric::TRRT 44 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 45 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 46 | max_states_failed: 10 # when to start increasing temp. default: 10 47 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 48 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 49 | init_temperature: 10e-6 # initial temperature. default: 10e-6 50 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 51 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 52 | k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() 53 | PRMkConfigDefault: 54 | type: geometric::PRM 55 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 56 | PRMstarkConfigDefault: 57 | type: geometric::PRMstar 58 | ur_manipulator: 59 | planner_configs: 60 | - SBLkConfigDefault 61 | - ESTkConfigDefault 62 | - LBKPIECEkConfigDefault 63 | - BKPIECEkConfigDefault 64 | - KPIECEkConfigDefault 65 | - RRTkConfigDefault 66 | - RRTConnectkConfigDefault 67 | - RRTstarkConfigDefault 68 | - TRRTkConfigDefault 69 | - PRMkConfigDefault 70 | - PRMstarkConfigDefault 71 | ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE 72 | #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) 73 | longest_valid_segment_fraction: 0.01 74 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/moveit2/planning_pipelines_config.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | default_planning_pipeline: ompl 4 | planning_pipelines: [pilz, ompl] 5 | pilz: 6 | planning_plugin: pilz_industrial_motion_planner/CommandPlanner 7 | request_adapters: "" 8 | start_state_max_bounds_error: 0.1 9 | ompl: 10 | planning_plugin: ompl_interface/OMPLPlanner 11 | request_adapters: 12 | default_planner_request_adapters/AddTimeOptimalParameterization 13 | default_planner_request_adapters/FixWorkspaceBounds 14 | default_planner_request_adapters/FixStartStateBounds 15 | default_planner_request_adapters/FixStartStateCollision 16 | default_planner_request_adapters/FixStartStatePathConstraints 17 | start_state_max_bounds_error: 0.1 18 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ur_robotiq_rs_description 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | moveit_core 14 | moveit_ros_planning_interface 15 | moveit_common 16 | moveit_ros_planning 17 | tf2 18 | rclcpp_action 19 | rclcpp_components 20 | control_msgs 21 | 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/rviz/view_urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | Splitter Ratio: 0.6264705657958984 9 | Tree Height: 555 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 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz_default_plugins/RobotModel 51 | Collision Enabled: false 52 | Description File: "" 53 | Description Source: Topic 54 | Description Topic: 55 | Depth: 5 56 | Durability Policy: Volatile 57 | History Policy: Keep Last 58 | Reliability Policy: Reliable 59 | Value: robot_description 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | arm_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | base_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | dummy_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | end_effector_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | forearm_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | gripper_base_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | left_finger_dist_link: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | left_finger_prox_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | lower_wrist_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | right_finger_dist_link: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | right_finger_prox_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | shoulder_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | table: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | tool_frame: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | upper_wrist_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | world: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Mass Properties: 144 | Inertia: false 145 | Mass: false 146 | Name: RobotModel 147 | TF Prefix: "" 148 | Update Interval: 0 149 | Value: true 150 | Visual Enabled: true 151 | Enabled: true 152 | Global Options: 153 | Background Color: 48; 48; 48 154 | Fixed Frame: world 155 | Frame Rate: 30 156 | Name: root 157 | Tools: 158 | - Class: rviz_default_plugins/Interact 159 | Hide Inactive Objects: true 160 | - Class: rviz_default_plugins/MoveCamera 161 | - Class: rviz_default_plugins/Select 162 | - Class: rviz_default_plugins/FocusCamera 163 | - Class: rviz_default_plugins/Measure 164 | Line color: 128; 128; 0 165 | - Class: rviz_default_plugins/SetInitialPose 166 | Covariance x: 0.25 167 | Covariance y: 0.25 168 | Covariance yaw: 0.06853891909122467 169 | Topic: 170 | Depth: 5 171 | Durability Policy: Volatile 172 | History Policy: Keep Last 173 | Reliability Policy: Reliable 174 | Value: /initialpose 175 | - Class: rviz_default_plugins/SetGoal 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /goal_pose 182 | - Class: rviz_default_plugins/PublishPoint 183 | Single click: true 184 | Topic: 185 | Depth: 5 186 | Durability Policy: Volatile 187 | History Policy: Keep Last 188 | Reliability Policy: Reliable 189 | Value: /clicked_point 190 | Transformation: 191 | Current: 192 | Class: rviz_default_plugins/TF 193 | Value: true 194 | Views: 195 | Current: 196 | Class: rviz_default_plugins/Orbit 197 | Distance: 2.1567115783691406 198 | Enable Stereo Rendering: 199 | Stereo Eye Separation: 0.05999999865889549 200 | Stereo Focal Distance: 1 201 | Swap Stereo Eyes: false 202 | Value: false 203 | Focal Point: 204 | X: -0.09681572020053864 205 | Y: -0.10843408107757568 206 | Z: 0.1451336145401001 207 | Focal Shape Fixed Size: true 208 | Focal Shape Size: 0.05000000074505806 209 | Invert Z Axis: false 210 | Name: Current View 211 | Near Clip Distance: 0.009999999776482582 212 | Pitch: 0.785398006439209 213 | Target Frame: 214 | Value: Orbit (rviz) 215 | Yaw: 0.785398006439209 216 | Saved: ~ 217 | Window Geometry: 218 | Displays: 219 | collapsed: false 220 | Height: 846 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Time: 227 | collapsed: false 228 | Tool Properties: 229 | collapsed: false 230 | Views: 231 | collapsed: false 232 | Width: 1200 233 | X: 1989 234 | Y: 261 235 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/src/ur_robotiq_rs_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ur_robotiq_rs_description/ur_robotiq_rs_node.h" 2 | 3 | UrRobotiqRsNode::UrRobotiqRsNode(const std::string & node_name, const rclcpp::NodeOptions &options) 4 | : Node(node_name, options){ 5 | // create action client 6 | gripper_action_client_ = rclcpp_action::create_client(this, gripper_action_name_); 7 | send_goal_options_.goal_response_callback = std::bind(&UrRobotiqRsNode::goal_response_callback, this, std::placeholders::_1); 8 | send_goal_options_.feedback_callback = std::bind(&UrRobotiqRsNode::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); 9 | send_goal_options_.result_callback = std::bind(&UrRobotiqRsNode::result_callback, this, std::placeholders::_1); 10 | // wait for action server to come up 11 | RCLCPP_INFO(this->get_logger(), "Waiting for action server"); 12 | gripper_action_client_->wait_for_action_server(std::chrono::seconds(10)); 13 | if (!gripper_action_client_->action_server_is_ready()) { 14 | RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 15 | throw std::runtime_error("Action server not available"); 16 | } 17 | } 18 | 19 | void UrRobotiqRsNode::init(){ 20 | // moveit 21 | move_group_ = std::make_shared(shared_from_this(), PLANNING_GROUP); 22 | RCLCPP_INFO(this->get_logger(), "Reference frame: %s", move_group_->getPlanningFrame().c_str()); 23 | 24 | // move_group_->setPlanningTime(5.0); 25 | // move_group_->setNumPlanningAttempts(5); 26 | // move_group_->setMaxVelocityScalingFactor(0.5); 27 | // move_group_->setMaxAccelerationScalingFactor(0.5); 28 | // move_group_->setGoalTolerance(0.01); 29 | // move_group_->setGoalJointTolerance(0.01); 30 | // move_group_->setGoalOrientationTolerance(0.01); 31 | // move_group_->setGoalPositionTolerance(0.01); 32 | // move_group_->setPlannerId("RRTConnectkConfigDefault"); 33 | 34 | // move to ready configuration 35 | move_group_->setStartStateToCurrentState(); 36 | move_group_->allowReplanning(true); 37 | move_group_->setNamedTarget("ready"); 38 | move_group_->move(); 39 | 40 | } 41 | 42 | void UrRobotiqRsNode::cartesian_target_2_joint_target( 43 | const geometry_msgs::msg::Pose & target_pose, 44 | const std::string & reference_frame){ 45 | geometry_msgs::msg::PoseStamped target_pose_stamped; 46 | target_pose_stamped.header.frame_id = reference_frame; 47 | target_pose_stamped.pose = target_pose; 48 | 49 | // Set the joint state goal for a particular joint by computing IK. 50 | move_group_->setJointValueTarget(target_pose_stamped); 51 | } 52 | 53 | 54 | bool UrRobotiqRsNode::plan_and_execute(const geometry_msgs::msg::Pose & target_pose){ 55 | cartesian_target_2_joint_target(target_pose, "base_link"); 56 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 57 | bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); 58 | if(success){ 59 | move_group_->execute(my_plan); 60 | RCLCPP_INFO(this->get_logger(), "Plan and execute successfully"); 61 | } 62 | return success; 63 | } 64 | 65 | 66 | void UrRobotiqRsNode::grasp(double gripper_position){ 67 | auto gripper_goal_msg = GripperCommand::Goal(); 68 | gripper_goal_msg.command.position = gripper_position; 69 | gripper_goal_msg.command.max_effort = -1.0; // do not limit the effort 70 | 71 | RCLCPP_INFO(this->get_logger(), "Sending gripper goal"); 72 | auto f = gripper_action_client_->async_send_goal(gripper_goal_msg, send_goal_options_); 73 | // TODO: find a way to check action 74 | } 75 | 76 | void UrRobotiqRsNode::goal_response_callback(const GoalHandleGripperCommand::SharedPtr & goal_handle){ 77 | if(!goal_handle){ 78 | RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); 79 | } else { 80 | RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); 81 | } 82 | } 83 | void UrRobotiqRsNode::feedback_callback(GoalHandleGripperCommand::SharedPtr, const std::shared_ptr feedback){ 84 | RCLCPP_INFO(this->get_logger(), "Got Feedback: Current position is %f", feedback->position); 85 | } 86 | void UrRobotiqRsNode::result_callback(const GoalHandleGripperCommand::WrappedResult & result){ 87 | switch (result.code) 88 | { 89 | case rclcpp_action::ResultCode::SUCCEEDED: 90 | break; 91 | case rclcpp_action::ResultCode::ABORTED: 92 | RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 93 | return; 94 | case rclcpp_action::ResultCode::CANCELED: 95 | RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 96 | return; 97 | default: 98 | RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 99 | return; 100 | } 101 | RCLCPP_INFO(this->get_logger(), "Goal is completed, current position is %f", result.result->position); 102 | } -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/srdf/ur_robotiq_rs.srdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/srdf/ur_robotiq_rs_macro.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 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/urdf/realsense_d435.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0 18 | 0 19 | 0 20 | 1 21 | 22 | 1 23 | 0 0 0 24 | 26 | 1e+13 27 | 1 28 | 30 | 31 | 32 | 1.57 33 | 34 | 1280 35 | 720 36 | RGB_INT8 37 | 38 | 39 | 0.1 40 | 100 41 | 42 | 43 | gaussian 44 | 0.0 45 | 0.007 46 | 47 | 48 | 1 49 | 30 50 | 1 51 | 52 | 53 | 54 | 1.57 55 | 56 | 1280 57 | 720 58 | L_INT8 59 | 60 | 61 | 0.1 62 | 100 63 | 64 | 65 | gaussian 66 | 0.0 67 | 0.05 68 | 69 | 70 | 1 71 | 1 72 | 0 73 | 74 | 75 | 76 | 1.57 77 | 78 | 1280 79 | 720 80 | L_INT8 81 | 82 | 83 | 0.1 84 | 100 85 | 86 | 87 | gaussian 88 | 0.0 89 | 0.05 90 | 91 | 92 | 1 93 | 1 94 | 0 95 | 96 | 97 | 98 | 1.57 99 | 100 | 1280 101 | 720 102 | 103 | 104 | 0.1 105 | 100 106 | 107 | 108 | gaussian 109 | 0.0 110 | 0.100 111 | 112 | 113 | 1 114 | 30 115 | 0 116 | 117 | 118 | 119 | 120 | 121 | ${camera_name} 122 | 30.0 123 | 30.0 124 | 1.0 125 | ${topics_ns}/${camera_name}/depth/image_raw 126 | ${topics_ns}/${camera_name}/depth/camera_info 127 | ${topics_ns}/${camera_name}/color/image_raw 128 | ${topics_ns}/${camera_name}/color/camera_info 129 | ${topics_ns}/${camera_name}/infra1/image_raw 130 | ${topics_ns}/${camera_name}/infra1/camera_info 131 | ${topics_ns}/${camera_name}/infra2/image_raw 132 | ${topics_ns}/${camera_name}/infra2/camera_info 133 | ${color_optical_frame} 134 | ${depth_optical_frame} 135 | ${infrared1_optical_frame} 136 | ${infrared2_optical_frame} 137 | 0.2 138 | 10.0 139 | true 140 | ${topics_ns}/${camera_name}/depth/points 141 | 0.5 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/urdf/realsense_d435.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 18 | 19 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 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 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 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 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/urdf/ur_robotiq_rs.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 29 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | $(arg simulation_controllers) 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | $(arg tf_prefix)robotiq_85_gripper_arm 48 | $(arg tf_prefix)wrist_3_link 49 | $(arg tf_prefix)robotiq_85_left_finger_tip_link 50 | $(arg tf_prefix)robotiq_85_right_finger_tip_link 51 | $(arg tf_prefix)robotiq_85_left_knuckle_link 52 | $(arg tf_prefix)robotiq_85_right_knuckle_link 53 | $(arg tf_prefix)robotiq_85_left_inner_knuckle_link 54 | $(arg tf_prefix)robotiq_85_right_inner_knuckle_link 55 | 56 | 150 57 | 30 58 | 1 59 | 2 60 | 0.01 61 | false 62 | __default_topic__ 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /src/robot_setup/ur_robotiq_rs_description/urdf/ur_robotiq_rs_macro.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | 96 | 97 | 98 | 99 | 100 | 105 | 106 | 107 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /src/tasks/dual_plan/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(dual_plan) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(moveit_core REQUIRED) 12 | find_package(moveit_ros_planning_interface REQUIRED) 13 | find_package(moveit_common REQUIRED) 14 | find_package(moveit_ros_planning REQUIRED) 15 | find_package(tf2 REQUIRED) 16 | find_package(rclcpp_action REQUIRED) 17 | find_package(rclcpp_components REQUIRED) 18 | find_package(control_msgs REQUIRED) 19 | find_package(moveit_visual_tools REQUIRED) 20 | find_package(dual_ur_robotiq_rs_description REQUIRED) 21 | 22 | add_executable(dual_plan src/dual_plan.cpp) 23 | target_include_directories(dual_plan PUBLIC 24 | $ 25 | $) 26 | target_compile_features(dual_plan PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 27 | ament_target_dependencies( 28 | dual_plan 29 | "rclcpp" 30 | "moveit_core" 31 | "moveit_ros_planning_interface" 32 | "moveit_common" 33 | "moveit_ros_planning" 34 | "tf2" 35 | "rclcpp_action" 36 | "rclcpp_components" 37 | "control_msgs" 38 | "dual_ur_robotiq_rs_description" 39 | "moveit_visual_tools" 40 | ) 41 | 42 | install(TARGETS dual_plan 43 | DESTINATION lib/${PROJECT_NAME}) 44 | 45 | install( 46 | DIRECTORY 47 | launch 48 | config 49 | gazebo 50 | rviz 51 | DESTINATION 52 | share/${PROJECT_NAME} 53 | ) 54 | 55 | 56 | if(BUILD_TESTING) 57 | find_package(ament_lint_auto REQUIRED) 58 | # the following line skips the linter which checks for copyrights 59 | # comment the line when a copyright and license is added to all source files 60 | set(ament_cmake_copyright_FOUND TRUE) 61 | # the following line skips cpplint (only works in a git repo) 62 | # comment the line when this package is in a git repo and when 63 | # a copyright and license is added to all source files 64 | set(ament_cmake_cpplint_FOUND TRUE) 65 | ament_lint_auto_find_test_dependencies() 66 | endif() 67 | 68 | ament_package() 69 | -------------------------------------------------------------------------------- /src/tasks/dual_plan/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | # it will be parsed in dual_ur_robotiq_rs.urdf.xacro 3 | left_base_frame: 4 | x: 0.0 5 | y: 0.5 6 | z: 1.02 7 | roll: 0.0 8 | pitch: 0.0 9 | yaw: 0.0 10 | right_base_frame: 11 | x: 0.0 12 | y: -0.5 13 | z: 1.02 14 | roll: 0.0 15 | pitch: 0.0 16 | yaw: 0.0 -------------------------------------------------------------------------------- /src/tasks/dual_plan/config/left_initial_positions.yaml: -------------------------------------------------------------------------------- 1 | # you can use `ros2 launch ur_robotiq_rs_description view_ur_robotiq_rs.launch.py` 2 | # to help you find the desired joint angles 3 | shoulder_pan_joint: 3.14 4 | shoulder_lift_joint: -1.57 5 | elbow_joint: -1.57 6 | wrist_1_joint: -1.57 7 | wrist_2_joint: 1.57 8 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/tasks/dual_plan/config/right_initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/tasks/dual_plan/launch/bringup.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch_ros.substitutions import FindPackageShare 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.substitutions import PathJoinSubstitution 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | 12 | dual_ur_robotiq_rs_launch = IncludeLaunchDescription( 13 | PythonLaunchDescriptionSource([ 14 | FindPackageShare('dual_ur_robotiq_rs_description'), 15 | '/launch/dual_ur_robotiq_rs.launch.py' 16 | ]), 17 | launch_arguments={ 18 | 'runtime_config_package': 'dual_plan', 19 | 'gazebo_world_file': 'sim_env.world', 20 | 'base_frame_file': 'base_frame.yaml', 21 | 'rviz_config_file': 'dual_plan.rviz', 22 | 'use_sim_time': 'true', 23 | }.items() 24 | ) 25 | 26 | return LaunchDescription([ 27 | dual_ur_robotiq_rs_launch 28 | ]) -------------------------------------------------------------------------------- /src/tasks/dual_plan/launch/dual_plan.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from launch_ros.substitutions import FindPackageShare 5 | from launch.actions import ExecuteProcess 6 | from ament_index_python.packages import get_package_share_directory 7 | from moveit_configs_utils import MoveItConfigsBuilder 8 | from launch.substitutions import PathJoinSubstitution 9 | 10 | 11 | def generate_launch_description(): 12 | 13 | # kinematics config, needed for move group in pick_and_place 14 | robot_description_kinematics = PathJoinSubstitution( 15 | [FindPackageShare("dual_ur_robotiq_rs_description"), "moveit2", "kinematics.yaml"] 16 | ) 17 | 18 | return LaunchDescription([ 19 | Node( 20 | package='dual_plan', 21 | executable='dual_plan', 22 | name='dual_plan_node', 23 | parameters=[{ 24 | "use_sim_time":True, 25 | }, 26 | robot_description_kinematics, 27 | ], 28 | output='screen' 29 | ), 30 | ]) -------------------------------------------------------------------------------- /src/tasks/dual_plan/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dual_plan 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | moveit_core 14 | moveit_ros_planning_interface 15 | moveit_common 16 | moveit_ros_planning 17 | tf2 18 | rclcpp_action 19 | rclcpp_components 20 | control_msgs 21 | dual_ur_robotiq_rs_description 22 | moveit_visual_tools 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/tasks/find_hole/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(find_hole) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(moveit_common REQUIRED) 15 | find_package(moveit_core REQUIRED) 16 | find_package(moveit_ros_planning_interface REQUIRED) 17 | find_package(moveit_ros_planning REQUIRED) 18 | find_package(moveit_visual_tools REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(rclcpp_action REQUIRED) 21 | find_package(control_msgs REQUIRED) 22 | find_package(ur_ft_description REQUIRED) 23 | 24 | add_executable(find_hole src/find_hole.cpp) 25 | target_include_directories(find_hole PUBLIC 26 | $ 27 | $) 28 | ament_target_dependencies(find_hole 29 | rclcpp tf2 30 | moveit_core moveit_ros_planning control_msgs 31 | ur_ft_description 32 | ) 33 | 34 | install(TARGETS find_hole 35 | DESTINATION lib/${PROJECT_NAME}) 36 | 37 | 38 | install( 39 | DIRECTORY 40 | launch 41 | config 42 | gazebo 43 | rviz 44 | DESTINATION 45 | share/${PROJECT_NAME} 46 | ) 47 | 48 | if(BUILD_TESTING) 49 | find_package(ament_lint_auto REQUIRED) 50 | # the following line skips the linter which checks for copyrights 51 | # comment the line when a copyright and license is added to all source files 52 | set(ament_cmake_copyright_FOUND TRUE) 53 | # the following line skips cpplint (only works in a git repo) 54 | # comment the line when this package is in a git repo and when 55 | # a copyright and license is added to all source files 56 | set(ament_cmake_cpplint_FOUND TRUE) 57 | ament_lint_auto_find_test_dependencies() 58 | endif() 59 | 60 | ament_package() 61 | -------------------------------------------------------------------------------- /src/tasks/find_hole/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | base_frame: 3 | x: 0.0 4 | y: 0.0 5 | z: 0.0 6 | roll: 0.0 7 | pitch: 0.0 8 | yaw: 0.0 -------------------------------------------------------------------------------- /src/tasks/find_hole/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/tasks/find_hole/gazebo/sim_env.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 0 0 -9.8 11 | 12 | 13 | 14 | 15 | model://sun 16 | 17 | 18 | 19 | table_with_hole 20 | model://find_hole/gazebo/table_with_hole 21 | 0.5 0.0 0.0 0 0 0 22 | 23 | 24 | 25 | 26 | 27 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 28 | orbit 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /src/tasks/find_hole/gazebo/table_with_hole/meshes/table_with_hole.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zitongbai/ur_project_ros2/93533cbb08d07c5825fa17582573858023145250/src/tasks/find_hole/gazebo/table_with_hole/meshes/table_with_hole.stl -------------------------------------------------------------------------------- /src/tasks/find_hole/gazebo/table_with_hole/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | table_with_hole 4 | 1.0 5 | model.sdf 6 | 7 | xiaobaige 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/tasks/find_hole/gazebo/table_with_hole/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7.62604 7 | 8 | 0.064118 9 | 0 10 | 0 11 | 0.064118 12 | 0 13 | 0.126322 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0 0 0 0 -0 0 18 | 1 19 | 0 20 | 0 21 | 0 22 | 23 | 0 0 0 0 -0 0 24 | 25 | 26 | model://find_hole/gazebo/table_with_hole/meshes/table_with_hole.stl 27 | 1 1 1 28 | 29 | 30 | 31 | 1 32 | 36 | 37 | __default__ 38 | 39 | 40 | 0 41 | 1 42 | 43 | 44 | 0 45 | 10 46 | 0 0 0 0 -0 0 47 | 48 | 49 | model://find_hole/gazebo/table_with_hole/meshes/table_with_hole.stl 50 | 1 1 1 51 | 52 | 53 | 54 | 55 | 56 | 1 57 | 1 58 | 0 0 0 59 | 0 60 | 0 61 | 62 | 63 | 1 64 | 0 65 | 0 66 | 1 67 | 68 | 0 69 | 70 | 71 | 72 | 73 | 0 74 | 1e+06 75 | 76 | 77 | 0 78 | 1 79 | 1 80 | 81 | 0 82 | 0.2 83 | 1e+13 84 | 1 85 | 0.01 86 | 0 87 | 88 | 89 | 1 90 | -0.01 91 | 0 92 | 0.2 93 | 1e+13 94 | 1 95 | 96 | 97 | 98 | 99 | 100 | 1 101 | 1 102 | 103 | 104 | -------------------------------------------------------------------------------- /src/tasks/find_hole/include/find_hole/temp.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zitongbai/ur_project_ros2/93533cbb08d07c5825fa17582573858023145250/src/tasks/find_hole/include/find_hole/temp.h -------------------------------------------------------------------------------- /src/tasks/find_hole/launch/bringup.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch_ros.substitutions import FindPackageShare 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.substitutions import PathJoinSubstitution 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | 12 | ur_ft_launch = IncludeLaunchDescription( 13 | PythonLaunchDescriptionSource([ 14 | FindPackageShare('ur_ft_description'), 15 | '/launch/ur_ft.launch.py' 16 | ]), 17 | launch_arguments={ 18 | 'runtime_config_package': 'find_hole', 19 | 'gazebo_world_file': 'sim_env.world', 20 | 'base_frame_file': 'base_frame.yaml', 21 | 'rviz_config_file': 'find_hole.rviz', 22 | 'use_sim_time': 'true', 23 | }.items() 24 | ) 25 | 26 | 27 | 28 | return LaunchDescription([ 29 | ur_ft_launch 30 | ]) -------------------------------------------------------------------------------- /src/tasks/find_hole/launch/find_hole.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from launch_ros.substitutions import FindPackageShare 5 | from launch.actions import ExecuteProcess 6 | from ament_index_python.packages import get_package_share_directory 7 | from moveit_configs_utils import MoveItConfigsBuilder 8 | from launch.substitutions import PathJoinSubstitution 9 | 10 | 11 | def generate_launch_description(): 12 | 13 | # kinematics config, needed for move group in pick_and_place 14 | robot_description_kinematics = PathJoinSubstitution( 15 | [FindPackageShare("ur_ft_description"), "moveit2", "kinematics.yaml"] 16 | ) 17 | 18 | return LaunchDescription([ 19 | Node( 20 | package='find_hole', 21 | executable='find_hole', 22 | name='find_hole_node', 23 | parameters=[{ 24 | "use_sim_time":True, 25 | }, 26 | robot_description_kinematics, 27 | ], 28 | output='screen' 29 | ), 30 | ]) -------------------------------------------------------------------------------- /src/tasks/find_hole/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | find_hole 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rclcpp 16 | moveit_core 17 | moveit_ros_planning_interface 18 | moveit_common 19 | moveit_ros_planning 20 | tf2 21 | rclcpp_action 22 | rclcpp_components 23 | control_msgs 24 | ur_ft_description 25 | 26 | 27 | ament_cmake 28 | 31 | 32 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/tasks/find_hole/src/find_hole.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ur_ft_description/ur_ft_node.h" 3 | 4 | 5 | int main(int argc, char ** argv){ 6 | rclcpp::init(argc, argv); 7 | rclcpp::NodeOptions node_options; 8 | node_options.automatically_declare_parameters_from_overrides(true); 9 | auto node = std::make_shared("find_hole", node_options); 10 | node->init(); 11 | node->set_planning_group_name("ur_manipulator").set_wrench_topic_name("/robotiq_ft300/wrench"); 12 | 13 | rclcpp::executors::SingleThreadedExecutor executor; 14 | executor.add_node(node); 15 | std::thread([&executor]() { executor.spin(); }).detach(); 16 | 17 | // // add collision object 18 | // moveit_msgs::msg::CollisionObject collision_table; 19 | // collision_table.header.frame_id = "world"; 20 | // collision_table.id = "table"; 21 | // shape_msgs::msg::SolidPrimitive primitive; 22 | // primitive.type = primitive.BOX; 23 | // primitive.dimensions.resize(3); 24 | // primitive.dimensions[primitive.BOX_X] = 0.3; 25 | // primitive.dimensions[primitive.BOX_Y] = 0.3; 26 | // primitive.dimensions[primitive.BOX_Z] = 0.058; // it is slightly lower than real height 27 | // // the pose of the table should be the same as that in `sim_env.world` 28 | // geometry_msgs::msg::Pose box_pose; 29 | // box_pose.orientation.w = 1.0; 30 | // box_pose.position.x = 0.5; 31 | // box_pose.position.y = 0.0; 32 | // box_pose.position.z = primitive.dimensions[primitive.BOX_Z]/2.0; 33 | // collision_table.primitives.push_back(primitive); 34 | // collision_table.primitive_poses.push_back(box_pose); 35 | // collision_table.operation = collision_table.ADD; 36 | // // create collision_objects for planning_scene_interface 37 | // std::vector collision_objects; 38 | // collision_objects.push_back(collision_table); 39 | // node->add_collision_objects(collision_objects); 40 | // RCLCPP_INFO(node->get_logger(), "Add collision objects successfully"); 41 | 42 | 43 | auto & move_group = node->get_move_group(); 44 | std::string planning_frame = "base_link"; 45 | 46 | // first make sure the tip link is in horizontal plane 47 | geometry_msgs::msg::Pose target_pose = move_group->getCurrentPose().pose; 48 | tf2::Quaternion quat; 49 | quat.setRPY(0, M_PI, 0); 50 | quat.normalize(); 51 | target_pose.orientation.w = quat.w(); 52 | target_pose.orientation.x = quat.x(); 53 | target_pose.orientation.y = quat.y(); 54 | target_pose.orientation.z = quat.z(); 55 | target_pose.position.x = 0.46; 56 | target_pose.position.y = 0.0; 57 | target_pose.position.z = 0.0588; 58 | node->plan_and_execute(target_pose, planning_frame); 59 | 60 | target_pose.position.x = 0.5; 61 | target_pose.position.y = 0.0; 62 | target_pose.position.z = 0.058; 63 | node->plan_and_execute_cartesian(target_pose, planning_frame); 64 | 65 | target_pose.position.x = 0.5; 66 | target_pose.position.y = 0.0; 67 | target_pose.position.z = 0.03; 68 | node->plan_and_execute_cartesian(target_pose, planning_frame); 69 | 70 | target_pose.position.x = 0.5; 71 | target_pose.position.y = 0.0; 72 | target_pose.position.z = 0.07; 73 | node->plan_and_execute_cartesian(target_pose, planning_frame); 74 | 75 | 76 | 77 | 78 | 79 | 80 | rclcpp::shutdown(); 81 | return 0; 82 | } -------------------------------------------------------------------------------- /src/tasks/pick_and_place/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(pick_and_place) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(moveit_common REQUIRED) 15 | find_package(moveit_core REQUIRED) 16 | find_package(moveit_ros_planning_interface REQUIRED) 17 | find_package(moveit_ros_planning REQUIRED) 18 | find_package(moveit_visual_tools REQUIRED) 19 | find_package(rclcpp_components REQUIRED) 20 | find_package(rclcpp_action REQUIRED) 21 | find_package(control_msgs REQUIRED) 22 | find_package(ur_robotiq_rs_description REQUIRED) 23 | 24 | # include_directories( 25 | # include 26 | # ${ur_robotiq_rs_description_INCLUDE_DIRS} 27 | # ) 28 | 29 | # add_executable(pick_and_place src/pick_and_place.cpp) 30 | 31 | # ament_target_dependencies(pick_and_place 32 | # rclcpp moveit_ros_planning_interface tf2 33 | # moveit_core moveit_ros_planning control_msgs 34 | # ur_robotiq_rs_description 35 | # ) 36 | 37 | # install(TARGETS pick_and_place 38 | # DESTINATION lib/${PROJECT_NAME} 39 | # ) 40 | 41 | add_executable(pick_and_place src/pick_and_place.cpp) 42 | target_include_directories(pick_and_place PUBLIC 43 | $ 44 | $) 45 | ament_target_dependencies(pick_and_place 46 | rclcpp tf2 47 | moveit_core moveit_ros_planning control_msgs 48 | ur_robotiq_rs_description 49 | ) 50 | 51 | install(TARGETS pick_and_place 52 | DESTINATION lib/${PROJECT_NAME}) 53 | 54 | install( 55 | DIRECTORY 56 | launch 57 | config 58 | gazebo 59 | rviz 60 | DESTINATION 61 | share/${PROJECT_NAME} 62 | ) 63 | 64 | if(BUILD_TESTING) 65 | find_package(ament_lint_auto REQUIRED) 66 | # the following line skips the linter which checks for copyrights 67 | # comment the line when a copyright and license is added to all source files 68 | set(ament_cmake_copyright_FOUND TRUE) 69 | # the following line skips cpplint (only works in a git repo) 70 | # comment the line when this package is in a git repo and when 71 | # a copyright and license is added to all source files 72 | set(ament_cmake_cpplint_FOUND TRUE) 73 | ament_lint_auto_find_test_dependencies() 74 | endif() 75 | 76 | ament_package() 77 | -------------------------------------------------------------------------------- /src/tasks/pick_and_place/config/base_frame.yaml: -------------------------------------------------------------------------------- 1 | # Position and orientation of the robot base wrt World 2 | base_frame: 3 | x: 0.0 4 | y: 0.0 5 | z: 0.97 6 | roll: 0.0 7 | pitch: 0.0 8 | yaw: 0.0 -------------------------------------------------------------------------------- /src/tasks/pick_and_place/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | shoulder_pan_joint: 0.0 2 | shoulder_lift_joint: -1.57 3 | elbow_joint: 1.57 4 | wrist_1_joint: -1.57 5 | wrist_2_joint: -1.57 6 | wrist_3_joint: 0.0 -------------------------------------------------------------------------------- /src/tasks/pick_and_place/config/target_pose_list.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | target_pose_list: 4 | # x, y, z, roll, pitch, yaw, grasp 5 | - "0.5, -0.5, 0.4, 0, 3.14, 1.57, 0.0" 6 | - "0.5, -0.5, 0.17, 0, 3.14, 1.57, 0.4" 7 | - "0.5, 0.0, 0.4, 0, 3.14, 1.57, 0.4" 8 | - "0.5, 0.0, 0.4, 0, 3.14, 1.57, 0.0" -------------------------------------------------------------------------------- /src/tasks/pick_and_place/launch/bringup.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch_ros.substitutions import FindPackageShare 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.substitutions import PathJoinSubstitution 6 | from launch.launch_description_sources import PythonLaunchDescriptionSource 7 | 8 | 9 | def generate_launch_description(): 10 | 11 | 12 | ur_robotiq_rs_launch = IncludeLaunchDescription( 13 | PythonLaunchDescriptionSource([ 14 | FindPackageShare('ur_robotiq_rs_description'), 15 | '/launch/ur_robotiq_rs.launch.py' 16 | ]), 17 | launch_arguments={ 18 | 'runtime_config_package': 'pick_and_place', 19 | 'gazebo_world_file': 'sim_env.world', 20 | 'base_frame_file': 'base_frame.yaml', 21 | 'rviz_config_file': 'pick_and_place.rviz', 22 | 'use_sim_time': 'true', 23 | }.items() 24 | ) 25 | 26 | 27 | 28 | return LaunchDescription([ 29 | ur_robotiq_rs_launch 30 | ]) -------------------------------------------------------------------------------- /src/tasks/pick_and_place/launch/pick_and_place.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from launch_ros.substitutions import FindPackageShare 5 | from launch.actions import ExecuteProcess 6 | from ament_index_python.packages import get_package_share_directory 7 | from moveit_configs_utils import MoveItConfigsBuilder 8 | from launch.substitutions import PathJoinSubstitution 9 | 10 | 11 | def generate_launch_description(): 12 | 13 | # kinematics config, needed for move group in pick_and_place 14 | robot_description_kinematics = PathJoinSubstitution( 15 | [FindPackageShare("ur_robotiq_rs_description"), "moveit2", "kinematics.yaml"] 16 | ) 17 | 18 | target_pose_list = PathJoinSubstitution( 19 | [FindPackageShare("pick_and_place"), "config", "target_pose_list.yaml"] 20 | ) 21 | 22 | return LaunchDescription([ 23 | Node( 24 | package='pick_and_place', 25 | executable='pick_and_place', 26 | name='pick_and_place_node', 27 | parameters=[{ 28 | "use_sim_time":True, 29 | }, 30 | robot_description_kinematics, 31 | target_pose_list 32 | ], 33 | output='screen' 34 | ), 35 | ]) -------------------------------------------------------------------------------- /src/tasks/pick_and_place/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pick_and_place 5 | 0.0.0 6 | TODO: Package description 7 | xiaobaige 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | moveit_core 14 | moveit_ros_planning_interface 15 | moveit_common 16 | moveit_ros_planning 17 | tf2 18 | rclcpp_action 19 | rclcpp_components 20 | control_msgs 21 | ur_robotiq_rs_description 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/tasks/pick_and_place/rviz/pick_and_place.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | Splitter Ratio: 0.6264705657958984 9 | Tree Height: 555 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 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz_default_plugins/RobotModel 51 | Collision Enabled: false 52 | Description File: "" 53 | Description Source: Topic 54 | Description Topic: 55 | Depth: 5 56 | Durability Policy: Volatile 57 | History Policy: Keep Last 58 | Reliability Policy: Reliable 59 | Value: robot_description 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | arm_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | base_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | dummy_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | end_effector_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | forearm_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | gripper_base_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | left_finger_dist_link: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | left_finger_prox_link: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | lower_wrist_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | right_finger_dist_link: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | right_finger_prox_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | shoulder_link: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | table: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | tool_frame: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | upper_wrist_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | world: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Mass Properties: 144 | Inertia: false 145 | Mass: false 146 | Name: RobotModel 147 | TF Prefix: "" 148 | Update Interval: 0 149 | Value: true 150 | Visual Enabled: true 151 | Enabled: true 152 | Global Options: 153 | Background Color: 48; 48; 48 154 | Fixed Frame: world 155 | Frame Rate: 30 156 | Name: root 157 | Tools: 158 | - Class: rviz_default_plugins/Interact 159 | Hide Inactive Objects: true 160 | - Class: rviz_default_plugins/MoveCamera 161 | - Class: rviz_default_plugins/Select 162 | - Class: rviz_default_plugins/FocusCamera 163 | - Class: rviz_default_plugins/Measure 164 | Line color: 128; 128; 0 165 | - Class: rviz_default_plugins/SetInitialPose 166 | Covariance x: 0.25 167 | Covariance y: 0.25 168 | Covariance yaw: 0.06853891909122467 169 | Topic: 170 | Depth: 5 171 | Durability Policy: Volatile 172 | History Policy: Keep Last 173 | Reliability Policy: Reliable 174 | Value: /initialpose 175 | - Class: rviz_default_plugins/SetGoal 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /goal_pose 182 | - Class: rviz_default_plugins/PublishPoint 183 | Single click: true 184 | Topic: 185 | Depth: 5 186 | Durability Policy: Volatile 187 | History Policy: Keep Last 188 | Reliability Policy: Reliable 189 | Value: /clicked_point 190 | Transformation: 191 | Current: 192 | Class: rviz_default_plugins/TF 193 | Value: true 194 | Views: 195 | Current: 196 | Class: rviz_default_plugins/Orbit 197 | Distance: 2.1567115783691406 198 | Enable Stereo Rendering: 199 | Stereo Eye Separation: 0.05999999865889549 200 | Stereo Focal Distance: 1 201 | Swap Stereo Eyes: false 202 | Value: false 203 | Focal Point: 204 | X: -0.09681572020053864 205 | Y: -0.10843408107757568 206 | Z: 0.1451336145401001 207 | Focal Shape Fixed Size: true 208 | Focal Shape Size: 0.05000000074505806 209 | Invert Z Axis: false 210 | Name: Current View 211 | Near Clip Distance: 0.009999999776482582 212 | Pitch: 0.785398006439209 213 | Target Frame: 214 | Value: Orbit (rviz) 215 | Yaw: 0.785398006439209 216 | Saved: ~ 217 | Window Geometry: 218 | Displays: 219 | collapsed: false 220 | Height: 846 221 | Hide Left Dock: false 222 | Hide Right Dock: false 223 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Selection: 225 | collapsed: false 226 | Time: 227 | collapsed: false 228 | Tool Properties: 229 | collapsed: false 230 | Views: 231 | collapsed: false 232 | Width: 1200 233 | X: 1989 234 | Y: 261 235 | -------------------------------------------------------------------------------- /src/tasks/pick_and_place/src/pick_and_place.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ur_robotiq_rs_description/ur_robotiq_rs_node.h" 3 | 4 | 5 | void str_list_2_double_list(const std::vector & str_list, 6 | std::vector> & double_list){ 7 | double_list.clear(); 8 | // parse the string 9 | // each element in the list is a string 10 | // each string is a list of doubles, with ',' as delimiter 11 | for (auto & pose_str : str_list){ 12 | std::vector pose; 13 | std::stringstream ss(pose_str); 14 | std::string token; 15 | while (std::getline(ss, token, ',')){ 16 | pose.push_back(std::stod(token)); 17 | } 18 | double_list.push_back(pose); 19 | } 20 | } 21 | 22 | 23 | int main(int argc, char **argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | rclcpp::NodeOptions node_options; 27 | node_options.automatically_declare_parameters_from_overrides(true); 28 | auto node = std::make_shared("pick_and_place", node_options); 29 | node->init(); 30 | node->set_planning_group_name("ur_manipulator").set_gripper_action_name("robotiq_gripper_controller/gripper_cmd"); 31 | 32 | rclcpp::executors::SingleThreadedExecutor executor; 33 | executor.add_node(node); 34 | std::thread([&executor]() { executor.spin(); }).detach(); 35 | 36 | // get target pose list from parameter 37 | const std::vector target_pose_str_list = node->get_parameter("target_pose_list").as_string_array(); 38 | // make sure it is not empty 39 | assert (target_pose_str_list.size() > 0); 40 | const std::vector> target_pose_list = [&target_pose_str_list](){ 41 | std::vector> target_pose_list; 42 | str_list_2_double_list(target_pose_str_list, target_pose_list); 43 | return target_pose_list; 44 | }(); 45 | const std::vector target_pose_msg_list = [&target_pose_list](){ 46 | std::vector target_pose_msg_list; 47 | for (auto & pose : target_pose_list){ 48 | geometry_msgs::msg::Pose pose_msg; 49 | pose_msg.position.x = pose[0]; 50 | pose_msg.position.y = pose[1]; 51 | pose_msg.position.z = pose[2]; 52 | tf2::Quaternion quat; 53 | quat.setRPY(pose[3], pose[4], pose[5]); 54 | quat.normalize(); 55 | pose_msg.orientation.x = quat.x(); 56 | pose_msg.orientation.y = quat.y(); 57 | pose_msg.orientation.z = quat.z(); 58 | pose_msg.orientation.w = quat.w(); 59 | target_pose_msg_list.push_back(pose_msg); 60 | } 61 | return target_pose_msg_list; 62 | }(); 63 | const std::vector gripper_position_list = [&target_pose_list](){ 64 | std::vector gripper_position_list; 65 | for (auto & pose : target_pose_list){ 66 | gripper_position_list.push_back(pose[6]); 67 | } 68 | return gripper_position_list; 69 | }(); 70 | 71 | for(size_t i=0; iplan_and_execute(target_pose_msg_list[i]); 73 | node->grasp(gripper_position_list[i]); 74 | rclcpp::sleep_for(std::chrono::seconds(1)); 75 | } 76 | 77 | rclcpp::shutdown(); 78 | return 0; 79 | } 80 | --------------------------------------------------------------------------------