Added PID control capability to mimic joint plugin

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dd3a25c..4e3f7c2 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,8 @@ project(roboticsgroup_gazebo_plugins)
 # Load catkin and all dependencies required for this package
 find_package(catkin REQUIRED COMPONENTS 
   roscpp 
-  gazebo_ros 
+  gazebo_ros
+  control_toolbox
 )
 
 # Depend on system install of Gazebo
@@ -23,5 +24,6 @@ target_link_libraries(roboticsgroup_gazebo_disable_link_plugin ${catkin_LIBRARIE
 catkin_package(
   DEPENDS 
     roscpp 
-    gazebo_ros 
+    gazebo_ros
+    control_toolbox
 )
\ No newline at end of file

diff --git a/README.md b/README.md
index d8e820b..a2afc12 100644 (file)
--- a/README.md
+++ b/README.md
@@ -32,6 +32,12 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
 
       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".
 
+    - robotNamespace
+      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.
+
 ###DisableLinkPlugin
 
 A simple (Model) plugin for Gazebo that allows you to disable a link in Gazebo's physics engine.

diff --git a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
index 92e63db..bd21dc8 100644 (file)
--- a/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
+++ b/include/roboticsgroup_gazebo_plugins/mimic_joint_plugin.h
@@ -26,6 +26,9 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 // ROS includes
 #include <ros/ros.h>
 
+// ros_control
+#include <control_toolbox/pid.h>
+
 // Boost includes
 #include <boost/bind.hpp>
 
@@ -35,9 +38,6 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 #include <gazebo/physics/physics.hh>
 #include <gazebo/common/common.hh>
 
-
-using std::string;
-
 namespace gazebo
 {
   class MimicJointPlugin : public ModelPlugin
@@ -51,11 +51,15 @@ namespace gazebo
 
     private:
       // Parameters
-      string joint_name_, mimic_joint_name_;
+      std::string joint_name_, mimic_joint_name_, robot_namespace_;
       double multiplier_, offset_, sensitiveness_, max_effort_;
+      bool has_pid_;
 
       bool kill_sim;
 
+      // PID controller if needed
+      control_toolbox::Pid pid_;
+
       // Pointers to the joints
       physics::JointPtr joint_, mimic_joint_;
 

diff --git a/package.xml b/package.xml
index 49b236f..21a2ceb 100644 (file)
--- a/package.xml
+++ b/package.xml
@@ -37,10 +37,12 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE
 
   <build_depend>gazebo_ros</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>control_toolbox</build_depend>
 
 
   <run_depend>gazebo_ros</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>control_toolbox</run_depend>
 
 
   <export>

diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp
index 2846cb7..ef8860f 100644 (file)
--- a/src/mimic_joint_plugin.cpp
+++ b/src/mimic_joint_plugin.cpp
@@ -41,8 +41,30 @@ MimicJointPlugin::~MimicJointPlugin()
 
 void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
 {
-  this->model_ = _parent;
-  this->world_ = this->model_->GetWorld();
+  ros::NodeHandle model_nh;
+  model_ = _parent;
+  world_ = model_->GetWorld();
+
+  // Error message if the model couldn't be found
+  if (!model_)
+  {
+    ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded.");
+    return;
+  }
+
+  // Check that ROS has been initialized
+  if(!ros::isInitialized())
+  {
+    ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin.");
+    return;
+  }
+
+  // Check for robot namespace
+  robot_namespace_ = "/";
+  if(_sdf->HasElement("robotNamespace"))
+  {
+    robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
+  }
 
   // Check for joint element
   if(!_sdf->HasElement("joint"))
@@ -62,6 +84,22 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   
   mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();
 
+  has_pid_ = false;
+  // Check if PID controller wanted
+  if(_sdf->HasElement("hasPID"))
+  {
+    has_pid_ = true;
+
+    const ros::NodeHandle nh(model_nh, std::string(robot_namespace_+"/gazebo_ros_control/pid_gains/")+mimic_joint_name_);
+    double p, i,d ;
+    // TO-DO: include i_clamp e.t.c.
+    nh.param("p", p, 0.0);
+    nh.param("i", i, 0.0);
+    nh.param("d", d, 0.0);
+
+    pid_ = control_toolbox::Pid(p,i,d);
+  }
+
   // Check for multiplier element
   multiplier_ = 1.0;
   if(_sdf->HasElement("multiplier"))
@@ -80,7 +118,9 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   // 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_);
@@ -97,7 +137,8 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
   }
   
   // Set max effort
-  mimic_joint_->SetMaxForce(0,max_effort_);
+  if(!has_pid_)
+    mimic_joint_->SetMaxForce(0,max_effort_);
 
   // Listen to the update event. This event is broadcast every
   // simulation iteration.
@@ -107,9 +148,24 @@ void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
 
 void MimicJointPlugin::UpdateChild()
 {
+  static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize());
+
   // Set mimic joint's angle based on joint's angle
-  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_));
+  double angle = joint_->GetAngle(0).Radian()*multiplier_+offset_;
+  
+  if(abs(angle-mimic_joint_->GetAngle(0).Radian())>=sensitiveness_)
+  {
+    if(has_pid_)
+    {
+      double a = mimic_joint_->GetAngle(0).Radian();
+      if(a!=a)
+        a = angle;
+      double error = angle-a;
+      double effort = gazebo::math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);
+    }
+    else
+      mimic_joint_->SetAngle(0, math::Angle(angle));
+  }
 }
 
 GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
\ No newline at end of file