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 <dynamic-graph/command.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>

#include <jrl/mal/matrixabstractlayer.hh>

namespace sot
{
  namespace dyninv
  {

    using namespace dg;

    DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");

    ControllerPD::
    ControllerPD( const std::string & name )
      :Entity(name)
      ,sensorWorldRotationSIN(NULL,"sotControllerPD("+name
			      +")::input(MatrixRotation)::sensorWorldRotation")
      ,sensorEmbeddedPositionSIN(NULL,"sotControllerPD("+name
				 +")::input(MatrixHomo)::sensorEmbeddedPosition")
      ,contactWorldPositionSIN(NULL,"sotControllerPD("+name
			       +")::input(MatrixHomo)::contactWorldPosition")
      ,contactEmbeddedPositionSIN(NULL,"sotControllerPD("+name
				  +")::input(MatrixHomo)::contactEmbeddedPosition")

      ,anglesSOUT( boost::bind(&ControllerPD::computeAngles,this,_1,_2),
		   sensorWorldRotationSIN<<sensorEmbeddedPositionSIN
		   <<contactWorldPositionSIN<<contactEmbeddedPositionSIN,
		   "sotControllerPD("+name+")::output(Vector)::angles" )
      ,flexibilitySOUT( boost::bind(&ControllerPD::computeFlexibilityFromAngles
				    ,this,_1,_2),
			anglesSOUT,
			"sotControllerPD("+name+")::output(matrixRotation)::flexibility" )
      ,driftSOUT( boost::bind(&ControllerPD::computeDriftFromAngles,this,_1,_2),
		  anglesSOUT,
		  "sotControllerPD("+name+")::output(matrixRotation)::drift" )
      ,sensorWorldRotationSOUT( boost::bind(&ControllerPD::computeSensorWorldRotation
					    ,this,_1,_2),
				anglesSOUT<<sensorWorldRotationSIN,
				"sotControllerPD("+name
				+")::output(matrixRotation)::sensorCorrectedRotation" )
      ,waistWorldRotationSOUT( boost::bind(&ControllerPD::computeWaistWorldRotation
					   ,this,_1,_2),
			       sensorWorldRotationSOUT<<sensorEmbeddedPositionSIN,
			       "sotControllerPD("+name
			       +")::output(matrixRotation)::waistWorldRotation" )
      ,waistWorldPositionSOUT( boost::bind(&ControllerPD::computeWaistWorldPosition
					   ,this,_1,_2),
			       flexibilitySOUT << contactEmbeddedPositionSIN,
			       "sotControllerPD("+name
			       +")::output(MatrixHomogeneous)::waistWorldPosition" )
      ,waistWorldPoseRPYSOUT( boost::bind(&ControllerPD::computeWaistWorldPoseRPY
					  ,this,_1,_2),
			      waistWorldPositionSOUT,
			      "sotControllerPD("+name
			      +")::output(vectorRollPitchYaw)::waistWorldPoseRPY" )

      ,fromSensor_(true)
    {
      sotDEBUGIN(5);

      signalRegistration( sensorWorldRotationSIN     << sensorEmbeddedPositionSIN
			  << contactWorldPositionSIN << contactEmbeddedPositionSIN
			  << anglesSOUT              << flexibilitySOUT
			  << driftSOUT               << sensorWorldRotationSOUT
			  << waistWorldRotationSOUT
			  << waistWorldPositionSOUT  << waistWorldPoseRPYSOUT  );

      // Commands
      std::string docstring;
      docstring = "    \n"
	"    Set flag specifying whether angle is measured from sensors or simulated.\n"
	"    \n"
	"      Input:\n"
	"        - a boolean value.\n"
	"    \n";
      addCommand("setFromSensor",
		 new ::dynamicgraph::command::Setter<ControllerPD, bool>
		 (*this, &ControllerPD::fromSensor, docstring));

      docstring = "    \n"
	"    Get flag specifying whether angle is measured from sensors or simulated.\n"
	"    \n"
	"      No input,\n"
	"      return a boolean value.\n"
	"    \n";
      addCommand("getFromSensor",
		 new ::dynamicgraph::command::Getter<ControllerPD, bool>
		 (*this, &ControllerPD::fromSensor, docstring));

      sotDEBUGOUT(5);
    }


