From: nlamprian <nlamprian@gmail.com>
Date: Tue, 19 Feb 2019 13:20:21 +0000 (+0100)
Subject: Update parameters
X-Git-Tag: 0.1.0~5^2
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=433ca36dce90ecfc1cbf700758d15632d1c547f9;p=roboticsgroup_upatras_gazebo_plugins.git

Update parameters
* Default max effort to limit from sdf model
* Default namespace to empty string
* Fix sensitiveness calculation
---

diff --git a/README.md b/README.md
index 4452aa3..7d4ccea 100644
--- a/README.md
+++ b/README.md
@@ -29,7 +29,7 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
 
     - maxEffort
 
-      A **double** specifying the max effort the mimic joint can generate. Defaults to 1.0.
+      A **double** specifying the max effort the mimic joint can generate. Defaults to the effort limit in the sdf model.
 
     - sensitiveness
 
@@ -37,11 +37,11 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
 
     - robotNamespace
 
-      A **string** specifying the namespace the robot is under. Defaults to '/'.
+      A **string** specifying the namespace the robot is under. Defaults to "".
 
     - hasPID
 
-      Determines whether the joint has PID in order to be controlled via PID position/effort controller. Takes no value: *\<hasPID/\>* means that the mimic joint is controlled via PID. Ommit it so that the mimic joint is controlled via setAngle.
+      Determines whether the joint has PID in order to be controlled via PID position/effort controller. Takes no value: *\<hasPID/\>* means that the mimic joint is controlled via PID. Omit it so that the mimic joint is controlled via setAngle.
 
 DisableLinkPlugin
 -----------------
diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index a6c2177..49f72d7 100644
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -60,7 +60,7 @@ namespace gazebo {
         }
 
         // Check for robot namespace
-        robot_namespace_ = "/";
+        robot_namespace_ = "";
         if (_sdf->HasElement("robotNamespace")) {
             robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
         }
@@ -105,12 +105,6 @@ namespace gazebo {
         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_) {
@@ -123,6 +117,16 @@ namespace gazebo {
             return;
         }
 
+        // Check for max effort
+#if GAZEBO_MAJOR_VERSION > 2
+        max_effort_ = mimic_joint_->GetEffortLimit(0);
+#else
+        max_effort_ = mimic_joint_->GetMaxForce(0);
+#endif
+        if (_sdf->HasElement("maxEffort")) {
+            max_effort_ = _sdf->GetElement("maxEffort")->Get<double>();
+        }
+
         // Set max effort
         if (!has_pid_) {
 #if GAZEBO_MAJOR_VERSION > 2
@@ -160,7 +164,7 @@ namespace gazebo {
         double a = mimic_joint_->GetAngle(0).Radian();
 #endif
 
-        if (abs(angle - a) >= sensitiveness_) {
+        if (fabs(angle - a) >= sensitiveness_) {
             if (has_pid_) {
                 if (a != a)
                     a = angle;