Added maxEffort parameter to MimicJoint plugin

diff --git a/README.md b/README.md
index 18c48a3..d8e820b 100644 (file)
--- a/README.md
+++ b/README.md
@@ -23,6 +23,11 @@ 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.
+
+    - maxEffort
+
+      A **double** specifying the max effort the mimic joint can generate. Defaults to 1.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".

diff --git a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
index 08f8b55..92e63db 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_, sensitiveness_;
+      double multiplier_, offset_, sensitiveness_, max_effort_;
 
       bool kill_sim;
 

diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index b991463..2846cb7 100644 (file)
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -77,6 +77,11 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   if (_sdf->HasElement("sensitiveness"))
     sensitiveness_ = _sdf->GetElement("sensitiveness")->Get<double>();
 
+  // Check for max effort
+  max_effort_ = 1.0;
+  if (_sdf->HasElement("maxEffort"))
+    max_effort_ = _sdf->GetElement("maxEffort")->Get<double>();
+
   // Get pointers to joints
   joint_ = model_->GetJoint(joint_name_);
   if(!joint_)
@@ -90,6 +95,9 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
     ROS_ERROR("No (mimic) joint named %s. MimicJointPlugin could not be loaded.", mimic_joint_name_.c_str());
     return;
   }
+  
+  // Set max effort
+  mimic_joint_->SetMaxForce(0,max_effort_);
 
   // Listen to the update event. This event is broadcast every
   // simulation iteration.