    ControllerPD::
    ~ControllerPD( void )
    {
      sotDEBUGIN(5);

      sotDEBUGOUT(5);
      return;
    }

    /* --- SIGNALS -------------------------------------------------------------- */
    /* --- SIGNALS -------------------------------------------------------------- */
    /* --- SIGNALS -------------------------------------------------------------- */
    ml::Vector& ControllerPD::
    computeAngles( ml::Vector& res,
		   const int& time )
    {
      sotDEBUGIN(15);

      res.resize(3);

      const MatrixRotation &worldestRchest = sensorWorldRotationSIN( time );
      sotDEBUG(35) << "worldestRchest = " << std::endl << worldestRchest;
      const MatrixHomogeneous &waistMchest = sensorEmbeddedPositionSIN( time );
      MatrixRotation waistRchest; waistMchest.extract( waistRchest );

      const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
      MatrixRotation waistRleg; waistMleg.extract( waistRleg );
      MatrixRotation chestRleg; waistRchest.transpose().multiply( waistRleg,chestRleg );
      MatrixRotation worldestRleg; worldestRchest.multiply( chestRleg,worldestRleg );

      sotDEBUG(35) << "worldestRleg = " << std::endl << worldestRleg;

      /* Euler angles with following code: (-z)xy, -z being the yaw drift, x the
       * first flexibility and y the second flexibility. */
      const double TOLERANCE_TH = 1e-6;

      const MatrixRotation &R = worldestRleg;
      if( (fabs(R(0,1))<TOLERANCE_TH)&&(fabs(R(1,1))<TOLERANCE_TH) ) 
	{
	  /* This means that cos(X) is very low, ie flex1 is close to 90deg.
	   * I take the case into account, but it is bloody never going 
	   * to happens. */
	  if( R(2,1)>0 ) res(0)=M_PI/2; else res(0) = -M_PI/2;
	  res(2) = atan2( -R(0,2),R(0,0) );
	  res(1) = 0;

	  /* To sum up: if X=PI/2, then Y and Z are in singularity.
	   * we cannot decide both of then. I fixed Y=0, which
	   * means that all the measurement coming from the sensor
	   * is assumed to be drift of the gyro. */
	}
      else
	{
	  double &X = res(0);
	  double &Y = res(1);
	  double &Z = res(2);

	  Y = atan2( R(2,0),R(2,2) );
	  Z = atan2( R(0,1),R(1,1) );
	  if( fabs(R(2,0))>fabs(R(2,2)) )
	    { X=atan2( R(2,1)*sin(Y),R(2,0) ); }
	  else 
	    { X=atan2( R(2,1)*cos(Y),R(2,2) ); }
	}
  
      sotDEBUG(35) << "angles = " << res;


      sotDEBUGOUT(15);
      return res;
    }

    /* compute the transformation matrix of the flexibility, ie
     * feetRleg. 
     */
    MatrixRotation& ControllerPD::
    computeFlexibilityFromAngles( MatrixRotation& res,
				  const int& time )
    {
      sotDEBUGIN(15);

      const ml::Vector & angles = anglesSOUT( time );
      double cx= cos( angles(0) );
      double sx= sin( angles(0) );
      double cy= cos( angles(1) );
      double sy= sin( angles(1) );
  
      res(0,0) = cy;
      res(0,1) = 0;
      res(0,2) = -sy;

      res(1,0) = -sx*sy;
      res(1,1) = cx;
      res(1,2) = -sx*cy;

      res(2,0) = cx*sy;
      res(2,1) = sx;
      res(2,2) = cx*cy;

      sotDEBUGOUT(15);
      return res;
    }

