├── README.md ├── lara_bringup ├── CMakeLists.txt ├── config │ ├── aubo_i5_controllers.yaml │ └── aubo_i5_joint_names.yaml ├── launch │ ├── bringup.launch │ └── bringup_old.launch └── package.xml ├── lara_description ├── CMakeLists.txt ├── launch │ ├── display.launch │ ├── spawn_parts.launch │ ├── spawn_plastic_bin.launch │ └── urdf.rviz ├── meshes │ ├── parts │ │ ├── bowl.stl │ │ ├── mug.stl │ │ ├── pencil.stl │ │ ├── plastic_bin.stl │ │ ├── soda_can.stl │ │ └── tennis_ball.stl │ └── workspace │ │ ├── arm_mount.stl │ │ ├── base_mount.stl │ │ └── table.stl ├── package.xml └── urdf │ ├── lara_base_scene.xacro │ ├── parts │ ├── bowl.urdf │ ├── box.urdf │ ├── cube.urdf │ ├── mug.urdf │ ├── pencil.urdf │ ├── plastic_bin.urdf │ ├── soda_can.urdf │ └── tennis_ball.urdf │ └── workspace │ ├── lara_workspace.gazebo │ ├── lara_workspace_macro.xacro │ └── materials.xacro ├── lara_manipulation ├── CMakeLists.txt ├── package.xml └── src │ └── pick_and_place.py ├── lara_moveit_config ├── .setup_assistant ├── CMakeLists.txt ├── config │ ├── cartesian_limits.yaml │ ├── chomp_planning.yaml │ ├── fake_controllers.yaml │ ├── gazebo_controllers.yaml │ ├── gazebo_lara_base_scene.urdf │ ├── gazebo_moveit_controllers.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── lara_base_scene.srdf │ ├── ompl_planning.yaml │ ├── ros_controllers.yaml │ ├── sensors_3d.yaml │ ├── simple_moveit_controllers.yaml │ └── stomp_planning.yaml ├── launch │ ├── chomp_planning_pipeline.launch.xml │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── demo_gazebo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── gazebo.launch │ ├── gazebo_moveit_controller_manager.launch.xml │ ├── joystick_control.launch │ ├── lara_base_scene_moveit_controller_manager.launch.xml │ ├── lara_base_scene_moveit_sensor_manager.launch.xml │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl-chomp_planning_pipeline.launch.xml │ ├── ompl_planning_pipeline.launch.xml │ ├── pilz_industrial_motion_planner_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── ros_control_moveit_controller_manager.launch.xml │ ├── ros_controllers.launch │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── simple_moveit_controller_manager.launch.xml │ ├── stomp_planning_pipeline.launch.xml │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml ├── lara_moveit_servo ├── CMakeLists.txt ├── config │ ├── joystick_servo_config.yaml │ ├── pose_tracking_servo_config.yaml │ └── pose_tracking_settings.yaml ├── launch │ ├── pose_tracker.launch │ └── servo_server.launch ├── package.xml └── src │ └── pose_tracking_server.cpp ├── lara_simulation_tools ├── CMakeLists.txt ├── package.xml ├── src │ └── scene_manager.py └── urdf │ └── object.xacro ├── lara_teleop ├── CMakeLists.txt ├── launch │ └── aubo_joy_teleop.launch ├── package.xml └── scripts │ └── aubo_joy_teleop.py └── media ├── aubo_gazebo.gif └── aubo_vr.gif /README.md: -------------------------------------------------------------------------------- 1 | # LARA_AUBOi5_AG95 2 | 3 | Welcome to the **LARA_AUBOi5_AG95** repository! This repository contains ROS packages designed for running the Aubo i5 robot with the DH Robotics AG95 gripper. The primary focus of this repository is to provide tools and resources for experimenting with pick and place tasks using the Moveit motion planning framework and simulating in Gazebo. The project was developed within the context of the LARA lab. 4 | 5 | Check out some cool videos showcasing the environment in action: 6 | 7 | ![Aubo Gazebo Pick and Place](media/aubo_gazebo.gif) 8 | 9 | ![Aubo Meta Quest 2 Hand Controller Teleoperation](media/aubo_vr.gif) 10 | 11 | ## Features 12 | 13 | - Integration of Aubo i5 robot with DH Robotics AG95 gripper. 14 | - Simulation environment setup in Gazebo for pick and place tasks. 15 | - Implementation of Moveit for motion planning and manipulation. 16 | - Teleoperation capabilities using a Meta Quest 2 VR Hand Controller. 17 | - Teleoperation support via a joystick. 18 | 19 | ## Note 20 | 21 | Please note that this repository is not actively maintained. It was created during a learning phase while exploring the capabilities of ROS (Robot Operating System) and experimenting with robotics concepts. As a result, the code and documentation provided here might be incomplete or outdated. While the content remains available for reference and inspiration, there are no guarantees of ongoing updates or support. 22 | 23 | ## Tutorial 24 | 25 | To get started with using the **LARA_AUBOi5_AG95** repository and setting up the required environment, follow these steps: 26 | 27 | 1. **Update System and Install Dependencies**: 28 | 29 | ``` 30 | # Update 31 | rosdep update 32 | sudo apt update 33 | sudo apt dist-upgrade 34 | 35 | # Dependencies to install MoveIt from source 36 | sudo apt install python3-wstool python3-catkin-tools clang-format-10 python3-rosdep 37 | ``` 38 | 39 | 2. **Create Workspace and Clone Repositories**: 40 | 41 | ``` 42 | # Make workspace 43 | mkdir -p ~/catkin_ws/src 44 | 45 | # Clone packages 46 | cd ~/catkin_ws/src 47 | git clone https://github.com/ian-chuang/LARA_AUBOi5_AG95.git 48 | git clone https://github.com/ian-chuang/dh_gripper_ros.git # AG95 gripper description and drivers 49 | git clone https://github.com/ian-chuang/aubo_robot.git -b melodic # Aubo i5 description and drivers 50 | git clone https://github.com/ian-chuang/oculus_reader.git # Reading Meta Quest 2 Controllers 51 | ``` 52 | 53 | 3. **Clone Gazebo Plugins**: 54 | 55 | ``` 56 | # Clone gazebo plugins 57 | cd ~/catkin_ws/src 58 | mkdir gazebo_pkgs 59 | cd gazebo_pkgs 60 | git clone https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins.git # mimic joint plugin 61 | git clone https://github.com/pal-robotics/realsense_gazebo_plugin.git -b melodic-devel # realsense simulation plugin 62 | git clone https://github.com/ian-chuang/gazebo_gripper_action_controller.git # modified gripper action controller for gazebo 63 | ``` 64 | 65 | 4. **Install MoveIt from Source**: 66 | 67 | ``` 68 | # Install MoveIt from source 69 | cd ~/catkin_ws/src 70 | mkdir moveit 71 | wstool init moveit 72 | wstool merge -t moveit https://raw.githubusercontent.com/ros-planning/moveit/melodic-devel/moveit.rosinstall 73 | wstool update -t moveit 74 | ``` 75 | 76 | 5. **Install ROS Package Dependencies**: 77 | 78 | ``` 79 | # Install ROS package dependencies 80 | cd ~/catkin_ws/src 81 | rosdep install -y --from-paths . --ignore-src --rosdistro melodic 82 | ``` 83 | 84 | 6. **Install Other Dependencies LARA Uses**: 85 | 86 | ``` 87 | # Install other dependencies LARA uses 88 | sudo apt-get install ros-melodic-realsense2-camera # realsense drivers 89 | ``` 90 | 91 | 7. **Add Aubo Dependencies**: 92 | 93 | ``` 94 | # Add aubo dependencies 95 | sudo apt-get install gdebi 96 | wget http://archive.ubuntu.com/ubuntu/pool/main/p/protobuf/libprotobuf9v5_2.6.1-1.3_amd64.deb 97 | sudo gdebi ./libprotobuf9v5_2.6.1-1.3_amd64.deb 98 | rm ./libprotobuf9v5_2.6.1-1.3_amd64.deb 99 | cd ~/catkin_ws/src 100 | sudo ln -s aubo_robot/aubo_driver/lib/lib64/config/libconfig.so.11 /usr/lib/libconfig.so.11 101 | sudo ln -s aubo_robot/aubo_driver/lib/lib64/log4cplus/liblog4cplus-1.2.so.5.1.4 /usr/lib/liblog4cplus-1.2.so.5.1.4 102 | sudo ldconfig 103 | ``` 104 | 105 | 8. **Build Workspace**: 106 | 107 | ``` 108 | # Build 109 | cd ~/catkin_ws 110 | catkin build 111 | source devel/setup.bash 112 | ``` 113 | 114 | ## Getting Started and Running the Simulation 115 | 116 | Before you start, make sure you have completed the setup steps mentioned in the previous sections. 117 | 118 | * **Setting Up Meta Quest 2 Teleop**: 119 | To enable teleoperation using the Meta Quest 2 VR Hand Controller, follow the instructions provided in the [oculus_reader README](https://github.com/ian-chuang/oculus_reader/blob/main/README.md). 120 | 121 | * **Running Simulation and MoveIt**: 122 | To run the simulation and MoveIt, execute the following commands: 123 | 124 | ```bash 125 | # Launch simulation and MoveIt 126 | roslaunch lara_moveit_config demo_gazebo.launch cameras:=true 127 | 128 | # Spawn objects in the simulation 129 | roslaunch lara_description spawn_parts.launch 130 | 131 | # Run the pick routine 132 | rosrun lara_manipulation pick_and_place.py 133 | ``` 134 | 135 | * **Realtime Servoing with Meta Quest 2 Teleop**: 136 | If you want to experience realtime servoing using the Meta Quest 2 VR Hand Controller, follow these steps: 137 | 138 | ```bash 139 | # Ensure that simulation and MoveIt are running 140 | 141 | # Switch from trajectory controller to position controller 142 | rosservice call /controller_manager/switch_controller "start_controllers: 143 | - 'arm_position_controller' 144 | stop_controllers: 145 | - 'arm_trajectory_controller' 146 | strictness: 2" 147 | 148 | # Run the realtime servoing server 149 | roslaunch lara_moveit_servo pose_tracker.launch 150 | 151 | # Run the Meta Quest teleop 152 | rosrun oculus_reader pose_teleop.py 153 | ``` 154 | 155 | ## Changes I Did to Get It Working 156 | 157 | In the process of working with this repository, I made several adjustments to ensure its functionality. Here's a breakdown of the changes I made: 158 | 159 | #### lara_description: 160 | * **aubo_i5_macro.xacro**: Created a new URDF macro specifically for the Aubo i5 robot. This macro includes added joint transmissions to facilitate proper joint movement. 161 | * **pgc140_macro.xacro**: Developed a URDF macro for the PGC140 Gripper, incorporating joint transmissions and a mimic joint gazebo plugin to accurately simulate the gripper's behavior. 162 | * **aubo_i5_pgc140.urdf.xacro**: This URDF file combines the Aubo i5 arm and the PGC140 Gripper. It attaches the gripper to the arm, fixes them to the world, and integrates the necessary gazebo ROS control plugin for simulation. 163 | * **display.launch**: Created a launch file, allowing the URDF model to be visualized in RViz. 164 | 165 | #### lara_moveit_config (auto-generated by moveit setup assistant): 166 | * **gazebo_controllers.yaml**: Generated a configuration file that defines the arm and gripper controllers. This file also includes PID values for the mimic joint gazebo plugin, ensuring accurate joint control during simulation. 167 | * **ros_controllers.yaml**: Modified the ROS controllers configuration file to enable proper interfacing with the gazebo controllers. 168 | * **gazebo.launch**: Adjusted the gazebo.launch file to correctly load the xacro macro as a URDF. Additionally, the modifications allow the controllers defined in gazebo_controllers.yaml to be spawned in the simulation. 169 | 170 | **Disclaimer:** The information provided here is based on the changes I applied to the repository according to the description provided. Actual changes and code adjustments may vary depending on the specifics of your ROS setup and the codebase at the time of your interaction with it. 171 | -------------------------------------------------------------------------------- /lara_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_bringup) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES lara_bringup 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/lara_bringup.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/lara_bringup_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lara_bringup.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /lara_bringup/config/aubo_i5_controllers.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: &loop_hz 200 4 | 5 | aubo_i5_controller: 6 | type: position_controllers/JointTrajectoryController 7 | joints: 8 | - shoulder_pan_joint 9 | - shoulder_lift_joint 10 | - elbow_joint 11 | - wrist_1_joint 12 | - wrist_2_joint 13 | - wrist_3_joint 14 | constraints: 15 | # goal_time: 0.6 16 | # stopped_velocity_tolerance: 0.05 17 | # shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} 18 | # shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} 19 | # elbow_joint: {trajectory: 0.2, goal: 0.1} 20 | # wrist_1_joint: {trajectory: 0.2, goal: 0.1} 21 | # wrist_2_joint: {trajectory: 0.2, goal: 0.1} 22 | # wrist_3_joint: {trajectory: 0.2, goal: 0.1} 23 | goal_time: 0.6 24 | stopped_velocity_tolerance: 0.05 25 | shoulder_pan_joint: {trajectory: 0, goal: 0} 26 | shoulder_lift_joint: {trajectory: 0, goal: 0} 27 | elbow_joint: {trajectory: 0, goal: 0} 28 | wrist_1_joint: {trajectory: 0, goal: 0} 29 | wrist_2_joint: {trajectory: 0, goal: 0} 30 | wrist_3_joint: {trajectory: 0, goal: 0} 31 | stop_trajectory_duration: 0.5 32 | state_publish_rate: *loop_hz 33 | action_monitor_rate: 20 34 | 35 | arm_position_controller: 36 | type: position_controllers/JointGroupPositionController 37 | joints: 38 | - shoulder_pan_joint 39 | - shoulder_lift_joint 40 | - elbow_joint 41 | - wrist_1_joint 42 | - wrist_2_joint 43 | - wrist_3_joint -------------------------------------------------------------------------------- /lara_bringup/config/aubo_i5_joint_names.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint] 2 | robot_name: aubo_i5 -------------------------------------------------------------------------------- /lara_bringup/launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 27 | 28 | 29 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /lara_bringup/launch/bringup_old.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /lara_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_bringup 4 | 0.0.0 5 | The lara_bringup package 6 | 7 | 8 | 9 | 10 | laraauboi5 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /lara_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES lara_description 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/lara_description.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/lara_description_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lara_description.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /lara_description/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 7 | 11 | 15 | 21 | -------------------------------------------------------------------------------- /lara_description/launch/spawn_parts.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 14 | 15 | 16 | 19 | 22 | 25 | 28 | 31 | 34 | 37 | 40 | 41 | 42 | 45 | 46 | -------------------------------------------------------------------------------- /lara_description/launch/spawn_plastic_bin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 13 | 14 | -------------------------------------------------------------------------------- /lara_description/launch/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | Splitter Ratio: 0.5 12 | Tree Height: 555 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 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 | aubo_i5_base_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | ee_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | finger1_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | finger2_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | foreArm_Link: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | pgc140_base_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | shoulder_Link: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | upperArm_Link: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | world: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | wrist1_Link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | wrist2_Link: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | wrist3_Link: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | Name: RobotModel 127 | Robot Description: robot_description 128 | TF Prefix: "" 129 | Update Interval: 0 130 | Value: true 131 | Visual Enabled: true 132 | - Class: rviz/TF 133 | Enabled: false 134 | Frame Timeout: 15 135 | Frames: 136 | All Enabled: false 137 | Marker Scale: 1 138 | Name: TF 139 | Show Arrows: true 140 | Show Axes: true 141 | Show Names: true 142 | Tree: 143 | {} 144 | Update Interval: 0 145 | Value: false 146 | Enabled: true 147 | Global Options: 148 | Background Color: 48; 48; 48 149 | Default Light: true 150 | Fixed Frame: world 151 | Frame Rate: 30 152 | Name: root 153 | Tools: 154 | - Class: rviz/Interact 155 | Hide Inactive Objects: true 156 | - Class: rviz/MoveCamera 157 | - Class: rviz/Select 158 | - Class: rviz/FocusCamera 159 | - Class: rviz/Measure 160 | - Class: rviz/SetInitialPose 161 | Theta std deviation: 0.2617993950843811 162 | Topic: /initialpose 163 | X std deviation: 0.5 164 | Y std deviation: 0.5 165 | - Class: rviz/SetGoal 166 | Topic: /move_base_simple/goal 167 | - Class: rviz/PublishPoint 168 | Single click: true 169 | Topic: /clicked_point 170 | Value: true 171 | Views: 172 | Current: 173 | Class: rviz/Orbit 174 | Distance: 1.001585602760315 175 | Enable Stereo Rendering: 176 | Stereo Eye Separation: 0.05999999865889549 177 | Stereo Focal Distance: 1 178 | Swap Stereo Eyes: false 179 | Value: false 180 | Focal Point: 181 | X: -0.11454904824495316 182 | Y: -0.1611148864030838 183 | Z: 0.46070143580436707 184 | Focal Shape Fixed Size: true 185 | Focal Shape Size: 0.05000000074505806 186 | Invert Z Axis: false 187 | Name: Current View 188 | Near Clip Distance: 0.009999999776482582 189 | Pitch: 1.2203975915908813 190 | Target Frame: 191 | Value: Orbit (rviz) 192 | Yaw: 6.213587760925293 193 | Saved: ~ 194 | Window Geometry: 195 | Displays: 196 | collapsed: false 197 | Height: 846 198 | Hide Left Dock: false 199 | Hide Right Dock: false 200 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 201 | Selection: 202 | collapsed: false 203 | Time: 204 | collapsed: false 205 | Tool Properties: 206 | collapsed: false 207 | Views: 208 | collapsed: false 209 | Width: 1200 210 | X: 2407 211 | Y: 333 212 | -------------------------------------------------------------------------------- /lara_description/meshes/parts/bowl.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/bowl.stl -------------------------------------------------------------------------------- /lara_description/meshes/parts/mug.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/mug.stl -------------------------------------------------------------------------------- /lara_description/meshes/parts/pencil.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/pencil.stl -------------------------------------------------------------------------------- /lara_description/meshes/parts/plastic_bin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/plastic_bin.stl -------------------------------------------------------------------------------- /lara_description/meshes/parts/soda_can.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/soda_can.stl -------------------------------------------------------------------------------- /lara_description/meshes/parts/tennis_ball.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/parts/tennis_ball.stl -------------------------------------------------------------------------------- /lara_description/meshes/workspace/arm_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/workspace/arm_mount.stl -------------------------------------------------------------------------------- /lara_description/meshes/workspace/base_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/workspace/base_mount.stl -------------------------------------------------------------------------------- /lara_description/meshes/workspace/table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_description/meshes/workspace/table.stl -------------------------------------------------------------------------------- /lara_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_description 4 | 0.0.0 5 | The lara_description package 6 | 7 | 8 | 9 | 10 | ian 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | 53 | joint_trajectory_controller 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /lara_description/urdf/lara_base_scene.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 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 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/bowl.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | ${body_color} 33 | 1000000.0 34 | 1.0 35 | 1.16 36 | 1.16 37 | 0.0 38 | 0.003 39 | 40 | 41 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/box.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | Gazebo/Red 31 | 1000000.0 32 | 1.0 33 | 1.16 34 | 1.16 35 | 0.0 36 | 0.003 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/cube.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | Gazebo/Blue 31 | 1000000.0 32 | 1.0 33 | 1.16 34 | 1.16 35 | 0.0 36 | 0.003 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/mug.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | Gazebo/Blue 32 | 1000000.0 33 | 1.0 34 | 1.16 35 | 1.16 36 | 0.0 37 | 0.003 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/pencil.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | Gazebo/Yellow 34 | 1000000.0 35 | 1.0 36 | 1.16 37 | 1.16 38 | 0.0 39 | 0.003 40 | 41 | 42 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/plastic_bin.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Gazebo/Green 29 | 1000000.0 30 | 1.0 31 | 0.8 32 | 0.8 33 | 0.0 34 | 0.001 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/soda_can.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | ${body_color} 44 | 1000000.0 45 | 1.0 46 | 1.16 47 | 1.16 48 | 0.0 49 | 0.003 50 | 51 | 52 | -------------------------------------------------------------------------------- /lara_description/urdf/parts/tennis_ball.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | Gazebo/Green 31 | 1000000.0 32 | 1.0 33 | 1.16 34 | 1.16 35 | 0.0 36 | 0.003 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /lara_description/urdf/workspace/lara_workspace.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ${aluminum_color} 9 | 1000000.0 10 | 1.0 11 | 0.8 12 | 0.8 13 | 0.0 14 | 0.001 15 | true 16 | 17 | 18 | 19 | ${aluminum_color} 20 | 1000000.0 21 | 1.0 22 | 0.8 23 | 0.8 24 | 0.0 25 | 0.001 26 | true 27 | 28 | 29 | 30 | ${aluminum_color} 31 | 1000000.0 32 | 1.0 33 | 0.8 34 | 0.8 35 | 0.0 36 | 0.001 37 | true 38 | 39 | 40 | 41 | ${wood_color} 42 | 1000000.0 43 | 1.0 44 | 0.8 45 | 0.8 46 | 0.0 47 | 0.001 48 | true 49 | 50 | 51 | -------------------------------------------------------------------------------- /lara_description/urdf/workspace/lara_workspace_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 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 | -------------------------------------------------------------------------------- /lara_description/urdf/workspace/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /lara_manipulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_manipulation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | moveit_msgs 13 | moveit_ros_planning_interface 14 | roscpp 15 | rospy 16 | std_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | # generate_messages( 75 | # DEPENDENCIES 76 | # geometry_msgs# moveit_msgs# std_msgs 77 | # ) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES lara_manipulation 111 | # CATKIN_DEPENDS geometry_msgs moveit_msgs moveit_ros_planning_interface roscpp rospy std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/lara_manipulation.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/lara_manipulation_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # catkin_install_python(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lara_manipulation.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /lara_manipulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_manipulation 4 | 0.0.0 5 | The lara_manipulation package 6 | 7 | 8 | 9 | 10 | ian 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | geometry_msgs 53 | moveit_msgs 54 | moveit_ros_planning_interface 55 | roscpp 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | moveit_msgs 60 | moveit_ros_planning_interface 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | moveit_msgs 66 | moveit_ros_planning_interface 67 | roscpp 68 | rospy 69 | std_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /lara_manipulation/src/pick_and_place.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import sys 4 | import copy 5 | import rospy 6 | import moveit_commander 7 | import moveit_msgs.msg 8 | import geometry_msgs.msg 9 | from tf.transformations import quaternion_from_euler, translation_matrix, quaternion_matrix, translation_from_matrix, quaternion_from_matrix 10 | import numpy as np 11 | import tf 12 | from math import pi 13 | 14 | planning_frame = "manipulation_frame" 15 | 16 | def create_pose(x, y, z, roll, pitch, yaw, header=False): 17 | pose = geometry_msgs.msg.Pose() 18 | quat_tf = quaternion_from_euler(roll, pitch, yaw) 19 | orientation = geometry_msgs.msg.Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3]) 20 | pose.position.x = x 21 | pose.position.y = y 22 | pose.position.z = z 23 | pose.orientation = orientation 24 | 25 | if not header: 26 | return pose 27 | else: 28 | poseStamped = geometry_msgs.msg.PoseStamped() 29 | poseStamped.header.frame_id = planning_frame 30 | poseStamped.pose = pose 31 | return poseStamped 32 | 33 | class Manipulation: 34 | def __init__(self): 35 | self.scene = moveit_commander.PlanningSceneInterface() 36 | self.robot = moveit_commander.RobotCommander() 37 | self.arm = moveit_commander.MoveGroupCommander("arm") 38 | self.gripper = moveit_commander.MoveGroupCommander("gripper") 39 | 40 | self.arm.set_max_velocity_scaling_factor(1) 41 | self.arm.set_max_acceleration_scaling_factor(1) 42 | 43 | self.arm.set_pose_reference_frame(planning_frame) 44 | self.arm.set_support_surface_name('table') 45 | 46 | def move_to_pose(self, pose): 47 | self.arm.set_pose_target(pose) 48 | 49 | error_code_val, plan, planning_time, error_code = self.arm.plan() 50 | 51 | error_code_val = self.arm.execute(plan, wait=True) 52 | success = (error_code_val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS) 53 | if not success: 54 | return False 55 | 56 | return True 57 | 58 | def move_gripper(self, val): 59 | joint_goal = self.gripper.get_current_joint_values() 60 | joint_goal = [val] * len(joint_goal) 61 | 62 | self.gripper.set_joint_value_target(joint_goal) 63 | 64 | error_code_val, plan, planning_time, error_code = self.gripper.plan() 65 | 66 | error_code_val = self.gripper.execute(plan, wait=True) 67 | success = (error_code_val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS) 68 | if not success: 69 | return False 70 | 71 | return True 72 | 73 | def move_cartesian_path(self, pose): 74 | waypoints = [] 75 | waypoints.append(pose) 76 | 77 | (plan, fraction) = self.arm.compute_cartesian_path( 78 | waypoints, 79 | eef_step=0.01, 80 | jump_threshold=0, # 0? 81 | avoid_collisions=True 82 | ) 83 | 84 | if fraction < 1: 85 | return False 86 | 87 | error_code_val = self.arm.execute(plan, wait=True) 88 | success = (error_code_val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS) 89 | if not success: 90 | return False 91 | 92 | return True 93 | 94 | 95 | 96 | 97 | 98 | 99 | if __name__ == '__main__': 100 | rospy.init_node('pick_and_place') 101 | 102 | manip = Manipulation() 103 | eef_link = manip.arm.get_end_effector_link() 104 | 105 | ys = [-.7, -.55, -.395, -.25, -.1, .05] 106 | heights = [0.04, 0.02, 0.045, 0.008, 0.07, 0.02] 107 | gripper_max = 0.93 108 | 109 | for y, height in zip(ys, heights): 110 | y = y + 0.4 111 | pose = create_pose(0.4, y, 0.15, 0.0, pi/2, 0.0) 112 | manip.move_to_pose(pose) 113 | manip.move_gripper(0) 114 | pose = create_pose(0.4, y, height, 0.0, pi/2, 0.0) 115 | manip.move_cartesian_path(pose) 116 | manip.move_gripper(gripper_max) 117 | pose = create_pose(0.4, y, 0.2, 0.0, pi/2, 0.0) 118 | manip.move_cartesian_path(pose) 119 | rospy.sleep(0.1) 120 | pose = create_pose(0.4, y, height, 0.0, pi/2, 0.0) 121 | manip.move_cartesian_path(pose) 122 | manip.move_gripper(0) 123 | pose = create_pose(0.4, y, 0.2, 0.0, pi/2, 0.0) 124 | manip.move_cartesian_path(pose) 125 | 126 | # y = 0.19 127 | # height = 0.1 128 | # y = y + 0.4 129 | # pose = create_pose(0.4, y, 0.2, 0.0, pi/2, 0.0) 130 | # manip.move_to_pose(pose) 131 | # manip.move_gripper(0) 132 | # pose = create_pose(0.4, y, height, 0.0, pi/2, 0.0) 133 | # manip.move_cartesian_path(pose) 134 | # manip.move_gripper(gripper_max) 135 | # pose = create_pose(0.4, y, 0.2, 0.0, pi/2, 0.0) 136 | # manip.move_cartesian_path(pose) 137 | # rospy.sleep(1) 138 | # pose = create_pose(0.4, y, height+0.01, 0.0, pi/2, 0.0) 139 | # manip.move_cartesian_path(pose) 140 | # pose = create_pose(0.4, -.3+0.4, height+0.01, 0.0, pi/2, 0.0) 141 | # manip.move_cartesian_path(pose) 142 | 143 | 144 | 145 | # # rospy.spin() 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /lara_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: lara_description 4 | relative_path: urdf/lara_base_scene.xacro 5 | xacro_args: "" 6 | SRDF: 7 | relative_path: config/lara_base_scene.srdf 8 | CONFIG: 9 | author_name: Ian Chuang 10 | author_email: itchuang@ucdavis.edu 11 | generated_timestamp: 1677560275 -------------------------------------------------------------------------------- /lara_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.3) 2 | project(lara_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /lara_moveit_config/config/cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | cartesian_limits: 2 | max_trans_vel: 1 3 | max_trans_acc: 2.25 4 | max_trans_dec: -5 5 | max_rot_vel: 1.57 6 | -------------------------------------------------------------------------------- /lara_moveit_config/config/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_time_limit: 10.0 2 | max_iterations: 200 3 | max_iterations_after_collision_free: 5 4 | smoothness_cost_weight: 0.1 5 | obstacle_cost_weight: 1.0 6 | learning_rate: 0.01 7 | smoothness_cost_velocity: 0.0 8 | smoothness_cost_acceleration: 1.0 9 | smoothness_cost_jerk: 0.0 10 | ridge_factor: 0.0 11 | use_pseudo_inverse: false 12 | pseudo_inverse_ridge_factor: 1e-4 13 | joint_update_limit: 0.1 14 | collision_clearance: 0.2 15 | collision_threshold: 0.07 16 | use_stochastic_descent: true 17 | enable_failure_recovery: false 18 | max_recovery_attempts: 5 19 | -------------------------------------------------------------------------------- /lara_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | type: $(arg fake_execution_type) 4 | joints: 5 | - shoulder_pan_joint 6 | - shoulder_lift_joint 7 | - elbow_joint 8 | - wrist_1_joint 9 | - wrist_2_joint 10 | - wrist_3_joint 11 | - name: fake_gripper_controller 12 | type: $(arg fake_execution_type) 13 | joints: 14 | - left_outer_knuckle_joint 15 | initial: # Define initial robot poses per group 16 | - group: arm 17 | pose: home 18 | - group: gripper 19 | pose: open -------------------------------------------------------------------------------- /lara_moveit_config/config/gazebo_controllers.yaml: -------------------------------------------------------------------------------- 1 | # max_effort and max_velocity can be set in the urdf for gripper and arm joints 2 | 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: &loop_hz 500 6 | 7 | arm_trajectory_controller: 8 | type: position_controllers/JointTrajectoryController 9 | joints: &robot_joints 10 | - shoulder_pan_joint 11 | - shoulder_lift_joint 12 | - elbow_joint 13 | - wrist_1_joint 14 | - wrist_2_joint 15 | - wrist_3_joint 16 | constraints: 17 | goal_time: 0.6 18 | stopped_velocity_tolerance: 0.05 19 | shoulder_pan_joint: { trajectory: 0.1, goal: 0.1 } 20 | shoulder_lift_joint: { trajectory: 0.1, goal: 0.1 } 21 | elbow_joint: { trajectory: 0.1, goal: 0.1 } 22 | wrist_1_joint: { trajectory: 0.1, goal: 0.1 } 23 | wrist_2_joint: { trajectory: 0.1, goal: 0.1 } 24 | wrist_3_joint: { trajectory: 0.1, goal: 0.1 } 25 | stop_trajectory_duration: 0.5 26 | state_publish_rate: *loop_hz 27 | action_monitor_rate: 10 28 | 29 | gripper_action_controller: 30 | type: position_controllers/GazeboGripperActionController 31 | joint: left_outer_knuckle_joint 32 | goal_tolerance: 0.1 33 | stall_position_threshold: 0.05 # Tune to fit your robot 34 | stall_timeout: 0.2 35 | action_monitor_rate: 10 36 | 37 | arm_position_controller: 38 | type: position_controllers/JointGroupPositionController 39 | joints: 40 | - shoulder_pan_joint 41 | - shoulder_lift_joint 42 | - elbow_joint 43 | - wrist_1_joint 44 | - wrist_2_joint 45 | - wrist_3_joint 46 | 47 | gripper_position_controller: 48 | type: position_controllers/JointPositionController 49 | joint: left_outer_knuckle_joint 50 | 51 | gazebo_ros_control: 52 | pid_gains: 53 | # arm 54 | shoulder_pan_joint: {p: 5000, d: 200, i: 1, i_clamp: 1} 55 | shoulder_lift_joint: {p: 12000, d: 200, i: 1, i_clamp: 1} 56 | elbow_joint: {p: 3000, d: 20, i: 1, i_clamp: 1} 57 | wrist_1_joint: {p: 1000, d: 1, i: 1, i_clamp: 1} 58 | wrist_2_joint: {p: 500, d: 1, i: 1, i_clamp: 1} 59 | wrist_3_joint: {p: 100, d: 0.1, i: 0, i_clamp: 1} 60 | # gripper 61 | left_outer_knuckle_joint: {p: 10, d: 0.1, i: 1, i_clamp: 1} 62 | right_outer_knuckle_joint: {p: 20, d: 0.1, i: 1, i_clamp: 1} -------------------------------------------------------------------------------- /lara_moveit_config/config/gazebo_moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: arm_trajectory_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: True 6 | joints: 7 | - shoulder_pan_joint 8 | - shoulder_lift_joint 9 | - elbow_joint 10 | - wrist_1_joint 11 | - wrist_2_joint 12 | - wrist_3_joint 13 | - name: gripper_action_controller 14 | action_ns: gripper_cmd 15 | default: True 16 | type: GripperCommand 17 | joints: 18 | - left_outer_knuckle_joint -------------------------------------------------------------------------------- /lara_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # 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. 5 | default_velocity_scaling_factor: 1.0 6 | default_acceleration_scaling_factor: 1.0 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | elbow_joint: 12 | has_velocity_limits: true 13 | max_velocity: 3.14159265359 14 | has_acceleration_limits: true 15 | max_acceleration: 10 16 | left_finger_joint: 17 | has_velocity_limits: true 18 | max_velocity: 2 19 | has_acceleration_limits: false 20 | max_acceleration: 0 21 | left_inner_knuckle_joint: 22 | has_velocity_limits: true 23 | max_velocity: 2 24 | has_acceleration_limits: false 25 | max_acceleration: 0 26 | left_outer_knuckle_joint: 27 | has_velocity_limits: true 28 | max_velocity: 2 29 | has_acceleration_limits: false 30 | max_acceleration: 0 31 | right_finger_joint: 32 | has_velocity_limits: true 33 | max_velocity: 2 34 | has_acceleration_limits: false 35 | max_acceleration: 0 36 | right_inner_knuckle_joint: 37 | has_velocity_limits: true 38 | max_velocity: 2 39 | has_acceleration_limits: false 40 | max_acceleration: 0 41 | right_outer_knuckle_joint: 42 | has_velocity_limits: true 43 | max_velocity: 2 44 | has_acceleration_limits: false 45 | max_acceleration: 0 46 | shoulder_lift_joint: 47 | has_velocity_limits: true 48 | max_velocity: 3.14159265359 49 | has_acceleration_limits: true 50 | max_acceleration: 10 51 | shoulder_pan_joint: 52 | has_velocity_limits: true 53 | max_velocity: 3.14159265359 54 | has_acceleration_limits: true 55 | max_acceleration: 10 56 | wrist_1_joint: 57 | has_velocity_limits: true 58 | max_velocity: 3.14159265359 59 | has_acceleration_limits: true 60 | max_acceleration: 10 61 | wrist_2_joint: 62 | has_velocity_limits: true 63 | max_velocity: 3.14159265359 64 | has_acceleration_limits: true 65 | max_acceleration: 10 66 | wrist_3_joint: 67 | has_velocity_limits: true 68 | max_velocity: 3.14159265359 69 | has_acceleration_limits: true 70 | max_acceleration: 10 -------------------------------------------------------------------------------- /lara_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 -------------------------------------------------------------------------------- /lara_moveit_config/config/lara_base_scene.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 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 | -------------------------------------------------------------------------------- /lara_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | AnytimePathShortening: 3 | type: geometric::AnytimePathShortening 4 | shortcut: true # Attempt to shortcut all new solution paths 5 | hybridize: true # Compute hybrid solution trajectories 6 | max_hybrid_paths: 24 # Number of hybrid paths generated per iteration 7 | num_planners: 4 # The number of default planners to use for planning 8 | SBL: 9 | type: geometric::SBL 10 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 11 | EST: 12 | type: geometric::EST 13 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 14 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 15 | LBKPIECE: 16 | type: geometric::LBKPIECE 17 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 18 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | BKPIECE: 21 | type: geometric::BKPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 24 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 25 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 26 | KPIECE: 27 | type: geometric::KPIECE 28 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 29 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 30 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 31 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 32 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 33 | RRT: 34 | type: geometric::RRT 35 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 36 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 37 | RRTConnect: 38 | type: geometric::RRTConnect 39 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 40 | RRTstar: 41 | type: geometric::RRTstar 42 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 43 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 44 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 45 | TRRT: 46 | type: geometric::TRRT 47 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 48 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 49 | max_states_failed: 10 # when to start increasing temp. default: 10 50 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 51 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 52 | init_temperature: 10e-6 # initial temperature. default: 10e-6 53 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 54 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 55 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 56 | PRM: 57 | type: geometric::PRM 58 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 59 | PRMstar: 60 | type: geometric::PRMstar 61 | FMT: 62 | type: geometric::FMT 63 | num_samples: 1000 # number of states that the planner should sample. default: 1000 64 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 65 | nearest_k: 1 # use Knearest strategy. default: 1 66 | cache_cc: 1 # use collision checking cache. default: 1 67 | heuristics: 0 # activate cost to go heuristics. default: 0 68 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 69 | BFMT: 70 | type: geometric::BFMT 71 | num_samples: 1000 # number of states that the planner should sample. default: 1000 72 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 73 | nearest_k: 1 # use the Knearest strategy. default: 1 74 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 75 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 76 | heuristics: 1 # activates cost to go heuristics. default: 1 77 | cache_cc: 1 # use the collision checking cache. default: 1 78 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 79 | PDST: 80 | type: geometric::PDST 81 | STRIDE: 82 | type: geometric::STRIDE 83 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 84 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 85 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 86 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 87 | max_degree: 18 # max degree of a node in the GNAT. default: 12 88 | min_degree: 12 # min degree of a node in the GNAT. default: 12 89 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 90 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 91 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 92 | BiTRRT: 93 | type: geometric::BiTRRT 94 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 95 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 96 | init_temperature: 100 # initial temperature. default: 100 97 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 98 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 99 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 100 | LBTRRT: 101 | type: geometric::LBTRRT 102 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 103 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 104 | epsilon: 0.4 # optimality approximation factor. default: 0.4 105 | BiEST: 106 | type: geometric::BiEST 107 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 108 | ProjEST: 109 | type: geometric::ProjEST 110 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 111 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 112 | LazyPRM: 113 | type: geometric::LazyPRM 114 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 115 | LazyPRMstar: 116 | type: geometric::LazyPRMstar 117 | SPARS: 118 | type: geometric::SPARS 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 123 | SPARStwo: 124 | type: geometric::SPARStwo 125 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 126 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 127 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 128 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 129 | arm: 130 | planner_configs: 131 | - AnytimePathShortening 132 | - SBL 133 | - EST 134 | - LBKPIECE 135 | - BKPIECE 136 | - KPIECE 137 | - RRT 138 | - RRTConnect 139 | - RRTstar 140 | - TRRT 141 | - PRM 142 | - PRMstar 143 | - FMT 144 | - BFMT 145 | - PDST 146 | - STRIDE 147 | - BiTRRT 148 | - LBTRRT 149 | - BiEST 150 | - ProjEST 151 | - LazyPRM 152 | - LazyPRMstar 153 | - SPARS 154 | - SPARStwo 155 | projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) 156 | longest_valid_segment_fraction: 0.005 157 | gripper: 158 | planner_configs: 159 | - AnytimePathShortening 160 | - SBL 161 | - EST 162 | - LBKPIECE 163 | - BKPIECE 164 | - KPIECE 165 | - RRT 166 | - RRTConnect 167 | - RRTstar 168 | - TRRT 169 | - PRM 170 | - PRMstar 171 | - FMT 172 | - BFMT 173 | - PDST 174 | - STRIDE 175 | - BiTRRT 176 | - LBTRRT 177 | - BiEST 178 | - ProjEST 179 | - LazyPRM 180 | - LazyPRMstar 181 | - SPARS 182 | - SPARStwo 183 | -------------------------------------------------------------------------------- /lara_moveit_config/config/ros_controllers.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/lara_moveit_config/config/ros_controllers.yaml -------------------------------------------------------------------------------- /lara_moveit_config/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | sensors: 2 | [] -------------------------------------------------------------------------------- /lara_moveit_config/config/simple_moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: aubo_i5_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: True 6 | joints: 7 | - shoulder_pan_joint 8 | - shoulder_lift_joint 9 | - elbow_joint 10 | - wrist_1_joint 11 | - wrist_2_joint 12 | - wrist_3_joint 13 | # - name: gripper_controller 14 | # action_ns: gripper_cmd 15 | # default: True 16 | # type: GripperCommand 17 | # joints: 18 | # - left_outer_knuckle_joint -------------------------------------------------------------------------------- /lara_moveit_config/config/stomp_planning.yaml: -------------------------------------------------------------------------------- 1 | stomp/arm: 2 | group_name: arm 3 | optimization: 4 | num_timesteps: 60 5 | num_iterations: 40 6 | num_iterations_after_valid: 0 7 | num_rollouts: 30 8 | max_rollouts: 30 9 | initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] 10 | control_cost_weight: 0.0 11 | task: 12 | noise_generator: 13 | - class: stomp_moveit/NormalDistributionSampling 14 | stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] 15 | cost_functions: 16 | - class: stomp_moveit/CollisionCheck 17 | collision_penalty: 1.0 18 | cost_weight: 1.0 19 | kernel_window_percentage: 0.2 20 | longest_valid_joint_move: 0.05 21 | noisy_filters: 22 | - class: stomp_moveit/JointLimits 23 | lock_start: True 24 | lock_goal: True 25 | - class: stomp_moveit/MultiTrajectoryVisualization 26 | line_width: 0.02 27 | rgb: [255, 255, 0] 28 | marker_array_topic: stomp_trajectories 29 | marker_namespace: noisy 30 | update_filters: 31 | - class: stomp_moveit/PolynomialSmoother 32 | poly_order: 6 33 | - class: stomp_moveit/TrajectoryVisualization 34 | line_width: 0.05 35 | rgb: [0, 191, 255] 36 | error_rgb: [255, 0, 0] 37 | publish_intermediate: True 38 | marker_topic: stomp_trajectory 39 | marker_namespace: optimized 40 | stomp/gripper: 41 | group_name: gripper 42 | optimization: 43 | num_timesteps: 60 44 | num_iterations: 40 45 | num_iterations_after_valid: 0 46 | num_rollouts: 30 47 | max_rollouts: 30 48 | initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] 49 | control_cost_weight: 0.0 50 | task: 51 | noise_generator: 52 | - class: stomp_moveit/NormalDistributionSampling 53 | stddev: [0.05] 54 | cost_functions: 55 | - class: stomp_moveit/CollisionCheck 56 | collision_penalty: 1.0 57 | cost_weight: 1.0 58 | kernel_window_percentage: 0.2 59 | longest_valid_joint_move: 0.05 60 | noisy_filters: 61 | - class: stomp_moveit/JointLimits 62 | lock_start: True 63 | lock_goal: True 64 | - class: stomp_moveit/MultiTrajectoryVisualization 65 | line_width: 0.02 66 | rgb: [255, 255, 0] 67 | marker_array_topic: stomp_trajectories 68 | marker_namespace: noisy 69 | update_filters: 70 | - class: stomp_moveit/PolynomialSmoother 71 | poly_order: 6 72 | - class: stomp_moveit/TrajectoryVisualization 73 | line_width: 0.05 74 | rgb: [0, 191, 255] 75 | error_rgb: [255, 0, 0] 76 | publish_intermediate: True 77 | marker_topic: stomp_trajectory 78 | marker_namespace: optimized -------------------------------------------------------------------------------- /lara_moveit_config/launch/chomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 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 | 32 | 33 | [move_group/fake_controller_joint_states] 34 | 35 | 37 | 38 | [move_group/fake_controller_joint_states] 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/demo_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/gazebo_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/lara_base_scene_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/lara_base_scene_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 32 | 33 | 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 | 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 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/ros_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/simple_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/stomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /lara_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /lara_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the lara_base_scene with the MoveIt Motion Planning Framework 7 | 8 | Ian Chuang 9 | Ian Chuang 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | moveit_ros_move_group 20 | moveit_fake_controller_manager 21 | moveit_kinematics 22 | moveit_planners 23 | moveit_ros_visualization 24 | moveit_setup_assistant 25 | moveit_simple_controller_manager 26 | joint_state_publisher 27 | joint_state_publisher_gui 28 | robot_state_publisher 29 | rviz 30 | tf2_ros 31 | xacro 32 | 34 | 35 | 36 | 37 | 38 | lara_description 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /lara_moveit_servo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_moveit_servo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | moveit_servo 12 | ) 13 | 14 | catkin_package( 15 | # INCLUDE_DIRS include 16 | # LIBRARIES lara_moveit_servo 17 | # CATKIN_DEPENDS roscpp rospy std_msgs 18 | # DEPENDS system_lib 19 | ) 20 | 21 | ########### 22 | ## Build ## 23 | ########### 24 | 25 | ## Specify additional locations of header files 26 | ## Your package locations should be listed before other locations 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable( 33 | pose_tracking_server src/pose_tracking_server.cpp 34 | ) 35 | add_dependencies(pose_tracking_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 36 | target_link_libraries(pose_tracking_server ${catkin_LIBRARIES} ) -------------------------------------------------------------------------------- /lara_moveit_servo/config/joystick_servo_config.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | use_gazebo: false # Whether the robot is started in a Gazebo simulation environment 5 | 6 | ## Properties of incoming commands 7 | command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 8 | scale: 9 | # Scale parameters are only used if command_in_type=="unitless" 10 | linear: 5.0 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. 11 | rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. 12 | # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. 13 | joint: 0.01 14 | low_pass_filter_coeff: 20. # Larger --> trust the filtered data more, trust the measurements less. 15 | 16 | ## Properties of outgoing commands 17 | publish_period: 0.005 # 1/Nominal publish rate [seconds] 18 | low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) 22 | # or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) 23 | command_out_type: std_msgs/Float64MultiArray 24 | 25 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 26 | publish_joint_positions: true 27 | publish_joint_velocities: false 28 | publish_joint_accelerations: false 29 | 30 | ## MoveIt properties 31 | move_group_name: arm # Often 'manipulator' or 'arm' 32 | planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' 33 | 34 | ## Other frames 35 | ee_frame_name: teleop_link # The name of the end effector link, used to return the EE pose 36 | robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector 37 | 38 | ## Stopping behaviour 39 | incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command 40 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 41 | # Important because ROS may drop some messages and we need the robot to halt reliably. 42 | num_outgoing_halt_msgs_to_publish: 4 43 | 44 | ## Configure handling of singularities and joint limits 45 | lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) 46 | hard_stop_singularity_threshold: 30 # Stop when the condition number hits this 47 | joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 48 | 49 | ## Topic names 50 | cartesian_command_in_topic: cmd_vel # Topic for incoming Cartesian twist commands 51 | joint_command_in_topic: cmd_joint # Topic for incoming joint angle commands 52 | joint_topic: joint_states 53 | status_topic: status # Publish status to this topic 54 | command_out_topic: /arm_position_controller/command # Publish outgoing commands here 55 | 56 | ## Collision checking for the entire robot body 57 | check_collisions: true # Check collisions? 58 | collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. 59 | # Two collision check algorithms are available: 60 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 61 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 62 | collision_check_type: threshold_distance 63 | # Parameters for "threshold_distance"-type collision checking 64 | self_collision_proximity_threshold: 0.015 # Start decelerating when a self-collision is this far [m] 65 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 66 | # Parameters for "stop_distance"-type collision checking 67 | collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency 68 | min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] -------------------------------------------------------------------------------- /lara_moveit_servo/config/pose_tracking_servo_config.yaml: -------------------------------------------------------------------------------- 1 | ############################################### 2 | # Modify all parameters related to servoing here 3 | ############################################### 4 | use_gazebo: true # Whether the robot is started in a Gazebo simulation environment 5 | 6 | ## Properties of incoming commands 7 | command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s 8 | scale: 9 | # Scale parameters are only used if command_in_type=="unitless" 10 | linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. 11 | rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. 12 | # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. 13 | joint: 0.01 14 | low_pass_filter_coeff: 30. # Larger --> trust the filtered data more, trust the measurements less. 15 | 16 | ## Properties of outgoing commands 17 | publish_period: 0.005 # 1/Nominal publish rate [seconds] 18 | low_latency_mode: true # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) 19 | 20 | # What type of topic does your robot driver expect? 21 | # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) 22 | # or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) 23 | command_out_type: std_msgs/Float64MultiArray 24 | 25 | # What to publish? Can save some bandwidth as most robots only require positions or velocities 26 | publish_joint_positions: true 27 | publish_joint_velocities: false 28 | publish_joint_accelerations: false 29 | 30 | ## MoveIt properties 31 | move_group_name: arm # Often 'manipulator' or 'arm' 32 | planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' 33 | 34 | ## Other frames 35 | ee_frame_name: teleop_link # The name of the end effector link, used to return the EE pose 36 | robot_link_command_frame: teleop_link # commands must be given in the frame of a robot link. Usually either the base or end effector 37 | 38 | ## Stopping behaviour 39 | incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command 40 | # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. 41 | # Important because ROS may drop some messages and we need the robot to halt reliably. 42 | num_outgoing_halt_msgs_to_publish: 4 43 | 44 | ## Configure handling of singularities and joint limits 45 | lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) 46 | hard_stop_singularity_threshold: 30 # Stop when the condition number hits this 47 | joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. 48 | 49 | ## Topic names 50 | cartesian_command_in_topic: cmd_vel # Topic for incoming Cartesian twist commands 51 | joint_command_in_topic: cmd_joint # Topic for incoming joint angle commands 52 | joint_topic: joint_states 53 | status_topic: status # Publish status to this topic 54 | command_out_topic: /arm_position_controller/command # Publish outgoing commands here 55 | 56 | ## Collision checking for the entire robot body 57 | check_collisions: true # Check collisions? 58 | collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. 59 | # Two collision check algorithms are available: 60 | # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. 61 | # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits 62 | collision_check_type: threshold_distance 63 | # Parameters for "threshold_distance"-type collision checking 64 | self_collision_proximity_threshold: 0.015 # Start decelerating when a self-collision is this far [m] 65 | scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] 66 | # Parameters for "stop_distance"-type collision checking 67 | collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency 68 | min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] -------------------------------------------------------------------------------- /lara_moveit_servo/config/pose_tracking_settings.yaml: -------------------------------------------------------------------------------- 1 | ################################# 2 | # PID parameters for pose seeking 3 | ################################# 4 | 5 | # Maximum value of error integral for all PID controllers 6 | windup_limit: 0.05 7 | 8 | # PID gains 9 | x_proportional_gain: 500.0 10 | y_proportional_gain: 500.0 11 | z_proportional_gain: 500.0 12 | x_integral_gain: 1.0 13 | y_integral_gain: 1.0 14 | z_integral_gain: 1.0 15 | x_derivative_gain: 0.1 16 | y_derivative_gain: 0.1 17 | z_derivative_gain: 0.1 18 | 19 | angular_proportional_gain: 2.0 20 | angular_integral_gain: 1.0 21 | angular_derivative_gain: 0.1 22 | 23 | # PID gains 24 | # x_proportional_gain: 100 25 | # y_proportional_gain: 100 26 | # z_proportional_gain: 100 27 | # x_integral_gain: 1.0 28 | # y_integral_gain: 1.0 29 | # z_integral_gain: 1.0 30 | # x_derivative_gain: 0.1 31 | # y_derivative_gain: 0.1 32 | # z_derivative_gain: 0.1 33 | 34 | # angular_proportional_gain: 30 35 | # angular_integral_gain: 1.0 36 | # angular_derivative_gain: 0.1 -------------------------------------------------------------------------------- /lara_moveit_servo/launch/pose_tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /lara_moveit_servo/launch/servo_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /lara_moveit_servo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_moveit_servo 4 | 0.0.0 5 | The lara_moveit_servo package 6 | 7 | 8 | 9 | 10 | ian 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | moveit_servo 63 | moveit_servo 64 | moveit_servo 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /lara_moveit_servo/src/pose_tracking_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | static const std::string LOGNAME = "cpp_interface_example"; 11 | 12 | // Class for monitoring status of moveit_servo 13 | class StatusMonitor 14 | { 15 | public: 16 | StatusMonitor(ros::NodeHandle& nh, const std::string& topic) 17 | { 18 | sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); 19 | } 20 | 21 | private: 22 | void statusCB(const std_msgs::Int8ConstPtr& msg) 23 | { 24 | moveit_servo::StatusCode latest_status = static_cast(msg->data); 25 | if (latest_status != status_) 26 | { 27 | status_ = latest_status; 28 | const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); 29 | ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); 30 | } 31 | } 32 | moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; 33 | ros::Subscriber sub_; 34 | }; 35 | 36 | /** 37 | * Instantiate the pose tracking interface. 38 | * Send a pose slightly different from the starting pose 39 | * Then keep updating the target pose a little bit 40 | */ 41 | int main(int argc, char** argv) 42 | { 43 | ros::init(argc, argv, LOGNAME); 44 | ros::NodeHandle nh("~"); 45 | ros::AsyncSpinner spinner(8); 46 | spinner.start(); 47 | 48 | // Load the planning scene monitor 49 | planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; 50 | planning_scene_monitor = std::make_shared("robot_description"); 51 | if (!planning_scene_monitor->getPlanningScene()) 52 | { 53 | ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); 54 | exit(EXIT_FAILURE); 55 | } 56 | 57 | planning_scene_monitor->startSceneMonitor(); 58 | planning_scene_monitor->startWorldGeometryMonitor( 59 | planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, 60 | planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, 61 | false /* skip octomap monitor */); 62 | planning_scene_monitor->startStateMonitor(); 63 | 64 | // Create the pose tracker 65 | moveit_servo::PoseTracking tracker(nh, planning_scene_monitor); 66 | 67 | // Subscribe to servo status (and log it when it changes) 68 | StatusMonitor status_monitor(nh, "status"); 69 | 70 | Eigen::Vector3d lin_tol{ 0.01, 0.01, 0.01 }; 71 | double rot_tol = 0.1; 72 | 73 | // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple 74 | // waypoints 75 | tracker.resetTargetPose(); 76 | 77 | while (ros::ok()) { 78 | // Run the pose tracking in a new thread 79 | tracker.moveToPose(lin_tol, rot_tol, 1 /* target pose timeout */); 80 | } 81 | 82 | // Make sure the tracker is stopped and clean up 83 | tracker.stopMotion(); 84 | 85 | return EXIT_SUCCESS; 86 | } 87 | -------------------------------------------------------------------------------- /lara_simulation_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_simulation_tools) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | control_msgs 13 | gazebo_msgs 14 | geometry_msgs 15 | roscpp 16 | rospy 17 | std_msgs 18 | std_srvs 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 42 | ## but can be declared for certainty nonetheless: 43 | ## * add a exec_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | ## Generate messages in the 'msg' folder 55 | # add_message_files( 56 | # FILES 57 | # Message1.msg 58 | # Message2.msg 59 | # ) 60 | 61 | ## Generate services in the 'srv' folder 62 | # add_service_files( 63 | # FILES 64 | # Service1.srv 65 | # Service2.srv 66 | # ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | # generate_messages( 77 | # DEPENDENCIES 78 | # control_msgs# gazebo_msgs# geometry_msgs# std_msgs 79 | # ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | # generate_dynamic_reconfigure_options( 97 | # cfg/DynReconf1.cfg 98 | # cfg/DynReconf2.cfg 99 | # ) 100 | 101 | ################################### 102 | ## catkin specific configuration ## 103 | ################################### 104 | ## The catkin_package macro generates cmake config files for your package 105 | ## Declare things to be passed to dependent projects 106 | ## INCLUDE_DIRS: uncomment this if your package contains header files 107 | ## LIBRARIES: libraries you create in this project that dependent projects also need 108 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 109 | ## DEPENDS: system dependencies of this project that dependent projects also need 110 | catkin_package( 111 | # INCLUDE_DIRS include 112 | # LIBRARIES lara_simulation_tools 113 | # CATKIN_DEPENDS actionlib control_msgs gazebo_msgs geometry_msgs roscpp rospy std_msgs std_srvs 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | # include 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/lara_simulation_tools.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/lara_simulation_tools_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | # target_link_libraries(${PROJECT_NAME}_node 155 | # ${catkin_LIBRARIES} 156 | # ) 157 | 158 | ############# 159 | ## Install ## 160 | ############# 161 | 162 | # all install targets should use catkin DESTINATION variables 163 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 164 | 165 | ## Mark executable scripts (Python etc.) for installation 166 | ## in contrast to setup.py, you can choose the destination 167 | # catkin_install_python(PROGRAMS 168 | # scripts/my_python_script 169 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark executables for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 174 | # install(TARGETS ${PROJECT_NAME}_node 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark libraries for installation 179 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 180 | # install(TARGETS ${PROJECT_NAME} 181 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 184 | # ) 185 | 186 | ## Mark cpp header files for installation 187 | # install(DIRECTORY include/${PROJECT_NAME}/ 188 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 189 | # FILES_MATCHING PATTERN "*.h" 190 | # PATTERN ".svn" EXCLUDE 191 | # ) 192 | 193 | ## Mark other files for installation (e.g. launch and bag files, etc.) 194 | # install(FILES 195 | # # myfile1 196 | # # myfile2 197 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 198 | # ) 199 | 200 | ############# 201 | ## Testing ## 202 | ############# 203 | 204 | ## Add gtest based cpp test target and link libraries 205 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lara_simulation_tools.cpp) 206 | # if(TARGET ${PROJECT_NAME}-test) 207 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 208 | # endif() 209 | 210 | ## Add folders to be run by python nosetests 211 | # catkin_add_nosetests(test) 212 | -------------------------------------------------------------------------------- /lara_simulation_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_simulation_tools 4 | 0.0.0 5 | The lara_simulation_tools package 6 | 7 | 8 | 9 | 10 | ian 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | actionlib 53 | control_msgs 54 | gazebo_msgs 55 | geometry_msgs 56 | roscpp 57 | rospy 58 | std_msgs 59 | std_srvs 60 | actionlib 61 | control_msgs 62 | gazebo_msgs 63 | geometry_msgs 64 | roscpp 65 | rospy 66 | std_msgs 67 | std_srvs 68 | actionlib 69 | control_msgs 70 | gazebo_msgs 71 | geometry_msgs 72 | roscpp 73 | rospy 74 | std_msgs 75 | std_srvs 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /lara_simulation_tools/src/scene_manager.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rospkg 4 | import rospy 5 | import xacro 6 | import gazebo_msgs.srv 7 | from std_msgs.msg import ColorRGBA 8 | from geometry_msgs.msg import Pose, Point, Quaternion 9 | # import controller_manager_msgs.srv 10 | from control_msgs.msg import FollowJointTrajectoryActionGoal, FollowJointTrajectoryAction 11 | import actionlib 12 | import std_srvs.srv 13 | import std_msgs.msg 14 | from threading import Thread 15 | import os 16 | 17 | object_db = { 18 | 'plastic_bin' : { 19 | 'mesh': 'plastic_bin.stl', 20 | 'mass': 1.1022530221461508, 21 | 'iox': 2.5908873064430885e-17, 22 | 'ioy': -6.47721826610772e-17, 23 | 'ioz': 0.0466345179467445, 24 | 'ixx': .0084861, 25 | 'iyy': .0163272, 26 | 'izz': .0183834, 27 | 'ixy': 0, 28 | 'iyz': 0, 29 | 'ixz': 0, 30 | }, 31 | 'cube': { 32 | 'mesh': 'cube.stl', 33 | 'mass': .006784, 34 | 'iox': 0, 35 | 'ioy': 0, 36 | 'ioz': 0.02, 37 | 'ixx': .18e-05, 38 | 'iyy': .18e-05, 39 | 'izz': .18e-05, 40 | 'ixy': 0, 41 | 'iyz': 0, 42 | 'ixz': 0, 43 | }, 44 | } 45 | 46 | class SceneManager: 47 | def __init__(self, package_name, object_db): 48 | rospack = rospkg.RosPack() 49 | self.package_path = rospack.get_path(package_name) 50 | self.object_db = object_db 51 | 52 | self.reference_frame = 'world' 53 | 54 | def set_color(self, model_name, color): 55 | req = gazebo_msgs.srv.SetLightPropertiesRequest() 56 | req.diffuse = color 57 | req.attenuation_constant = 0.0 58 | req.attenuation_linear = 0.0 59 | req.attenuation_quadratic = 0.0 60 | 61 | self.send_request( 62 | f'/gazebo_color_plugin/{model_name}', 63 | gazebo_msgs.srv.SetLightProperties, 64 | req 65 | ) 66 | 67 | def set_pose(self, model_name, pose): 68 | req = gazebo_msgs.srv.SetModelStateRequest() 69 | req.model_state.model_name = model_name 70 | req.model_state.pose = pose 71 | req.model_state.reference_frame = self.reference_frame 72 | 73 | self.send_request( 74 | f'/gazebo/set_model_state', 75 | gazebo_msgs.srv.SetModelState, 76 | req 77 | ) 78 | 79 | def reset_arm(self): 80 | 81 | client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) 82 | client.wait_for_server() 83 | goal = FollowJointTrajectoryActionGoal() 84 | goal.path 85 | 86 | traj = JointTrajectory() 87 | traj.joint_names = [ 88 | 'elbow_joint' 89 | 'shoulder_lift_joint' 90 | 'shoulder_pan_joint' 91 | 'wrist_1_joint' 92 | 'wrist_2_joint' 93 | 'wrist_3_joint' 94 | ] 95 | point = JointTrajectoryPoint() 96 | point.positions = [0,0,0,0,0,0] 97 | point.velocities = [0,0,0,0,0,0] 98 | point.accelerations = [0,0,0,0,0,0] 99 | point.effort = [] 100 | point.time_from_start = rospy.Duration(0) 101 | traj.points = [point] 102 | 103 | pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1) 104 | pub.publish(traj) 105 | print('done') 106 | 107 | 108 | # req = controller_manager_msgs.srv.SwitchControllerRequest() 109 | # req.start_controllers = ['arm_position_controller', 'gripper_position_controller'] 110 | # req.stop_controllers = ['arm_controller', 'gripper_controller'] 111 | # req.strictness = req.STRICT 112 | 113 | # self.send_request( 114 | # f'/controller_manager/switch_controller', 115 | # controller_manager_msgs.srv.SwitchController, 116 | # req 117 | # ) 118 | 119 | # msg = std_ 120 | 121 | 122 | def spawn_model(self, model_name, id, color, pose): 123 | mappings = self.object_db[id].copy() 124 | mappings['model_name'] = model_name 125 | mappings['color'] = f"{color.r} {color.g} {color.b} {color.a}" 126 | mappings = {key: str(val) for key, val in mappings.items()} 127 | xml = xacro.process_file( 128 | os.path.join(self.package_path, 'urdf/parts/object.xacro'), 129 | mappings=mappings 130 | ).toxml() 131 | 132 | req = gazebo_msgs.srv.SpawnModelRequest() 133 | req.model_name = model_name 134 | req.model_xml = xml 135 | req.initial_pose = pose 136 | req.reference_frame = self.reference_frame 137 | 138 | self.send_request( 139 | '/gazebo/spawn_urdf_model', 140 | gazebo_msgs.srv.SpawnModel, 141 | req 142 | ) 143 | 144 | def send_request(self, service_name, service_type, req): 145 | try: 146 | rospy.wait_for_service(service_name) 147 | client = rospy.ServiceProxy(service_name, service_type) 148 | resp = client(req) 149 | 150 | except rospy.ServiceException as e: 151 | print("Gazebo Service call failed: %s"%e) 152 | 153 | def send_request(self, service_name, service_type, req): 154 | pub = rospy.Publisher('chatter', String, queue_size=1) 155 | 156 | 157 | if __name__ == '__main__': 158 | 159 | objects = [ 160 | { 161 | 'id': 'cube', 162 | 'model_name': 'cube1', 163 | 'color' : ColorRGBA(255, 0, 0, 1), 164 | 'pose': Pose(Point(0.3, 0.2, 0), Quaternion(0,0,0,1)), 165 | }, 166 | { 167 | 'id': 'cube', 168 | 'model_name': 'cube2', 169 | 'color' : ColorRGBA(0, 255, 0, 1), 170 | 'pose': Pose(Point(0.4, 0.2, 0), Quaternion(0,0,0,1)), 171 | }, 172 | { 173 | 'id': 'cube', 174 | 'model_name': 'cube3', 175 | 'color' : ColorRGBA(0, 0, 255, 1), 176 | 'pose': Pose(Point(0.5, 0.2, 0), Quaternion(0,0,0,1)), 177 | }, 178 | { 179 | 'id': 'plastic_bin', 180 | 'model_name': 'plastic_bin1', 181 | 'color' : ColorRGBA(0, 153, 0, 1), 182 | 'pose': Pose(Point(0.4, -0.2, 0), Quaternion(0,0,0,1)), 183 | } 184 | ] 185 | 186 | rospy.init_node('scene_manager') 187 | scene_manager = SceneManager('homestri_robot_description', object_db) 188 | 189 | # for object in objects: 190 | # scene_manager.spawn_model(**object) 191 | 192 | # rospy.sleep(3) 193 | 194 | # scene_manager.set_color('plastic_bin1', ColorRGBA(255, 255, 255, 1)) 195 | # scene_manager.set_pose('plastic_bin1', Pose(Point(0.4, -0.4, 0), Quaternion(0,0,0,1))) 196 | 197 | scene_manager.reset_arm() 198 | 199 | 200 | rospy.spin() 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | -------------------------------------------------------------------------------- /lara_simulation_tools/urdf/object.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 | /gazebo_color_plugin/$(arg model_name) 28 | $(arg color) 29 | 30 | 31 | 1000000.0 32 | 1.0 33 | 0.8 34 | 0.8 35 | 0.0 36 | 0.001 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /lara_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lara_teleop) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | aubo_msgs 12 | joy 13 | rospy 14 | sensor_msgs 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # aubo_msgs# sensor_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES lara_teleop 109 | # CATKIN_DEPENDS aubo_msgs joy rospy sensor_msgs 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/lara_teleop.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/lara_teleop_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # catkin_install_python(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables for installation 169 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 170 | # install(TARGETS ${PROJECT_NAME}_node 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark libraries for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 176 | # install(TARGETS ${PROJECT_NAME} 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lara_teleop.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /lara_teleop/launch/aubo_joy_teleop.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /lara_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lara_teleop 4 | 0.0.0 5 | The lara_teleop package 6 | 7 | 8 | 9 | 10 | laraauboi5 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | aubo_msgs 53 | joy 54 | rospy 55 | sensor_msgs 56 | aubo_msgs 57 | joy 58 | rospy 59 | sensor_msgs 60 | aubo_msgs 61 | joy 62 | rospy 63 | sensor_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /lara_teleop/scripts/aubo_joy_teleop.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import geometry_msgs.msg 5 | import sensor_msgs.msg 6 | from aubo_msgs.msg import TeachCommand 7 | from dh_gripper_msgs.msg import GripperCtrl 8 | from threading import Lock 9 | from copy import deepcopy 10 | 11 | class AuboJoyTeleop(): 12 | def __init__(self): 13 | self.teach_pub = rospy.Publisher('/teach', TeachCommand, queue_size=1) 14 | self.gripper_pub = rospy.Publisher('/gripper/ctrl', GripperCtrl, queue_size=1) 15 | self.joy_sub = rospy.Subscriber("/joy", sensor_msgs.msg.Joy, self.callback) 16 | 17 | self.last_teach_command= None 18 | 19 | self.joy_msg = None 20 | self.joy_msg_mutex = Lock() 21 | 22 | def publish(self): 23 | self.joy_msg_mutex.acquire() 24 | msg = deepcopy(self.joy_msg) 25 | self.joy_msg_mutex.release() 26 | 27 | if msg != None: 28 | 29 | a = msg.buttons[0] 30 | b = msg.buttons[1] 31 | x = msg.buttons[2] 32 | y = msg.buttons[3] 33 | lb = msg.buttons[4] 34 | rb = msg.buttons[5] 35 | # ljoy_x = msg.axes[0] 36 | # ljoy_y = msg.axes[1] 37 | # rjoy_x = msg.axes[3] 38 | # rjoy_y = msg.axes[4] 39 | dpad_x = msg.axes[6] 40 | dpad_y = msg.axes[7] 41 | 42 | mov_x_plus = dpad_y == 1 43 | mov_x_minus = dpad_y == -1 44 | mov_y_plus = dpad_x == 1 45 | mov_y_minus = dpad_x == -1 46 | mov_z_plus = rb == 1 47 | mov_z_minus = lb == 1 48 | rot_z_plus = x == 1 49 | rot_z_minus = y == 1 50 | close_gripper = a == 1 51 | open_gripper = b == 1 52 | 53 | num_commands = sum([mov_x_plus, mov_x_minus, mov_y_plus, mov_y_minus, mov_z_plus, mov_z_minus, rot_z_plus, rot_z_minus]) 54 | 55 | msg = TeachCommand() 56 | if num_commands != 1: 57 | msg.command = TeachCommand.STOP 58 | elif mov_x_plus: 59 | msg.command = TeachCommand.MOV_X 60 | msg.direction = TeachCommand.POSITIVE 61 | elif mov_x_minus: 62 | msg.command = TeachCommand.MOV_X 63 | msg.direction = TeachCommand.NEGATIVE 64 | elif mov_y_plus: 65 | msg.command = TeachCommand.MOV_Y 66 | msg.direction = TeachCommand.POSITIVE 67 | elif mov_y_minus: 68 | msg.command = TeachCommand.MOV_Y 69 | msg.direction = TeachCommand.NEGATIVE 70 | elif mov_z_plus: 71 | msg.command = TeachCommand.MOV_Z 72 | msg.direction = TeachCommand.POSITIVE 73 | elif mov_z_minus: 74 | msg.command = TeachCommand.MOV_Z 75 | msg.direction = TeachCommand.NEGATIVE 76 | elif rot_z_plus: 77 | msg.command = TeachCommand.ROT_Z 78 | msg.direction = TeachCommand.POSITIVE 79 | elif rot_z_minus: 80 | msg.command = TeachCommand.ROT_Z 81 | msg.direction = TeachCommand.NEGATIVE 82 | 83 | self.teach_pub.publish(msg) 84 | 85 | 86 | num_commands = sum([close_gripper, open_gripper]) 87 | 88 | msg = GripperCtrl() 89 | msg.initialize = False 90 | msg.speed = 100 91 | msg.force = 100 92 | if num_commands != 1: 93 | pass 94 | elif close_gripper: 95 | msg.position = 0 96 | self.gripper_pub.publish(msg) 97 | elif open_gripper: 98 | msg.position = 1000 99 | self.gripper_pub.publish(msg) 100 | 101 | 102 | def callback(self, msg): 103 | self.joy_msg_mutex.acquire() 104 | self.joy_msg = msg 105 | self.joy_msg_mutex.release() 106 | 107 | 108 | 109 | if __name__ == '__main__': 110 | rospy.init_node('aubo_joy_teleop') 111 | 112 | aubo_joy_teleop = AuboJoyTeleop() 113 | 114 | rate = rospy.Rate(60) # 10hz 115 | while not rospy.is_shutdown(): 116 | aubo_joy_teleop.publish() 117 | rate.sleep() 118 | 119 | rospy.spin() -------------------------------------------------------------------------------- /media/aubo_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/media/aubo_gazebo.gif -------------------------------------------------------------------------------- /media/aubo_vr.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ian-chuang/LARA_AUBOi5_AG95/0403d14e7f9cd7f4b2a47e29e9511b2277d2fbb0/media/aubo_vr.gif --------------------------------------------------------------------------------