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;

    /* --- 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,
       			    KpSIN << KdSIN << positionSIN << positionRefSIN
       			    << velocitySIN << velocityRefSIN )
    {
      Entity::signalRegistration( KpSIN << KdSIN << positionSIN << positionRefSIN
				  << velocitySIN << velocityRefSIN
				  << controlSOUT );

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

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

      addCommand("velocityOnly",
		 makeCommandVoid0(*this,&ControllerPD::setGainVelocityOnly,
				  docCommandVoid0("Set a friction-style controller.")));

      addCommand("stdGain",
		 makeCommandVoid1(*this,&ControllerPD::setStandardGains,
				  docCommandVoid1("Set spefic gains.",
						  "string (in low|medium|high)")));

    }


    /* --- 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;
    }

    void ControllerPD::
    setGainVelocityOnly( void )
    {
      ml::Vector zero(_dimension); zero.fill(0);
      positionSIN = zero;
      positionRefSIN = zero;
      KpSIN = zero;
    }

    /** Give some specific values for the Kp and Kd gains. Possible
     * configs are "low", "middle" and "high".
     * Warning: middle and high only works for dim 30.
     * TODO: properly throw errors when needed.
     */
    void ControllerPD::
    setStandardGains( const std::string & config )
    {

      if( config =="low" )
	{
	  // Low gains
	  ml::Vector Kp(_dimension); Kp.fill(100);
	  ml::Vector Kd(_dimension); Kd.fill(20);
	  KpSIN = Kp;
	  KdSIN = Kd;
	}
      else if( config =="medium" )
	{
	  // Medium gains
	  if( _dimension != 30 )
	    { std::cerr << "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
	  if( _dimension != 30 )
	    { std::cerr << "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;
	}
    }
    /* --- 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" )
	{ setGainVelocityOnly(); }
      else if( cmdLine == "stdGain" )
	{
	  std::string config = "high";
	  cmdArgs >> std::ws; if( cmdArgs.good() ) cmdArgs >> config;
	  setStandardGains( config );
	}
      else
	{
	  Entity::commandLine(cmdLine,cmdArgs,os);
	}
    }






  } // namespace dyninv
} // namespace sot