├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── roboticsgroup_gazebo_plugins │ ├── disable_link_plugin.h │ └── mimic_joint_plugin.h ├── package.xml └── src ├── disable_link_plugin.cpp └── mimic_joint_plugin.cpp /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package roboticsgroup_gazebo_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.0 (2020-08-14) 6 | ------------------ 7 | * Added DisableLink Model Plugin 8 | * Added sensitiveness parameter to MimicJointPlugin 9 | * Added maxEffort parameter to MimicJoint plugin 10 | * Added PID control capability to mimic joint plugin 11 | * Move catkin_package macro so it is called before targets are defined. 12 | Fixes plugins not getting found when doing isolated builds 13 | * Add missing setForce() call (otherwise PID option doesn't do anything) 14 | * Support of Gazebo 7 was added 15 | * Support all PID gain parameters, dynamic_reconfigure 16 | This change does the following: 17 | * the PID controllers will read all PID gain parameters (p, i, d, i_clamp, antiwindup, publish_state, ...) 18 | * a warning will be printed if none of those parameters could be found 19 | * it's possible to adjust the parameters using dynamic_reconfigure 20 | * Adjust to Gazebo 8 API 21 | Note about the DisconnectWorldUpdateBegin: This function was deprecated 22 | in favor of resetting the ConnectionPtr, see here: 23 | https://bitbucket.org/osrf/gazebo/pull-requests/2329/deprecate-event-disconnect-connectionptr/diff 24 | * Add fix for gazebo_ros_pkgs#612 25 | This issue also affects the mimic joint plugin: 26 | https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612 27 | The commit here fixes that issue for Gazebo 9. We should change the 28 | GAZEBO_MAJOR_VERSION check to >= 7 if the following PR gets backported 29 | to Gazebo 7 and 8: 30 | https://bitbucket.org/osrf/gazebo/pull-requests/2814/fix-issue-2111-by-providing-options-to/diff 31 | * Add warning when triggering gazebo_ros_pkgs#612 32 | * Update parameters 33 | * Default max effort to limit from sdf model 34 | * Default namespace to empty string 35 | * Fix sensitiveness calculation 36 | * Add option to change the namespace of the pid 37 | * Set CMP0054 for building with Gazebo9 38 | * Use SetParam for effort limit 39 | * Add license notice 40 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.6 FATAL_ERROR) 2 | project(roboticsgroup_gazebo_plugins) 3 | 4 | # Set CMP0054 5 | cmake_policy(SET CMP0054 NEW) 6 | 7 | # Load catkin and all dependencies required for this package 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | gazebo_ros 11 | control_toolbox 12 | ) 13 | 14 | # Depend on system install of Gazebo 15 | find_package(gazebo REQUIRED) 16 | # Gazebo cxx flags should have all the required C++ flags 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 18 | 19 | find_package(Boost REQUIRED) 20 | 21 | catkin_package( 22 | DEPENDS 23 | roscpp 24 | gazebo_ros 25 | control_toolbox 26 | ) 27 | 28 | link_directories(${GAZEBO_LIBRARY_DIRS}) 29 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include) 30 | 31 | add_library(roboticsgroup_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp) 32 | target_link_libraries(roboticsgroup_gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 33 | 34 | add_library(roboticsgroup_gazebo_disable_link_plugin src/disable_link_plugin.cpp) 35 | target_link_libraries(roboticsgroup_gazebo_disable_link_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 36 | 37 | install(TARGETS roboticsgroup_gazebo_mimic_joint_plugin roboticsgroup_gazebo_disable_link_plugin 38 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | install(DIRECTORY include/${PROJECT_NAME} 43 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2014 Konstantinos Chatzilygeroudis 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | roboticsgroup_gazebo_plugins 2 | ================ 3 | 4 | This package has been **deprecated**. For the latest updates, visit [roboticsgroup_upatras_gazebo_plugins](https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins). 5 | 6 | Collection of small gazebo plugins 7 | ---------------------------------- 8 | 9 | MimicJointPlugin 10 | ---------------- 11 | 12 | A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint functionality that exists in URDF (ROS). Inspired by code of Goncalo Cabrita. 13 | 14 | - *XML Parameters* 15 | 16 | - joint (Required) 17 | 18 | A **string** specifying the name of the joint to be mimic-ed. 19 | 20 | - mimicJoint (Required) 21 | 22 | A **string** specifying the name of the mimic joint. 23 | 24 | - multiplier 25 | 26 | A **double** specifying the multiplier parameter of the mimic joint. Defaults to 1.0. 27 | 28 | - offset 29 | 30 | A **double** specifying the offset parameter of the mimic joint. Defaults to 0.0. 31 | 32 | - maxEffort 33 | 34 | A **double** specifying the max effort the mimic joint can generate. Defaults to the effort limit in the sdf model. 35 | 36 | - sensitiveness 37 | 38 | A **double** specifying the sensitiveness of the mimic joint. Defaults to 0.0. It basically is the threshold of the difference between the 2 angles (joint's and mimic's) before applying the "mimicness". 39 | 40 | - robotNamespace 41 | 42 | A **string** specifying the namespace the robot is under. Defaults to "". 43 | 44 | - hasPID 45 | 46 | Determines whether the joint has PID in order to be controlled via PID position/effort controller. *\* means that the mimic joint is controlled via PID. Omit it so that the mimic joint is controlled via setAngle. Optionally, specify a value to set the pid namespace. 47 | 48 | DisableLinkPlugin 49 | ----------------- 50 | 51 | A simple (Model) plugin for Gazebo that allows you to disable a link in Gazebo's physics engine. 52 | 53 | - *XML Parameters* 54 | 55 | - link (Required) 56 | 57 | A **string** specifying the name of the link to be disabled. It should be a valid sdf (not urdf) link. 58 | 59 | ### Hoping to add more plugins.... 60 | 61 | Usage 62 | ------ 63 | 64 | Standard Gazebo plugin import inside xacro/urdf. Use **libroboticsgroup_gazebo_** prefix. E.g. if you want to import MimicJointPlugin: 65 | 66 | ``` 67 | libroboticsgroup_gazebo_mimic_joint_plugin.so 68 | ``` 69 | 70 | Notes 71 | ------ 72 | 73 | If there is a need, please make an issue and I'll see what I can do to add that functionality/plugin. 74 | 75 | License 76 | ---- 77 | 78 | BSD 79 | 80 | 81 | Copyright (c) 2014, **Konstantinos Chatzilygeroudis** 82 | -------------------------------------------------------------------------------- /include/roboticsgroup_gazebo_plugins/disable_link_plugin.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #ifndef ROBOTICSGROUP_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN 24 | #define ROBOTICSGROUP_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN 25 | 26 | // ROS includes 27 | #include 28 | 29 | // Boost includes 30 | #include 31 | 32 | // Gazebo includes 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace gazebo { 39 | class DisableLinkPlugin : public ModelPlugin { 40 | public: 41 | DisableLinkPlugin(); 42 | ~DisableLinkPlugin(); 43 | 44 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 45 | void UpdateChild(); 46 | 47 | private: 48 | // Parameters 49 | std::string link_name_; 50 | 51 | bool kill_sim; 52 | 53 | // Pointers to the joints 54 | physics::LinkPtr link_; 55 | 56 | // Pointer to the model 57 | physics::ModelPtr model_; 58 | 59 | // Pointer to the world 60 | physics::WorldPtr world_; 61 | }; 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #ifndef ROBOTICSGROUP_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN 24 | #define ROBOTICSGROUP_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN 25 | 26 | // ROS includes 27 | #include 28 | 29 | // ros_control 30 | #include 31 | 32 | // Boost includes 33 | #include 34 | 35 | // Gazebo includes 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace gazebo { 42 | class MimicJointPlugin : public ModelPlugin { 43 | public: 44 | MimicJointPlugin(); 45 | ~MimicJointPlugin(); 46 | 47 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 48 | void UpdateChild(); 49 | 50 | private: 51 | // Parameters 52 | std::string joint_name_, mimic_joint_name_, robot_namespace_; 53 | double multiplier_, offset_, sensitiveness_, max_effort_; 54 | bool has_pid_; 55 | 56 | // PID controller if needed 57 | control_toolbox::Pid pid_; 58 | 59 | // Pointers to the joints 60 | physics::JointPtr joint_, mimic_joint_; 61 | 62 | // Pointer to the model 63 | physics::ModelPtr model_; 64 | 65 | // Pointer to the world 66 | physics::WorldPtr world_; 67 | 68 | // Pointer to the update event connection 69 | event::ConnectionPtr updateConnection; 70 | }; 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | > 22 | roboticsgroup_gazebo_plugins 23 | 0.1.0 24 | Collection of gazebo plugins 25 | 26 | Konstantinos Chatzilygeroudis 27 | Konstantinos Chatzilygeroudis 28 | Nick Lamprianidis 29 | 30 | BSD 31 | 32 | https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins 33 | 34 | catkin 35 | gazebo_ros 36 | roscpp 37 | control_toolbox 38 | gazebo_ros 39 | roscpp 40 | control_toolbox 41 | gazebo_ros 42 | roscpp 43 | control_toolbox 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /src/disable_link_plugin.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #include 24 | 25 | namespace gazebo { 26 | 27 | DisableLinkPlugin::DisableLinkPlugin() 28 | { 29 | kill_sim = false; 30 | 31 | link_.reset(); 32 | } 33 | 34 | DisableLinkPlugin::~DisableLinkPlugin() 35 | { 36 | kill_sim = true; 37 | } 38 | 39 | void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 40 | { 41 | model_ = _parent; 42 | world_ = model_->GetWorld(); 43 | 44 | // Check for link element 45 | if (!_sdf->HasElement("link")) { 46 | ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded."); 47 | return; 48 | } 49 | 50 | link_name_ = _sdf->GetElement("link")->Get(); 51 | 52 | // Get pointers to joints 53 | link_ = model_->GetLink(link_name_); 54 | if (link_) { 55 | link_->SetEnabled(false); 56 | // Output some confirmation 57 | ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_); 58 | } 59 | else 60 | ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded."); 61 | } 62 | 63 | GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin); 64 | } 65 | -------------------------------------------------------------------------------- /src/mimic_joint_plugin.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #include 24 | 25 | #if GAZEBO_MAJOR_VERSION >= 8 26 | namespace math = ignition::math; 27 | #else 28 | namespace math = gazebo::math; 29 | #endif 30 | 31 | namespace gazebo { 32 | 33 | MimicJointPlugin::MimicJointPlugin() 34 | { 35 | joint_.reset(); 36 | mimic_joint_.reset(); 37 | } 38 | 39 | MimicJointPlugin::~MimicJointPlugin() 40 | { 41 | this->updateConnection.reset(); 42 | } 43 | 44 | void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 45 | { 46 | model_ = _parent; 47 | world_ = model_->GetWorld(); 48 | 49 | // Error message if the model couldn't be found 50 | if (!model_) { 51 | ROS_ERROR("Parent model is NULL! MimicJointPlugin could not be loaded."); 52 | return; 53 | } 54 | 55 | // Check that ROS has been initialized 56 | if (!ros::isInitialized()) { 57 | ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin."); 58 | return; 59 | } 60 | 61 | // Check for robot namespace 62 | robot_namespace_ = ""; 63 | if (_sdf->HasElement("robotNamespace")) { 64 | robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); 65 | } 66 | ros::NodeHandle model_nh(robot_namespace_); 67 | 68 | // Check for joint element 69 | if (!_sdf->HasElement("joint")) { 70 | ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded."); 71 | return; 72 | } 73 | 74 | joint_name_ = _sdf->GetElement("joint")->Get(); 75 | 76 | // Check for mimicJoint element 77 | if (!_sdf->HasElement("mimicJoint")) { 78 | ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded."); 79 | return; 80 | } 81 | 82 | mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get(); 83 | 84 | // Check if PID controller wanted 85 | has_pid_ = _sdf->HasElement("hasPID"); 86 | if (has_pid_) { 87 | std::string name = _sdf->GetElement("hasPID")->Get(); 88 | if (name.empty()) { 89 | name = "gazebo_ros_control/pid_gains/" + mimic_joint_name_; 90 | } 91 | const ros::NodeHandle nh(model_nh, name); 92 | pid_.init(nh); 93 | } 94 | 95 | // Check for multiplier element 96 | multiplier_ = 1.0; 97 | if (_sdf->HasElement("multiplier")) 98 | multiplier_ = _sdf->GetElement("multiplier")->Get(); 99 | 100 | // Check for offset element 101 | offset_ = 0.0; 102 | if (_sdf->HasElement("offset")) 103 | offset_ = _sdf->GetElement("offset")->Get(); 104 | 105 | // Check for sensitiveness element 106 | sensitiveness_ = 0.0; 107 | if (_sdf->HasElement("sensitiveness")) 108 | sensitiveness_ = _sdf->GetElement("sensitiveness")->Get(); 109 | 110 | // Get pointers to joints 111 | joint_ = model_->GetJoint(joint_name_); 112 | if (!joint_) { 113 | ROS_ERROR_STREAM("No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded."); 114 | return; 115 | } 116 | mimic_joint_ = model_->GetJoint(mimic_joint_name_); 117 | if (!mimic_joint_) { 118 | ROS_ERROR_STREAM("No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded."); 119 | return; 120 | } 121 | 122 | // Check for max effort 123 | #if GAZEBO_MAJOR_VERSION > 2 124 | max_effort_ = mimic_joint_->GetEffortLimit(0); 125 | #else 126 | max_effort_ = mimic_joint_->GetMaxForce(0); 127 | #endif 128 | if (_sdf->HasElement("maxEffort")) { 129 | max_effort_ = _sdf->GetElement("maxEffort")->Get(); 130 | } 131 | 132 | // Set max effort 133 | if (!has_pid_) { 134 | #if GAZEBO_MAJOR_VERSION > 2 135 | mimic_joint_->SetParam("fmax", 0, max_effort_); 136 | #else 137 | mimic_joint_->SetMaxForce(0, max_effort_); 138 | #endif 139 | } 140 | 141 | // Listen to the update event. This event is broadcast every 142 | // simulation iteration. 143 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 144 | boost::bind(&MimicJointPlugin::UpdateChild, this)); 145 | 146 | // Output some confirmation 147 | ROS_INFO_STREAM("MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\"" 148 | << ", Multiplier: " << multiplier_ << ", Offset: " << offset_ 149 | << ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_); 150 | } 151 | 152 | void MimicJointPlugin::UpdateChild() 153 | { 154 | #if GAZEBO_MAJOR_VERSION >= 8 155 | static ros::Duration period(world_->Physics()->GetMaxStepSize()); 156 | #else 157 | static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize()); 158 | #endif 159 | 160 | // Set mimic joint's angle based on joint's angle 161 | #if GAZEBO_MAJOR_VERSION >= 8 162 | double angle = joint_->Position(0) * multiplier_ + offset_; 163 | double a = mimic_joint_->Position(0); 164 | #else 165 | double angle = joint_->GetAngle(0).Radian() * multiplier_ + offset_; 166 | double a = mimic_joint_->GetAngle(0).Radian(); 167 | #endif 168 | 169 | if (fabs(angle - a) >= sensitiveness_) { 170 | if (has_pid_) { 171 | if (a != a) 172 | a = angle; 173 | double error = angle - a; 174 | double effort = math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_); 175 | mimic_joint_->SetForce(0, effort); 176 | } 177 | else { 178 | #if GAZEBO_MAJOR_VERSION >= 9 179 | mimic_joint_->SetPosition(0, angle, true); 180 | #elif GAZEBO_MAJOR_VERSION > 2 181 | ROS_WARN_ONCE("The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity."); 182 | ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); 183 | ROS_WARN_ONCE("Please set gazebo_pid parameters or upgrade to Gazebo 9."); 184 | ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); 185 | mimic_joint_->SetPosition(0, angle); 186 | #else 187 | mimic_joint_->SetAngle(0, math::Angle(angle)); 188 | #endif 189 | } 190 | } 191 | } 192 | 193 | GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin); 194 | } 195 | --------------------------------------------------------------------------------