Load PID joint controller parameters from sdf

[osrf-migration]

Original report (archived issue) by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

It would be cool to be able to specify PID joint controller parameters from an sdformat model or world file. This would require creating sdf definitions for PID parameters and also the joint controller.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• set version to "all"

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

I am working on a plugin to initialize PID joint controller parameters as an intermediate measure. Do you have recommendations for the sdf structure? I am consider the below format, where an arbitrary number of <controller> elements can be added.

<plugin name="initialize_joint_control" filename="libJointControlPlugin.so">
  <controller name="upper_joint" type="position">
    <target>1.0</target>
    <gains>1.0 0.0 0.2</gains>
  </controller>
</plugin>

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I think that looks fine. It would also be interesting to explore setting the gains based on joint parameters like the effort limit. For example, for a velocity controller, you can define a velocity_tolerance, then set the gain as effort_limit / velocity_tolerance. It will then be using the full effort when the error reaches the tolerance.

If the tolerance concept scales for joints connecting links of different sizes, you could also try setting the gains in a batch manner, either taking a list of joint names or a wildcard / regex. This would make it easier to quickly set gains for robots with lots of joints.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Actually, I would maybe add pid somewhere in the xml that you've proposed, just so it's clear what the 3 numbers are in the gain tag. Maybe call it <pid_gains> or <pid_controller>?

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

I added the joint names list and regex functionality and created pull request #2751. I will look into supporting a <tolerance> element in place of the <pid_gains> and testing it on the robonaut model. I am not completely sure how to set the ID gains automatically. Perhaps default to an iGain of 0 and dGain proportional to the square root of the pGain?

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

I could add something like this

diff -r 4fe7844f6107bb2895caa712a8440443dc2ee621 plugins/JointControlPlugin.cc
--- a/plugins/JointControlPlugin.cc	Fri Jul 28 10:44:51 2017 -0700
+++ b/plugins/JointControlPlugin.cc	Fri Jul 28 11:54:43 2017 -0700
@@ -17,6 +17,7 @@
 
 #include <algorithm>
 #include <regex>
+#include <cmath>
 #include <boost/algorithm/string.hpp>
 
 #include <gazebo/common/Assert.hh>
@@ -61,6 +62,8 @@
         ignition::msgs::JointCmd msg;
         std::string controllerType = child->Get<std::string>("type");
 
+        double tolerance = 0;
+
         if (controllerType == "force")
         {
           if (child->HasElement("target"))
@@ -78,7 +81,11 @@
           {
             msg.mutable_position()->set_target(child->Get<double>("target"));
           }
-          if (child->HasElement("pid_gains"))
+          if (child->HasElement("tolerance"))
+          {
+            tolerance = child->Get<double>("tolerance");
+          }
+          else if (child->HasElement("pid_gains"))
           {
             auto gains = child->Get<ignition::math::Vector3d>("pid_gains");
             msg.mutable_position()->set_p_gain(gains.X());
@@ -131,6 +138,14 @@
             }
             for (auto match : matches)
             {
+              if (tolerance > 0.0)
+              {
+                physics::JointPtr joint = _model->GetJoint(match);
+                double pGain = joint->GetEffortLimit(0)/tolerance;
+                msg.mutable_position()->set_p_gain(pGain);
+                msg.mutable_position()->set_i_gain(0.01*pGain);
+                msg.mutable_position()->set_d_gain(2*std::sqrt(pGain));
+              }
               msg.set_name(match);
               jointPub.Publish(msg);
             }

[osrf-migration]

Original comment by Peter Horak (Bitbucket: pchorak).

---

I can set a tolerance that works well for the robonaut arms, but then if I include the neck it causes vibrations since the effort limits are so large.

<?xml version="1.0"?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://r2_description</uri>
      <plugin name="init_joint_control" filename="libJointControlPlugin.so">
        <controller type="position">
          <joint>r2/left_arm/joint.</joint>
          <joint>r2/right_arm/joint.</joint>
          <joint>r2/neck/joint.</joint> <!-- Causes instability -->
          <target>0</target>
          <tolerance>0.1</tolerance>
        </controller>
      </plugin>
    </include>
  </world>
</sdf>