From: Konstantinos Chatzilygeroudis Date: Thu, 29 May 2014 19:51:11 +0000 (+0300) Subject: Initial commit..MimicJointPlugin.. X-Git-Tag: 0.1.0~27 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b2d8dab1049898f2d91abfbee2e3b7ee3b8736b5;p=roboticsgroup_upatras_gazebo_plugins.git Initial commit..MimicJointPlugin.. --- b2d8dab1049898f2d91abfbee2e3b7ee3b8736b5 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..65ecc51 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_plugins) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + roscpp + gazebo_ros +) + +# Depend on system install of Gazebo +find_package(gazebo REQUIRED) +find_package(Boost REQUIRED) + +link_directories(${GAZEBO_LIBRARY_DIRS}) +include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} include) + +add_library(gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp) +target_link_libraries(gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +catkin_package( + DEPENDS + roscpp + gazebo_ros +) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..24e59b1 --- /dev/null +++ b/README.md @@ -0,0 +1,41 @@ +#gazebo_plugins + +##Collection of small gazebo plugins + +###MimicJointPlugin + +A simple plugin for Gazebo in order to add to Gazebo the mimic joint functionality that exists in URDF (ROS). Inspired by code of Goncalo Cabrita. + + - *XML Parameters* + + - joint (Required) + + A **string** specifying the name of the joint to be mimic-ed. + + - mimicJoint (Required) + + A **string** specifying the name of the mimic joint. + + - multiplier + + A **double** specifying the multiplier parameter of the mimic joint. + + - offset + + A **double** specifying the offset parameter of the mimic joint. + + +###Hoping to add more plugins.... + +Notes +------ + +If there is a need, please make an issue and I'll see what I can do to add that functionality/plugin. + +License +---- + +BSD + + +Copyright (c) 2014, **Konstantinos Chatzilygeroudis** diff --git a/include/gazebo_plugins/mimic_joint_plugin.h b/include/gazebo_plugins/mimic_joint_plugin.h new file mode 100644 index 0000000..f378cb5 --- /dev/null +++ b/include/gazebo_plugins/mimic_joint_plugin.h @@ -0,0 +1,74 @@ +/** +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +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. + +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. + +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. +**/ + +#ifndef GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN +#define GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN + +// ROS includes +#include + +// Boost includes +#include + +// Gazebo includes +#include +#include +#include +#include + + +using std::string; + +namespace gazebo +{ + class MimicJointPlugin : public ModelPlugin + { + public: + MimicJointPlugin(); + ~MimicJointPlugin(); + + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + void UpdateChild(); + + private: + // Parameters + string joint_name_, mimic_joint_name_; + double multiplier_, offset_; + + bool kill_sim; + + // Pointers to the joints + physics::JointPtr joint_, mimic_joint_; + + // Pointer to the model + physics::ModelPtr model_; + + // Pointer to the world + physics::WorldPtr world_; + + // Pointer to the update event connection + event::ConnectionPtr updateConnection; + + }; +} + +#endif \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f0610c7 --- /dev/null +++ b/package.xml @@ -0,0 +1,49 @@ + + + + gazebo_plugins + 0.0.1 + Collection of small gazebo plugins + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/gazebo_plugins + https://github.com/costashatz/gazebo_plugins/issues + + Konstantinos Chatzilygeroudis + + catkin + + + gazebo_ros + roscpp + + + gazebo_ros + roscpp + + + + + + \ No newline at end of file diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp new file mode 100644 index 0000000..840c26f --- /dev/null +++ b/src/mimic_joint_plugin.cpp @@ -0,0 +1,91 @@ +/** +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +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. + +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. + +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. +**/ + +#include + +using namespace gazebo; + +MimicJointPlugin::MimicJointPlugin() +{ + kill_sim = false; + + joint_.reset(); + mimic_joint_.reset(); +} + +MimicJointPlugin::~MimicJointPlugin() +{ + event::Events::DisconnectWorldUpdateBegin(this->updateConnection); + + kill_sim = true; +} + +void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + this->model_ = _parent; + this->world_ = this->model_->GetWorld(); + + // Check for joint element + if (!_sdf->HasElement("joint")) + { + ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded."); + return; + } + + joint_name_ = _sdf->GetElement("joint")->Get(); + + // Check for mimicJoint element + if (_sdf->HasElement("mimicJoint")) + { + ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded."); + return; + } + + mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get(); + + // Check for multiplier element + multiplier_ = 1.0; + if (_sdf->HasElement("multiplier")) + multiplier_ = _sdf->GetElement("multiplier")->Get(); + + // Check for offset element + offset_ = 0.0; + if (_sdf->HasElement("offset")) + offset_ = _sdf->GetElement("offset")->Get(); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection = event::Events::ConnectWorldUpdateBegin( + boost::bind(&MimicJointPlugin::UpdateChild, this)); + + // Get pointers to joints + joint_ = model_->GetJoint(joint_name_); + mimic_joint_ = model_->GetJoint(mimic_joint_name_); +} + +void MimicJointPlugin::UpdateChild() +{ + // Set mimic joint's angle based on joint's angle + mimic_joint_->SetAngle(0, math::Angle(joint_->GetAngle(0).Radian()*multiplier_+offset_)); +} + +GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin); \ No newline at end of file