Merge branch 'master' of https://github.com/sheim/sot-core into devel

include/sot/core/control-pd.hh

@@ -5,6 +5,17 @@
  *
  * CNRS/AIST
  *
+ * This file is part of sot-core.
+ * sot-core is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-core is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
  */
 
 #ifndef __SOT_Control_PD_HH__
@@ -28,12 +39,12 @@ namespace dg = dynamicgraph;
 /* --- API ------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#if defined (WIN32)
+#if defined (WIN32) 
 #  if defined (control_pd_EXPORTS)
 #    define ControlPD_EXPORT __declspec(dllexport)
-#  else
+#  else  
 #    define ControlPD_EXPORT  __declspec(dllimport)
-#  endif
+#  endif 
 #else
 #  define ControlPD_EXPORT
 #endif
@@ -64,13 +75,13 @@ namespace dynamicgraph {
 
     public: /* --- ENTITY INHERITANCE --- */
       static const std::string CLASS_NAME;
-      virtual void display( std::ostream& os ) const;
-      virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+      virtual void display( std::ostream& os ) const; 
+      virtual const std::string& getClassName( void ) const {return CLASS_NAME;}
 
 
-    protected:
-
-      /* Parameters of the torque-control function:
+    protected: 
+  
+      /* Parameters of the torque-control function: 
        * tau = kp * (qd-q) + kd* (dqd-dq) */
       double TimeStep;
       double _dimension;
@@ -84,11 +95,17 @@ namespace dynamicgraph {
       SignalPtr<dg::Vector,int> velocitySIN;
       SignalPtr<dg::Vector,int> desiredvelocitySIN;
       SignalTimeDependent<dg::Vector,int> controlSOUT;
+      SignalTimeDependent<dg::Vector,int> positionErrorSOUT;
+      SignalTimeDependent<dg::Vector,int> velocityErrorSOUT;
 
     protected:
 
       double& setsize(int dimension);
       dg::Vector& computeControl( dg::Vector& tau,int t );
+      dg::Vector position_error;
+      dg::Vector velocity_error;
+      dg::Vector& getPositionError( dg::Vector& position_error,int t );
+      dg::Vector& getVelocityError( dg::Vector& velocity_error,int t );
 
     };
 

src/control/control-pd.cpp

@@ -19,7 +19,7 @@
 using namespace dynamicgraph::sot;
 using namespace dynamicgraph;
 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD,"ControlPD");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD, "ControlPD");
 
 const double ControlPD::
 TIME_STEP_DEFAULT = .001;
@@ -31,25 +31,30 @@ TIME_STEP_DEFAULT = .001;
 
 #define __SOT_ControlPD_INIT                    \
 
-ControlPD::
-ControlPD( const std::string & name )
-  :Entity(name)
-  ,TimeStep(0)
-  ,KpSIN(NULL,"ControlPD("+name+")::input(vector)::Kp")
-  ,KdSIN(NULL,"ControlPD("+name+")::input(vector)::Kd")
-  ,positionSIN(NULL,"ControlPD("+name+")::input(vector)::position")
-  ,desiredpositionSIN(NULL,"ControlPD("+name+")::input(vector)::desiredposition")
-  ,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
-  ,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desiredvelocity")
-  ,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
-                KpSIN << KdSIN << positionSIN << desiredpositionSIN
-                << velocitySIN << desiredvelocitySIN,
-                "ControlPD("+name+")::output(vector)::control" )
+ControlPD::ControlPD( const std::string & name )
+ :Entity(name)
+ ,TimeStep(0)
+ ,KpSIN(NULL,"ControlPD("+name+")::input(vector)::Kp")
+ ,KdSIN(NULL,"ControlPD("+name+")::input(vector)::Kd")
+ ,positionSIN(NULL,"ControlPD("+name+")::input(vector)::position")
+ ,desiredpositionSIN(NULL,"ControlPD("+name+")::input(vector)::desired_position")
+ ,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
+ ,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desired_velocity")
+ ,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
+         KpSIN << KdSIN << positionSIN << desiredpositionSIN
+         << velocitySIN << desiredvelocitySIN,
+        "ControlPD("+name+")::output(vector)::control" )
+  ,positionErrorSOUT(boost::bind(&ControlPD::computeControl,this,_1,_2),
+        controlSOUT,
+        "ControlPD("+name+")::output(vector)::position_error")
+  ,velocityErrorSOUT(boost::bind(&ControlPD::computeControl,this,_1,_2),
+        controlSOUT,
+        "ControlPD("+name+")::output(vector)::velocity_error")
 {
   init(TimeStep);
-  Entity::signalRegistration( KpSIN << KdSIN << positionSIN
-			      << desiredpositionSIN << velocitySIN
-			      << desiredvelocitySIN << controlSOUT );
+  Entity::signalRegistration( KpSIN << KdSIN << positionSIN << 
+    desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT 
+    << positionErrorSOUT << velocityErrorSOUT );
 }
 
 void ControlPD::
