Added sensitiveness parameter to MimicJointPlugin

diff --git a/README.md b/README.md
index cfe0ad0..18c48a3 100644 (file)
--- a/README.md
+++ b/README.md
@@ -23,6 +23,9 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
     - offset
 
       A **double** specifying the offset parameter of the mimic joint. Defaults to 0.0.
+    - sensitiveness
+
+      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".
 
 ###DisableLinkPlugin
 

diff --git a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
index 0f9c04c..08f8b55 100644 (file)
--- a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
+++ b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
@@ -52,7 +52,7 @@ namespace gazebo
     private:
       // Parameters
       string joint_name_, mimic_joint_name_;
-      double multiplier_, offset_;
+      double multiplier_, offset_, sensitiveness_;
 
       bool kill_sim;
 

diff --git a/package.xml b/package.xml
index 4b84214..49b236f 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -27,8 +27,8 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 
   <license>BSD</license>
 
-  <url type="repository">https://github.com/costashatz/gazebo_plugins</url>
-  <url type="bugtracker">https://github.com/costashatz/gazebo_plugins/issues</url>
+  <url type="repository">https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins</url>
+  <url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins/issues</url>
 
   <author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
 

diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index 2049930..b991463 100644 (file)
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -72,6 +72,11 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   if (_sdf->HasElement("offset"))
     offset_ = _sdf->GetElement("offset")->Get<double>();
 
+  // Check for sensitiveness element
+  sensitiveness_ = 0.0;
+  if (_sdf->HasElement("sensitiveness"))
+    sensitiveness_ = _sdf->GetElement("sensitiveness")->Get<double>();
+
   // Get pointers to joints
   joint_ = model_->GetJoint(joint_name_);
   if(!joint_)
@@ -95,7 +100,8 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
 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_));
+  if(abs((joint_->GetAngle(0).Radian()*multiplier_+offset_)-mimic_joint_->GetAngle(0).Radian())>=sensitiveness_)
+    mimic_joint_->SetAngle(0, math::Angle(joint_->GetAngle(0).Radian()*multiplier_+offset_));
 }
 
 GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
\ No newline at end of file