├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── ur5_controllers.yaml ├── include └── moveit_sim_controller │ └── moveit_sim_hw_interface.h ├── launch ├── ur5.rviz ├── ur5_rviz.launch └── ur5_sim_controller.launch ├── package.xml ├── resources └── screenshot.png └── src ├── moveit_sim_hw_interface.cpp └── moveit_sim_hw_main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci package. 2 | sudo: required 3 | dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro 4 | services: 5 | - docker 6 | language: cpp 7 | compiler: gcc 8 | cache: ccache 9 | 10 | env: 11 | matrix: 12 | # - TEST="clang-format catkin_lint" 13 | - ROS_DISTRO=melodic 14 | - ROS_DISTRO=kinetic 15 | 16 | before_script: 17 | - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci 18 | 19 | script: 20 | - .moveit_ci/travis.sh 21 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package moveit_sim_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2019-09-18) 6 | ------------------ 7 | * Cleanup UR5 example (`#2 `_) 8 | * Update README 9 | * Add example simulated robot with UR5 10 | * Contributors: Dave Coleman 11 | 12 | 0.1.0 (2016-10-20) 13 | ------------------ 14 | * Add C++11 support 15 | * Fixed occational Nan 16 | * Minor formatting 17 | * added roslint and roslint changes 18 | * Contributors: Dave Coleman 19 | 20 | 0.0.5 (2016-01-13) 21 | ------------------ 22 | * Fixed deprecated API for rosparam_shortcuts 23 | * Contributors: Dave Coleman 24 | 25 | 0.0.4 (2015-12-27) 26 | ------------------ 27 | * Improved rosparam_shortcuts api 28 | * Fixed initialization of default joint positions 29 | * Fix travis 30 | * Contributors: Dave Coleman 31 | 32 | 0.0.3 (2015-12-10) 33 | ------------------ 34 | * Fix missing dep 35 | * Contributors: Dave Coleman 36 | 37 | 0.0.2 (2015-12-10) 38 | ------------------ 39 | * Initial release 40 | * Contributors: Dave Coleman 41 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(moveit_sim_controller) 3 | 4 | # C++ 11 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | moveit_core 9 | moveit_ros_planning 10 | ros_control_boilerplate 11 | rosparam_shortcuts 12 | roscpp 13 | roslint 14 | ) 15 | 16 | ################################### 17 | ## catkin specific configuration ## 18 | ################################### 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | LIBRARIES moveit_sim_hw_interface 23 | CATKIN_DEPENDS 24 | moveit_core 25 | moveit_ros_planning 26 | ros_control_boilerplate 27 | rosparam_shortcuts 28 | roscpp 29 | ) 30 | 31 | include_directories(include) 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | ########### 37 | ## Build ## 38 | ########### 39 | 40 | # Hardware Interface 41 | add_library(moveit_sim_hw_interface src/moveit_sim_hw_interface.cpp) 42 | target_link_libraries(moveit_sim_hw_interface 43 | ${catkin_LIBRARIES} 44 | ) 45 | 46 | ## Main executable 47 | add_executable(moveit_sim_hw_main src/moveit_sim_hw_main.cpp) 48 | target_link_libraries(moveit_sim_hw_main 49 | moveit_sim_hw_interface 50 | ${catkin_LIBRARIES} 51 | ) 52 | 53 | ############# 54 | ## Testing ## 55 | ############# 56 | 57 | ## Test for correct C++ source code 58 | roslint_cpp() 59 | 60 | ############# 61 | ## Install ## 62 | ############# 63 | 64 | ## Mark executables and/or libraries for installation 65 | install(TARGETS moveit_sim_hw_interface moveit_sim_hw_main 66 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | ) 70 | 71 | ## Mark cpp header files for installation 72 | install(DIRECTORY include/${PROJECT_NAME}/ 73 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 74 | FILES_MATCHING PATTERN "*.h" 75 | PATTERN ".svn" EXCLUDE 76 | ) 77 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Dave Coleman 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of moveit_sim_controller nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MoveIt Simulator Controller 2 | 3 | A simulation interface for a hardware interface for ros_control, and loads default joint values from SRDF 4 | 5 | Intended to replace ``moveit_fake_controller_manager`` - this repo almost exactly replicates a ros_control hardware setup, and has the new feature of being able to specify an inital position. 6 | 7 | 8 | 9 | This open source project was developed at [PickNik Robotics](https://picknik.ai/). Need professional ROS development and consulting? Contact us at projects@picknik.ai for a free consultation. 10 | 11 | ## Status: 12 | 13 | * [![Build Status](https://travis-ci.org/PickNikRobotics/moveit_sim_controller.svg)](https://travis-ci.org/ros-planning/moveit_sim_controller) Travis CI 14 | * [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_sim_controller__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_sim_controller__ubuntu_xenial_amd64__binary/) ROS Buildfarm - AMD64 Xenial Debian Build - Ubuntu 16.04 LTS 15 | * [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit_sim_controller__ubuntu_xenial_amd64)](http://build.ros.org/view/Kdev/job/Kdev__moveit_sim_controller__ubuntu_xenial_amd64/) ROS Buildfarm - AMD64 Xenial Devel Build - Ubuntu 16.04 LTS 16 | * [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_sim_controller__ubuntu_bionic__source)](http://build.ros.org/job/Msrc_uB__moveit_sim_controller__ubuntu_bionic__source/) ROS Buildfarm - AMD64 Bionic Source Build - Ubuntu 18.04 LTS 17 | * [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit_sim_controller__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit_sim_controller__ubuntu_bionic_amd64/) ROS Buildfarm - AMD64 Bionic Devel Build - Ubuntu 18.04 LTS 18 | 19 | ![](resources/screenshot.png) 20 | 21 | ## Install 22 | 23 | ### Ubuntu Debian 24 | 25 | ``` 26 | sudo apt-get install ros-melodic-moveit-sim-controller 27 | ``` 28 | 29 | ## Code API 30 | 31 | See [Class Reference](http://docs.ros.org/melodic/api/moveit_sim_controller/html/) 32 | 33 | ## Quick Start 34 | 35 | Our example uses the UR5 robot: 36 | 37 | sudo apt-get install ros-melodic-ur5-moveit-config 38 | roslaunch moveit_sim_controller ur5_rviz.launch 39 | roslaunch moveit_sim_controller ur5_sim_controller.launch 40 | 41 | You should see the robot launch in Rviz with the arm oriented straight up, which is not the zero/home position. You can change the start position by editing in ``config/ur5_controllers.yaml`` the value ``joint_model_group_pose``. 42 | 43 | With this simulator you should also be able to use ``rostopic echo`` to see the ``joint_states`` and ``tf`` it is publishing - essentially you have just simulated a full ros_control-based robot without needing hardware. To test with an example joint trajectory, see the demo code in [ros_control_boilerplate](https://github.com/davetcoleman/ros_control_boilerplate). 44 | 45 | ## Usage 46 | 47 | To set your robot's initial simulated position, create a *planning group* in your SRDF using the *MoveIt Setup Assistant* named something like ``whole_body`` or ``arm`` that contains all of your robot's joints. Then, create a *pose* for the planning group that is your start position, and name it something like ``home``. 48 | 49 | Then load this node with the following ROS params (yaml is suggested use): 50 | 51 | # MoveIt-specific simulation settings: 52 | moveit_sim_hw_interface: 53 | joint_model_group: arm 54 | joint_model_group_pose: home 55 | 56 | See [ros_control_boilerplate](https://github.com/davetcoleman/ros_control_boilerplate) for more detailed instructions about using ros_control to visualize your robot - this package simply inherits from that package and adds some MoveIt! dependencies that can parse SRDFs for your initial state. 57 | 58 | ## Testing and Linting 59 | 60 | To run [roslint](http://wiki.ros.org/roslint), use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 61 | 62 | catkin build --no-status --no-deps --this --make-args roslint 63 | 64 | To run [catkin lint](https://pypi.python.org/pypi/catkin_lint), use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 65 | 66 | catkin lint -W2 67 | 68 | There are currently no unit or integration tests for this package. If there were you would use the following command with [catkin-tools](https://catkin-tools.readthedocs.org/): 69 | 70 | catkin run_tests --no-deps --this -i 71 | 72 | ## Contribute 73 | 74 | Please send PRs for new helper functions, fixes, etc! 75 | -------------------------------------------------------------------------------- /config/ur5_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Simulation settings 2 | generic_hw_control_loop: 3 | loop_hz: 100 4 | cycle_time_error_threshold: 0.02 5 | 6 | # MoveIt-specific simulation settings 7 | moveit_sim_hw_interface: 8 | joint_model_group: manipulator 9 | joint_model_group_pose: up 10 | 11 | # Settings for generic ros_control hardware interface 12 | hardware_interface: 13 | sim_control_mode: 0 # position 14 | joints: 15 | - elbow_joint 16 | - shoulder_lift_joint 17 | - shoulder_pan_joint 18 | - wrist_1_joint 19 | - wrist_2_joint 20 | - wrist_3_joint 21 | 22 | # Publish all joint states ---------------------------------- 23 | joint_state_controller: 24 | type: joint_state_controller/JointStateController 25 | publish_rate: 100 26 | 27 | # Position Joint Trajectory Controller ------------------------------- 28 | # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller 29 | position_trajectory_controller: 30 | type: position_controllers/JointTrajectoryController 31 | joints: 32 | - elbow_joint 33 | - shoulder_lift_joint 34 | - shoulder_pan_joint 35 | - wrist_1_joint 36 | - wrist_2_joint 37 | - wrist_3_joint 38 | constraints: 39 | goal_time: 1.0 # If the timestamp of the goal trajectory point is t, then following the trajectory succeeds if it reaches the goal within t +/- goal_time, and aborts otherwise. 40 | stopped_velocity_tolerance: 0.01 # Velocity to be considered approximately equal to zero -------------------------------------------------------------------------------- /include/moveit_sim_controller/moveit_sim_hw_interface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Simulates a robot using ros_control controllers 37 | */ 38 | 39 | #ifndef MOVEIT_SIM_CONTROLLER_MOVEIT_SIM_HW_INTERFACE_H 40 | #define MOVEIT_SIM_CONTROLLER_MOVEIT_SIM_HW_INTERFACE_H 41 | 42 | // C++ 43 | #include 44 | 45 | // ROS 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | namespace moveit_sim_controller 52 | { 53 | static const std::string ROBOT_DESCRIPTION = "robot_description"; 54 | 55 | class MoveItSimHWInterface : public ros_control_boilerplate::SimHWInterface 56 | { 57 | public: 58 | /** 59 | * \brief Constructor 60 | */ 61 | explicit MoveItSimHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model = NULL); 62 | 63 | /** \brief Initialize the robot hardware interface */ 64 | void init(); 65 | 66 | void loadDefaultJointValues(); 67 | 68 | private: 69 | std::string name_; 70 | 71 | std::string joint_model_group_; 72 | std::string joint_model_group_pose_; 73 | 74 | // Note: this doesn't need to be a member variable (only used once) but there are warnings about 75 | // unloading shared objects so this is a work around at least for now 76 | robot_model_loader::RobotModelLoaderPtr robot_model_loader_; 77 | }; // class 78 | 79 | // Create boost pointers for this class 80 | typedef boost::shared_ptr MoveItSimHWInterfacePtr; 81 | typedef boost::shared_ptr MoveItSimHWInterfaceConstPtr; 82 | 83 | } // namespace moveit_sim_controller 84 | 85 | #endif // MOVEIT_SIM_CONTROLLER_MOVEIT_SIM_HW_INTERFACE_H 86 | -------------------------------------------------------------------------------- /launch/ur5.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 | Splitter Ratio: 0.5 10 | Tree Height: 599 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | base_link: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | Value: true 70 | ee_link: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | forearm_link: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | shoulder_link: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | tool0: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | upper_arm_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | world: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | wrist_1_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | wrist_2_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | wrist_3_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | Name: RobotModel 114 | Robot Description: robot_description 115 | TF Prefix: "" 116 | Update Interval: 0 117 | Value: true 118 | Visual Enabled: true 119 | Enabled: true 120 | Global Options: 121 | Background Color: 48; 48; 48 122 | Default Light: true 123 | Fixed Frame: world 124 | Frame Rate: 30 125 | Name: root 126 | Tools: 127 | - Class: rviz/Interact 128 | Hide Inactive Objects: true 129 | - Class: rviz/MoveCamera 130 | - Class: rviz/Select 131 | - Class: rviz/FocusCamera 132 | - Class: rviz/Measure 133 | - Class: rviz/SetInitialPose 134 | Topic: /initialpose 135 | - Class: rviz/SetGoal 136 | Topic: /move_base_simple/goal 137 | - Class: rviz/PublishPoint 138 | Single click: true 139 | Topic: /clicked_point 140 | Value: true 141 | Views: 142 | Current: 143 | Class: rviz/Orbit 144 | Distance: 2.49391556 145 | Enable Stereo Rendering: 146 | Stereo Eye Separation: 0.0599999987 147 | Stereo Focal Distance: 1 148 | Swap Stereo Eyes: false 149 | Value: false 150 | Focal Point: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Focal Shape Fixed Size: true 155 | Focal Shape Size: 0.0500000007 156 | Invert Z Axis: false 157 | Name: Current View 158 | Near Clip Distance: 0.00999999978 159 | Pitch: 0.135397971 160 | Target Frame: 161 | Value: Orbit (rviz) 162 | Yaw: 0.935397983 163 | Saved: ~ 164 | Window Geometry: 165 | Displays: 166 | collapsed: false 167 | Height: 846 168 | Hide Left Dock: false 169 | Hide Right Dock: false 170 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ebfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000042000002eb000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002ebfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000042000002eb000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650000000000000004b00000027000fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002eb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 171 | Selection: 172 | collapsed: false 173 | Time: 174 | collapsed: false 175 | Tool Properties: 176 | collapsed: false 177 | Views: 178 | collapsed: false 179 | Width: 1200 180 | X: 275 181 | Y: 27 182 | -------------------------------------------------------------------------------- /launch/ur5_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/ur5_sim_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_sim_controller 4 | 0.2.0 5 | A simulation interface for a hardware interface for ros_control, and loads default joint values from SRDF 6 | 7 | Dave Coleman 8 | 9 | BSD 10 | 11 | https://github.com/davetcoleman/moveit_sim_controller 12 | https://github.com/davetcoleman/moveit_sim_controller/issues 13 | https://github.com/davetcoleman/moveit_sim_controller/ 14 | 15 | Dave Coleman 16 | 17 | catkin 18 | 19 | moveit_core 20 | moveit_ros_planning 21 | ros_control_boilerplate 22 | rosparam_shortcuts 23 | roscpp 24 | roslint 25 | 26 | moveit_core 27 | moveit_ros_planning 28 | ros_control_boilerplate 29 | rosparam_shortcuts 30 | roscpp 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /resources/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/moveit_sim_controller/9425c50b557a3927a10ed394a827f1f099c84049/resources/screenshot.png -------------------------------------------------------------------------------- /src/moveit_sim_hw_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Simulates a robot using ros_control controllers with a default position loaded from MoveIt! 37 | */ 38 | 39 | #include 40 | 41 | // ROS parameter loading 42 | #include 43 | 44 | namespace moveit_sim_controller 45 | { 46 | MoveItSimHWInterface::MoveItSimHWInterface(ros::NodeHandle& nh, urdf::Model* urdf_model) 47 | : ros_control_boilerplate::SimHWInterface(nh, urdf_model), name_("moveit_sim_hw_interface") 48 | { 49 | // Load rosparams 50 | ros::NodeHandle rpnh(nh_, name_); 51 | std::size_t error = 0; 52 | error += !rosparam_shortcuts::get(name_, rpnh, "joint_model_group", joint_model_group_); 53 | error += !rosparam_shortcuts::get(name_, rpnh, "joint_model_group_pose", joint_model_group_pose_); 54 | rosparam_shortcuts::shutdownIfError(name_, error); 55 | } 56 | 57 | void MoveItSimHWInterface::init() 58 | { 59 | // Call parent class version of this function 60 | SimHWInterface::init(); 61 | 62 | // Load the loader 63 | robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION)); 64 | 65 | // Load default joint values 66 | loadDefaultJointValues(); 67 | 68 | ROS_INFO_STREAM_NAMED(name_, "MoveItSimHWInterface Ready."); 69 | } 70 | 71 | void MoveItSimHWInterface::loadDefaultJointValues() 72 | { 73 | // Load the robot model 74 | robot_model::RobotModelPtr robot_model = robot_model_loader_->getModel(); // Get a shared pointer to the robot 75 | 76 | // Check for existance of joint model group 77 | if (!robot_model->hasJointModelGroup(joint_model_group_)) 78 | { 79 | ROS_WARN_STREAM_NAMED(name_, "Unable to find joint model group '" << joint_model_group_ 80 | << "' for the fake controller manager"); 81 | return; 82 | } 83 | 84 | moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(joint_model_group_); 85 | 86 | // Load a robot state 87 | moveit::core::RobotState robot_state(robot_model); 88 | 89 | // First set whole robot default values to ensure there are no 'nan's 90 | robot_state.setToDefaultValues(); 91 | 92 | // Attempt to set pose 93 | if (!robot_state.setToDefaultValues(jmg, joint_model_group_pose_)) 94 | { 95 | ROS_WARN_STREAM_NAMED(name_, "Unable to find pose " << joint_model_group_pose_ << " for the fake controller " 96 | "manager"); 97 | } 98 | else { 99 | ROS_INFO_STREAM_NAMED(name_, "Set joints to pose " << joint_model_group_pose_); 100 | } 101 | 102 | for (std::size_t i = 0; i < joint_names_.size(); ++i) 103 | { 104 | const moveit::core::JointModel* jm = robot_state.getJointModel(joint_names_[i]); 105 | 106 | // Error check 107 | if (!jm) 108 | { 109 | ROS_WARN_STREAM_NAMED(name_, "Unable to find joint model group: " << joint_names_[i]); 110 | continue; 111 | } 112 | if (jm->getVariableCount() != 1) 113 | { 114 | ROS_WARN_STREAM_NAMED(name_, "Fake joint controller does not currently accept more than 1 " 115 | "variable per joint"); 116 | continue; 117 | } 118 | 119 | // Set position from SRDF 120 | joint_position_[i] = robot_state.getJointPositions(jm)[0]; 121 | joint_position_command_[i] = joint_position_[i]; 122 | 123 | if (std::isnan(joint_position_[i])) 124 | { 125 | ROS_ERROR_STREAM_NAMED(name_, "NaN found"); 126 | std::cout << std::endl; 127 | std::cout << "i: " << i << " name: " << jm->getName() << std::endl; 128 | std::cout << "joint_model_group: " << jmg->getName() << std::endl; 129 | std::cout << "getVariableCount(): " << jm->getVariableCount() << std::endl; 130 | std::cout << "joint_position_[i]: " << joint_position_[i] << std::endl; 131 | std::cout << "joint_position_command_[i]: " << joint_position_command_[i] << std::endl; 132 | robot_state.printStateInfo(); 133 | exit(-1); 134 | } 135 | } 136 | } 137 | 138 | } // namespace moveit_sim_controller 139 | -------------------------------------------------------------------------------- /src/moveit_sim_hw_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: MoveIt! robot simulator using ros_control 37 | */ 38 | 39 | #include 40 | #include 41 | 42 | // Support released kinetic & melodic versions of GenericHWControlLoop 43 | // can be dropped once kinetic is not supported anymore 44 | template using void_t = void; 45 | template struct WithRun : public T { using T::T; void run(){ ros::waitForShutdown(); } }; 46 | template struct WithRun > : public T { using T::T; }; 47 | 48 | int main(int argc, char** argv) 49 | { 50 | ros::init(argc, argv, "moveit_sim_hw_main"); 51 | ros::NodeHandle nh; 52 | 53 | // NOTE: We run the ROS loop in a separate thread as external calls such 54 | // as service callbacks to load controllers can block the (main) control loop 55 | ros::AsyncSpinner spinner(2); 56 | spinner.start(); 57 | 58 | // Create the hardware interface specific to your robot 59 | boost::shared_ptr moveit_sim_hw_iface( 60 | new moveit_sim_controller::MoveItSimHWInterface(nh)); 61 | moveit_sim_hw_iface->init(); 62 | 63 | // Start the control loop 64 | WithRun control_loop(nh, moveit_sim_hw_iface); 65 | control_loop.run(); // blocks until shutdown is signaled 66 | 67 | return 0; 68 | } 69 | --------------------------------------------------------------------------------