controller-pd.cpp

/*
 * Copyright 2011, Nicolas Mansard, LAAS-CNRS
 *
 * This file is part of sot-dyninv.
 * sot-dyninv 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-dyninv 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-dyninv.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <sot-dyninv/controller-pd.h>
#include <sot-core/debug.h>
#include <dynamic-graph/factory.h>

#include <sot-dyninv/commands-helper.h>

namespace sot
{
  namespace dyninv
  {


    namespace dg = ::dynamicgraph;
    using namespace dg;
    using ::dynamicgraph::command::makeDirectGetter;
    using ::dynamicgraph::command::docDirectGetter;
    using ::dynamicgraph::command::makeDirectSetter;
    using ::dynamicgraph::command::docDirectSetter;

    /* --- DG FACTORY ------------------------------------------------------- */
    DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");

    /* --- CONSTRUCTION ----------------------------------------------------- */
    /* --- CONSTRUCTION ----------------------------------------------------- */
    /* --- CONSTRUCTION ----------------------------------------------------- */
    ControllerPD::
    ControllerPD( const std::string & name )
      : Entity(name)

      ,CONSTRUCT_SIGNAL_IN(Kp,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(Kd,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(position,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(positionRef,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(velocityRef,ml::Vector)

      ,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,ControllerPD,
			    KpSIN << KdSIN << positionSIN << positionRefSIN
			    << velocitySIN << velocityRefSIN )
    {
      Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
				  << velocitySIN << velocityRefSIN
				  << controlSOUT );


      /* Commands. */
      addCommand("getSize",
		 makeDirectGetter(*this,&_dimension,
				  docDirectGetter("dimension","int")));

      // std::string docstring
      // 		 = "\nSet the vectors dimension.\n\nInput:\n- an int.\nVoid return.\n\n";
      // addCommand("setSize",
      // 		 new ::dynamicgraph::command::Setter<ControllerPD,int>
      // 		 (*this, &ControllerPD::size,docstring));

      addCommand("setSize",
		 makeDirectSetter(*this, &_dimension,
				  docDirectSetter("dimension","int")));
    }


    /* --- SIGNALS ---------------------------------------------------------- */
    /* --- SIGNALS ---------------------------------------------------------- */
    /* --- SIGNALS ---------------------------------------------------------- */

    ml::Vector& ControllerPD::
    controlSOUT_function( ml::Vector &tau, int iter )
    {
      sotDEBUGIN(15);
      const ml::Vector& Kp = KpSIN(iter);
      const ml::Vector& Kd = KdSIN(iter);
      const ml::Vector& position = positionSIN(iter);
      const ml::Vector& desiredposition = positionRefSIN(iter);
      const unsigned int size = Kp.size();

      assert( _dimension == (int)size );
      assert( size==Kp.size() );        assert( size==Kd.size() );
      assert( size==position.size() );  assert( size==desiredposition.size() );

      bool useVelocity = velocitySIN;
      ml::Vector velocity;
      bool useVelocityDesired = false;
      ml::Vector desiredvelocity;
      if( useVelocity ) // TODO: there is a useless copy here. Use a pointer?
	{
	  velocity = velocitySIN(iter);
	  assert( size==velocity.size() );
	  useVelocityDesired = velocityRefSIN;
	  if( useVelocityDesired )
	    {
	      desiredvelocity = velocityRefSIN(iter);
	      assert( size==desiredvelocity.size() );
	    }
	}

      tau.resize(size); double dv; // TODO: use dv from start to avoid the copy.
      for(unsigned i = 0u; i < size; ++i)
	{
	  tau(i) = Kp(i)*(desiredposition(i)-position(i));
	  if( useVelocity )
	    {
	      dv= velocity(i);
	      if( useVelocityDesired )
		dv-=desiredvelocity(i);
	      tau(i) -= Kd(i)*dv;
	    }
	}

      sotDEBUG(15) << "p = " << position << std::endl;
      sotDEBUG(15) << "pd = " << desiredposition << std::endl;
      if( useVelocity ) {sotDEBUG(15) << "v= " << velocity << std::endl;}
      if( useVelocityDesired ) { sotDEBUG(15) << "vd= " << velocity << std::endl; }
      sotDEBUG(15) << "kp = " << Kp << std::endl;
      sotDEBUG(15) << "kd = " << Kd << std::endl;
      sotDEBUG(15) << "PD torque= " << tau << std::endl;

      sotDEBUGOUT(15);
      return tau;
    }


    /* --- COMMANDS ---------------------------------------------------------- */
    /* --- MEMBERS ---------------------------------------------------------- */
    void ControllerPD::
    size(const int & dimension)
    {
      _dimension = dimension;
    }

    int ControllerPD::
    size(void) const
    {
      return _dimension;
    }

    /* --- ENTITY ----------------------------------------------------------- */
    /* --- ENTITY ----------------------------------------------------------- */
    /* --- ENTITY ----------------------------------------------------------- */

    void ControllerPD::
    display( std::ostream& os ) const
    {
      os << "ControllerPD "<<getName();
      try
	{
	  os <<"control = "<<controlSOUT;
	}
      catch (ExceptionSignal e) {}
    }

    void ControllerPD::
    commandLine( const std::string& cmdLine,
		 std::istringstream& cmdArgs,
		 std::ostream& os )
    {
      if( cmdLine == "help" )
	{
	  os << "sotControlPD:\n"
	     << " - size <arg>\t\tset the size of the vector.\n"
	     << " - stdGain \t\tset the input vector gains according to the size for HRP2.\n"
	     << " - velocityonly <arg>\t\tset Kp = 0.\n"
	     << std::endl;
	}
      else if( cmdLine == "size" )
	{
	  cmdArgs >> std::ws;
	  if( cmdArgs.good() )
	    {
	      unsigned int i; cmdArgs >> i ;
	      size(i);
	    }
	  else
	    {
	      os << "size = " << size() << std::endl;
	    }
	}
      else if( cmdLine == "velocityonly" )
	{
	  ml::Vector zero(_dimension); zero.fill(0);
	  positionSIN = zero;
	  positionRefSIN = zero;
	  KpSIN = zero;
	}
      else if( cmdLine == "stdGain" )
	{
	  std::string config = "high";
	  cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;

	  if( config =="low" )
	    {
	      // Low
	      ml::Vector Kp(_dimension); Kp.fill(100);
	      ml::Vector Kd(_dimension); Kp.fill(20);
	      KpSIN = Kp;
	      KdSIN = Kd;
	    }
	  else if( config =="medium" )
	    {
	      // Medium gains
	      // Kp = [30](400,1000,2000,1800,2000,1000,400,1000,2000,1800,2000,1000,500,250,200,200,300,300,200,400,400,200,200,300, 300,200,400,400,200,200);
	      // Kd = [30] (40,100,200,180,200,100,40,100,200,180,200,100,50,25,20,20,30,30,20,40,40,20,20,30, 30,20,40,40,20,20);
	      if( _dimension != 30 )
		{ os << "Only working for dim=30!" << std::endl; return; }

	      ml::Vector Kp(_dimension),Kd(_dimension);
	      unsigned int i=0;
	      Kp(i++)=400;   Kp(i++)=1000; Kp(i++)=2000;  Kp(i++)=1800;  Kp(i++)=2000;
	      Kp(i++)=1000;  Kp(i++)=400;  Kp(i++)=1000;  Kp(i++)=2000;  Kp(i++)=1800;
	      Kp(i++)=2000;  Kp(i++)=1000; Kp(i++)=500;  Kp(i++)=250;  Kp(i++)=200;
	      Kp(i++)=200;   Kp(i++)=300;  Kp(i++)=300;  Kp(i++)=200;  Kp(i++)=400;
	      Kp(i++)=400;   Kp(i++)=200;  Kp(i++)=200;  Kp(i++)=300;  Kp(i++)= 300;
	      Kp(i++)=200;   Kp(i++)=400;  Kp(i++)=400;  Kp(i++)=200;  Kp(i++)=200;

	      i=0;
	      Kd(i++)=40;  Kd(i++)=100;  Kd(i++)=200;  Kd(i++)=180;  Kd(i++)=200;
	      Kd(i++)=100; Kd(i++)=40;   Kd(i++)=100;  Kd(i++)=200;  Kd(i++)=180;
	      Kd(i++)=200; Kd(i++)=100;  Kd(i++)=50;   Kd(i++)=25;   Kd(i++)=20;
	      Kd(i++)=20;  Kd(i++)=30;   Kd(i++)=30;   Kd(i++)=20;   Kd(i++)=40;
	      Kd(i++)=40;  Kd(i++)=20;   Kd(i++)=20;   Kd(i++)=30;   Kd(i++)= 30;
	      Kd(i++)=20;  Kd(i++)=40;   Kd(i++)=40;   Kd(i++)=20;   Kd(i++)=20;

	      KpSIN = Kp;
	      KdSIN = Kd;
	    }
	  else // high
	    {
	      // High gains
	      // Kp = [30](4000,10000,20000,18000,20000,10000,4000,10000,20000,18000,20000,10000,5000,2500,2000,2000,3000,3000,2000,4000,4000,2000,2000,3000, 3000,2000,4000,4000,2000,2000);
	      //Kd = [30](120,300,600,500,600,300,120,300,600,500,600,300,150,75,60,60,90,90,60,120,120,60,60,90, 90,60,120,120,60,60);

	      if( _dimension != 30 )
		{ os << "Only working for dim=30!" << std::endl; return; }

	      ml::Vector Kp(_dimension),Kd(_dimension);
	      unsigned int i=0;
	      Kp(i++)=4000;  Kp(i++)=10000; Kp(i++)=20000; Kp(i++)=18000;  Kp(i++)=20000;
	      Kp(i++)=10000; Kp(i++)=4000;  Kp(i++)=10000; Kp(i++)=20000;  Kp(i++)=18000;
	      Kp(i++)=20000; Kp(i++)=10000; Kp(i++)=5000;  Kp(i++)=2500;   Kp(i++)=2000;
	      Kp(i++)=2000;  Kp(i++)=3000;  Kp(i++)=3000;  Kp(i++)=2000;   Kp(i++)=4000;
	      Kp(i++)=4000;  Kp(i++)=2000;  Kp(i++)=2000;  Kp(i++)=3000;   Kp(i++)= 3000;
	      Kp(i++)=2000;  Kp(i++)=4000;  Kp(i++)=4000;  Kp(i++)=2000;   Kp(i++)=2000;

	      i=0;
	      Kd(i++)=120;   Kd(i++)=300;   Kd(i++)=600;   Kd(i++)=500;    Kd(i++)=600;
	      Kd(i++)=300;   Kd(i++)=120;   Kd(i++)=300;   Kd(i++)=600;    Kd(i++)=500;
	      Kd(i++)=600;   Kd(i++)=300;   Kd(i++)=150;   Kd(i++)=75;     Kd(i++)=60;
	      Kd(i++)=60;    Kd(i++)=90;    Kd(i++)=90;    Kd(i++)=60;     Kd(i++)=120;
	      Kd(i++)=120;   Kd(i++)=60;    Kd(i++)=60;    Kd(i++)=90;     Kd(i++)= 90;
	      Kd(i++)=60;    Kd(i++)=120;   Kd(i++)=120;   Kd(i++)=60;     Kd(i++)=60;

	      KpSIN = Kp;
	      KdSIN = Kd;
	    }
	}
      else
	{
	  Entity::commandLine(cmdLine,cmdArgs,os);
	}
    }






  } // namespace dyninv
} // namespace sot