@@ -64,12 +69,11 @@ init(const double& Stept)
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-void ControlPD::
-display( std::ostream& os ) const
+void ControlPD::display( std::ostream& os ) const
 {
   os << "ControlPD "<<getName();
   try{
-    os <<"control = "<<controlSOUT;
+    os <<"control = "<<controlSOUT; 
   }
   catch (ExceptionSignal e) {}
   os <<" ("<<TimeStep<<") ";
@@ -87,25 +91,43 @@ double& ControlPD::setsize(int dimension)
 }
 
 dynamicgraph::Vector& ControlPD::
-computeControl( dynamicgraph::Vector &tau, int t )
+computeControl( dynamicgraph::Vector &tau, int t ) 
 {
-  sotDEBUGIN(15);
+  sotDEBUGIN(15);     
   const dynamicgraph::Vector& Kp = KpSIN(t);
-  const dynamicgraph::Vector& Kd = KdSIN(t);
+  const dynamicgraph::Vector& Kd = KdSIN(t);      
   const dynamicgraph::Vector& position = positionSIN(t);
-  const dynamicgraph::Vector& desiredposition = desiredpositionSIN(t);
-  const dynamicgraph::Vector& velocity = velocitySIN(t);
-  const dynamicgraph::Vector& desiredvelocity = desiredvelocitySIN(t);
-
-  dynamicgraph::Vector::Index size = Kp.size();
-
+  const dynamicgraph::Vector& desired_position = desiredpositionSIN(t);
+  const dynamicgraph::Vector& velocity = velocitySIN(t);      
+  const dynamicgraph::Vector& desired_velocity = desiredvelocitySIN(t);
+      
+  dynamicgraph::Vector::Index size = Kp.size();   
   tau.resize(size);
+  position_error.resize(size);
+  velocity_error.resize(size);
 
-  tau.array() = (desiredposition.array()-position.array())*Kp.array();
-  tau.array() = (desiredvelocity.array()-velocity.array())*Kd.array();
+  position_error.array() = desired_position.array()-position.array();  
+  velocity_error.array() = desired_velocity.array()-velocity.array();
+
+  tau.array() = position_error.array()*Kp.array() 
+              + velocity_error.array()*Kd.array();
+  
   sotDEBUGOUT(15);
-  // std::cout << " tau " << tau << std::endl;
-  // std::cout << "velocity " << velocity << std::endl;
   return tau;
 
 }
+
+dynamicgraph::Vector& ControlPD::
+getPositionError( dynamicgraph::Vector &position_error, int t)
+{
+  //sotDEBUGOUT(15) ??
+  controlSOUT(t);
+  return position_error;
+}
+
+dynamicgraph::Vector& ControlPD::
+getVelocityError( dynamicgraph::Vector &velocity_error, int t)
+{
+  controlSOUT(t);
+  return velocity_error;
+}