    /* Compute the rotation matrix of the drift, ie the
     * transfo from the world frame to the estimated (drifted) world
     * frame: worldRworldest.
     */
    MatrixRotation& ControllerPD::
    computeDriftFromAngles( MatrixRotation& res,
			    const int& time )
    {
      sotDEBUGIN(15);
  
      const ml::Vector & angles = anglesSOUT( time );
      double cz = cos( angles(2) );
      double sz = sin( angles(2) );
  
      res(0,0) = cz;
      res(0,1) = -sz;
      res(0,2) = 0;

      // z is the positive angle (R_{-z} has been computed
      // in the <angles> function). Thus, the /-/sin(z) is in 0,1
      res(1,0) = sz;  
      res(1,1) = cz;
      res(1,2) = 0;

      res(2,0) = 0;
      res(2,1) = 0;
      res(2,2) = 1;

      sotDEBUGOUT(15);
      return res;
    }

    MatrixRotation& ControllerPD::
    computeSensorWorldRotation( MatrixRotation& res,
				const int& time )
    {
      sotDEBUGIN(15);
  
      const MatrixRotation & worldRworldest = driftSOUT( time );
      const MatrixRotation & worldestRsensor = sensorWorldRotationSIN( time );
  
      worldRworldest.multiply( worldestRsensor,res );

      sotDEBUGOUT(15);
      return res;
    }
 
    MatrixRotation& ControllerPD::
    computeWaistWorldRotation( MatrixRotation& res,
			       const int& time )
    {
      sotDEBUGIN(15);
  
      // chest = sensor
      const MatrixRotation & worldRsensor = sensorWorldRotationSOUT( time );
      const MatrixHomogeneous & waistMchest = sensorEmbeddedPositionSIN( time );
      MatrixRotation waistRchest; waistMchest.extract( waistRchest );
  
      worldRsensor .multiply( waistRchest.transpose(),res );

      sotDEBUGOUT(15);
      return res;
    }



    MatrixHomogeneous& ControllerPD::
    computeWaistWorldPosition( MatrixHomogeneous& res,
			       const int& time )
    {
      sotDEBUGIN(15);
  
      const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
      const MatrixHomogeneous& contactPos = contactWorldPositionSIN( time );
      MatrixHomogeneous legMwaist; waistMleg.inverse( legMwaist );
      MatrixHomogeneous tmpRes;
      if( fromSensor_ )
	{ 
	  const MatrixRotation & Rflex = flexibilitySOUT( time ); // footRleg
	  ml::Vector zero(3); zero.fill(0.);
	  MatrixHomogeneous footMleg; footMleg.buildFrom( Rflex,zero );
				    
	  footMleg.multiply( legMwaist,tmpRes );
	}
      else { tmpRes = legMwaist; }

      contactPos.multiply( tmpRes, res );
      sotDEBUGOUT(15);
      return res;
    }

    ml::Vector& ControllerPD::
    computeWaistWorldPoseRPY( ml::Vector& res,
			      const int& time )
    {
      const MatrixHomogeneous & M = waistWorldPositionSOUT( time );

      MatrixRotation R; M.extract(R);
      VectorRollPitchYaw r; r.fromMatrix(R);
      ml::Vector t(3); M.extract(t);

      res.resize(6);
      for( int i=0;i<3;++i ) { res(i)=t(i); res(i+3)=r(i); }

      return res;
    }

    /* --- PARAMS --------------------------------------------------------------- */
    /* --- PARAMS --------------------------------------------------------------- */
    /* --- PARAMS --------------------------------------------------------------- */
    void ControllerPD::
    commandLine( const std::string& cmdLine,
		 std::istringstream& cmdArgs,
		 std::ostream& os )
    {
      sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;

      if( cmdLine == "help" )
	{
	  Entity::commandLine(cmdLine,cmdArgs,os);
	}
      else if( cmdLine == "fromSensor" )
	{
	  std::string val; cmdArgs>>val;
	  if( ("true"==val)||("false"==val) )
	    {
	      fromSensor_ = ( val=="true" );
	    } else {
	    os << "fromSensor = " << (fromSensor_?"true":"false") << std::endl;
	  }
	}
      else { Entity::commandLine( cmdLine,cmdArgs,os); }
    }

  } // namespace dyninv
} // namespace sot