Initial commit..MimicJointPlugin..

diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644 (file)
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 (file)
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 (file)
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 <ros/ros.h>
+
+// Boost includes
+#include <boost/bind.hpp>
+
+// Gazebo includes
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+
+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 (file)
index 0000000..f0610c7
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<!-- 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. -->
+<package>
+  <name>gazebo_plugins</name>
+  <version>0.0.1</version>
+  <description>Collection of small gazebo plugins</description>
+
+  <maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
+
+  <license>BSD</license>
+
+  <url type="repository">https://github.com/costashatz/gazebo_plugins</url>
+  <url type="bugtracker">https://github.com/costashatz/gazebo_plugins/issues</url>
+
+  <author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+
+
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+
+
+  <export>
+    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
+  </export>
+</package>
\ No newline at end of file

diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
new file mode 100644 (file)
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 <gazebo_plugins/mimic_joint_plugin.h>
+
+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<std::string>();
+
+  // 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<std::string>();
+
+  // Check for multiplier element
+  multiplier_ = 1.0;
+  if (_sdf->HasElement("multiplier"))
+    multiplier_ = _sdf->GetElement("multiplier")->Get<double>();
+
+  // Check for offset element
+  offset_ = 0.0;
+  if (_sdf->HasElement("offset"))
+    offset_ = _sdf->GetElement("offset")->Get<double>();
+
+  // 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