force-compensation.cpp

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <sot-dynamic-pinocchio/force-compensation.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot/core/macros-signal.hh>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ForceCompensationPlugin,"ForceCompensation");

/* --- PLUGIN --------------------------------------------------------------- */
/* --- PLUGIN --------------------------------------------------------------- */
/* --- PLUGIN --------------------------------------------------------------- */
ForceCompensation::
ForceCompensation(void)
  :usingPrecompensation(false)
{}


ForceCompensationPlugin::
ForceCompensationPlugin( const std::string & name )
  :Entity(name)
  ,calibrationStarted(false)

  ,torsorSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::torsorIN")
  ,worldRhandSIN(NULL,"sotForceCompensation("+name+")::input(MatrixRotation)::worldRhand")

  ,handRsensorSIN(NULL,"sotForceCompensation("+name+")::input(MatrixRotation)::handRsensor")
  ,translationSensorComSIN(NULL,"sotForceCompensation("+name+")::input(vector3)::sensorCom")
  ,gravitySIN(NULL,"sotForceCompensation("+name+")::input(vector6)::gravity")
  ,precompensationSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::precompensation")
  ,gainSensorSIN(NULL,"sotForceCompensation("+name+")::input(matrix6)::gain")
  ,deadZoneLimitSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::deadZoneLimit")
  ,transSensorJointSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::sensorJoint")  ,inertiaJointSIN(NULL,"sotForceCompensation("+name+")::input(matrix6)::inertiaJoint")
  ,velocitySIN(NULL,"sotForceCompensation("+name+")::input(vector6)::velocity")
  ,accelerationSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::acceleration")

  ,handXworldSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeHandXworld,
				      worldRhandSIN,MatrixRotation,
				      translationSensorComSIN,dynamicgraph::Vector ),
		   "sotForceCompensation("+name+")::output(MatrixForce)::handXworld" )
   ,handVsensorSOUT( SOT_INIT_SIGNAL_1( ForceCompensation::computeHandVsensor,
					handRsensorSIN,MatrixRotation),
		     "sotForceCompensation("+name+")::output(MatrixForce)::handVsensor" )
   ,torsorDeadZoneSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::torsorNullifiedIN")

  ,sensorXhandSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeSensorXhand,
				       handRsensorSIN,MatrixRotation,
				       transSensorJointSIN,dynamicgraph::Vector ),
		   "sotForceCompensation("+name+")::output(MatrixForce)::sensorXhand" )
//   ,inertiaSensorSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeInertiaSensor,
// 					  inertiaJointSIN,dynamicgraph::Matrix,
// 					  sensorXhandSOUT,MatrixForce ),
// 		       "ForceCompensation("+name+")::output(MatrixForce)::inertiaSensor" )
   ,momentumSOUT( SOT_INIT_SIGNAL_4(ForceCompensation::computeMomentum,
				    velocitySIN,dynamicgraph::Vector,
				    accelerationSIN,dynamicgraph::Vector,
				    sensorXhandSOUT,MatrixForce,
				    inertiaJointSIN,dynamicgraph::Matrix),
		  "sotForceCompensation("+name+")::output(Vector6)::momentum" )
  ,momentumSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::momentumIN")

  ,torsorCompensatedSOUT( SOT_INIT_SIGNAL_7(ForceCompensation::computeTorsorCompensated,
					    torsorSIN,dynamicgraph::Vector,
					    precompensationSIN,dynamicgraph::Vector,
					    gravitySIN,dynamicgraph::Vector,
					    handXworldSOUT,MatrixForce,
					    handVsensorSOUT,MatrixForce,
					    gainSensorSIN,dynamicgraph::Matrix,
					    momentumSIN,dynamicgraph::Vector),
			  "sotForceCompensation("+name+")::output(Vector6)::torsor" )
  ,torsorDeadZoneSOUT( SOT_INIT_SIGNAL_2(ForceCompensation::computeDeadZone,
					 torsorDeadZoneSIN,dynamicgraph::Vector,
					 deadZoneLimitSIN,dynamicgraph::Vector),
		       "sotForceCompensation("+name+")::output(Vector6)::torsorNullified" )
   ,calibrationTrigerSOUT( boost::bind(&ForceCompensationPlugin::calibrationTriger,
				       this,_1,_2),
			   torsorSIN << worldRhandSIN,
			   "sotForceCompensation("+name+")::output(Dummy)::calibrationTriger")
{
  sotDEBUGIN(5);

  signalRegistration(torsorSIN);
  signalRegistration(worldRhandSIN);
  signalRegistration(handRsensorSIN);
  signalRegistration(translationSensorComSIN);
  signalRegistration(gravitySIN);
  signalRegistration(precompensationSIN);
  signalRegistration(gainSensorSIN);
  signalRegistration(deadZoneLimitSIN);
  signalRegistration(transSensorJointSIN);
  signalRegistration(inertiaJointSIN);
  signalRegistration(velocitySIN );
  signalRegistration(accelerationSIN);
  signalRegistration(handXworldSOUT);
  signalRegistration(handVsensorSOUT);
  signalRegistration(torsorDeadZoneSIN);
  signalRegistration(sensorXhandSOUT);
  signalRegistration(momentumSOUT);
  signalRegistration(momentumSIN);
  signalRegistration(torsorCompensatedSOUT);
  signalRegistration(torsorDeadZoneSOUT);
  signalRegistration(calibrationTrigerSOUT);
  torsorDeadZoneSIN.plug(&torsorCompensatedSOUT);

  // By default, I choose: momentum is not compensated.
  //  momentumSIN.plug( &momentumSOUT );
  dynamicgraph::Vector v(6); v.fill(0); momentumSIN = v;

  sotDEBUGOUT(5);
}


ForceCompensationPlugin::
~ForceCompensationPlugin( void )
{
  return;
}

/* --- CALIB --------------------------------------------------------------- */
/* --- CALIB --------------------------------------------------------------- */
/* --- CALIB --------------------------------------------------------------- */

MatrixRotation ForceCompensation::I3;

void ForceCompensation::
clearCalibration( void )
{
  torsorList.clear();
  rotationList.clear();
}


void ForceCompensation::
addCalibrationValue( const dynamicgraph::Vector& /*torsor*/,
		     const MatrixRotation & /*worldRhand*/ )
{
  sotDEBUGIN(45);

  //   sotDEBUG(25) << "Add torsor: t"<<torsorList.size() <<" = " << torsor;
  //   sotDEBUG(25) << "Add Rotation: wRh"<<torsorList.size() <<" = " << worldRhand;

  //   torsorList.push_back(torsor);
  //   rotationList.push_back(worldRhand);

  sotDEBUGOUT(45);
}

dynamicgraph::Vector ForceCompensation::
calibrateTransSensorCom( const dynamicgraph::Vector& gravity,
			 const MatrixRotation& /*handRsensor*/ )
{
  //   sotDEBUGIN(25);

  //   dynamicgraph::Vector grav3(3);
  //   dynamicgraph::Vector Rgrav3(3),tau(3),Rtau(3);
  //   for( unsigned int j=0;j<3;++j ) { grav3(j)=gravity(j); }

  //   std::list< dynamicgraph::Vector >::iterator iterTorsor = torsorList.begin();
  //   std::list< MatrixRotation >::const_iterator iterRotation
  //     = rotationList.begin();


  //   const unsigned int NVAL = torsorList.size();
  //   if( 0==NVAL )
  //     {
  //       dynamicgraph::Vector zero(3); zero.fill(0);
  //       return zero;
  //     }

  //   if(NVAL!=rotationList.size() )
  //     {
  // 	  // TODO: ERROR THROW
  //     }
  //   dynamicgraph::Matrix torsors( 3,NVAL );
  //   dynamicgraph::Matrix gravitys( 3,NVAL );

  //   for( unsigned int i=0;i<NVAL;++i )
  //     {
  //       if( (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
  // 	{
  // 	  // TODO: ERROR THROW
  // 	  break;
  // 	}
  //       const dynamicgraph::Vector & torsor = *iterTorsor;
  //       const MatrixRotation & worldRhand = *iterRotation;

  //       for( unsigned int j=0;j<3;++j ) { tau(j)=torsor(j+3); }
  //       handRsensor.multiply(tau,Rtau);
  //       worldRhand.transpose().multiply( grav3,Rgrav3 );
  //       for( unsigned int j=0;j<3;++j )
  // 	{
  // 	  torsors( j,i ) = -Rtau(j);
  // 	  gravitys( j,i ) = Rgrav3(j);
  // 	}
  //       sotDEBUG(35) << "R" << i << " = " << worldRhand;
  //       sotDEBUG(35) << "Rtau" << i << " = " << Rtau;
  //       sotDEBUG(35) << "Rg" << i << " = " << Rgrav3;

  //       iterTorsor++; iterRotation++;
  //     }

  //   sotDEBUG(35) << "Rgs = " << gravitys;
  //   sotDEBUG(35) << "Rtaus = " << torsors;

  //   dynamicgraph::Matrix gravsInv( gravitys.nbCols(),gravitys.nbRows() );
  //   sotDEBUG(25) << "Compute the pinv..." << std::endl;
  //   gravitys.pseudoInverse(gravsInv);
  //   sotDEBUG(25) << "Compute the pinv... [DONE]" << std::endl;
  //   sotDEBUG(25) << "gravsInv = " << gravsInv << std::endl;

  //   dynamicgraph::Matrix Skew(3,3);
  //   torsors.multiply( gravsInv,Skew );
  //   sotDEBUG(25) << "Skew = " << Skew << std::endl;

  //   dynamicgraph::Vector sc(3);
  //   sc(0)=(Skew(2,1)-Skew(1,2))*.5 ;
  //   sc(1)=(Skew(0,2)-Skew(2,0))*.5 ;
  //   sc(2)=(Skew(1,0)-Skew(0,1))*.5 ;
  //   sotDEBUG(15) << "SC = " << sc << std::endl;
  //   /* TAKE the antisym constraint into account inside the minimization pbm. */

  //   sotDEBUGOUT(25);
  //   return sc;
  return gravity;
}

dynamicgraph::Vector ForceCompensation::
calibrateGravity( const MatrixRotation& /*handRsensor*/,
		  bool /*precompensationCalibration*/,
		  const MatrixRotation& /*hand0RsensorArg*/ )
{
  sotDEBUGIN(25);

  //   MatrixRotation hand0Rsensor;
  //   if( &hand0Rsensor==&I3 ) hand0Rsensor.setIdentity();
  //   else hand0Rsensor=hand0RsensorArg;

  //   std::list< dynamicgraph::Vector >::const_iterator iterTorsor = torsorList.begin();
  //   std::list< MatrixRotation >::const_iterator iterRotation
  //     = rotationList.begin();

  //   const unsigned int NVAL = torsorList.size();
  //   if(NVAL!=rotationList.size() )
  //     {
  // 	  // TODO: ERROR THROW
  //     }
  //   if( 0==NVAL )
  //     {
  //       dynamicgraph::Vector zero(6); zero.fill(0);
  //       return zero;
  //     }

  //   dynamicgraph::Vector force(3),forceHand(3),forceWorld(3);
  //   dynamicgraph::Vector sumForce(3); sumForce.fill(0);

  //   for( unsigned int i=0;i<NVAL;++i )
  //     {
  //       if( (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
  // 	{
  // 	  // TODO: ERROR THROW
  // 	  break;
  // 	}
  //       const dynamicgraph::Vector & torsor = *iterTorsor;
  //       const MatrixRotation & R = *iterRotation;

  //       /* The sensor read [-] the value, and the grav is [-] the sensor force.
  //        * [-]*[-] = [+] -> force = + torsor(1:3). */
  //       for( unsigned int j=0;j<3;++j ) { force(j)=-torsor(j); }
  //       handRsensor.multiply(force,forceHand);
  //       if( usingPrecompensation )
  // 	{
  // 	  dynamicgraph::Matrix R_I(3,3); R_I = R.transpose();
  // 	  R_I -= hand0Rsensor;
  // 	  R_I.pseudoInverse(.01).multiply( forceHand,forceWorld );
  // 	}
  //       else
  // 	{ R.multiply( forceHand,forceWorld ); }

  //       sotDEBUG(35) << "R(" << i << "*3+1:" << i << "*3+3,:)  = " << R << "';";
  //       sotDEBUG(35) << "rhFs(" << i << "*3+1:" << i << "*3+3) = " << forceHand;
  //       sotDEBUG(45) << "fworld(" << i << "*3+1:" << i << "*3+3) = " << forceWorld;

  //       sumForce+= forceWorld;

  //       iterTorsor++; iterRotation++;
  //     }

  //   sumForce*= (1./NVAL);
  //   sotDEBUG(35) << "Fmean = " << sumForce;
  //   sumForce.resize(6,false);
  //   sumForce(3)=sumForce(4)=sumForce(5)=0.;

  //   sotDEBUG(25)<<"mg = " << sumForce<<std::endl;

  sotDEBUGOUT(25);
  dynamicgraph::Vector sumForce(3); sumForce.fill(0);
  return sumForce;
}


/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
MatrixForce& ForceCompensation::
computeHandXworld( const MatrixRotation & worldRhand,
		   const dynamicgraph::Vector & transSensorCom,
		   MatrixForce& res )
{
  sotDEBUGIN(35);

  sotDEBUG(25) << "wRrh = " << worldRhand <<std::endl;
  sotDEBUG(25) << "SC = " << transSensorCom <<std::endl;

  MatrixHomogeneous scRw;
  scRw.linear()=worldRhand.transpose();  scRw.translation()=transSensorCom;
  sotDEBUG(25) << "scMw = " << scRw <<std::endl;

  /////////////////////////////
  // res.buildFrom( scRw );
  Eigen::Vector3d _t = scRw.translation();
  MatrixRotation R(scRw.linear());
  Eigen::Matrix3d Tx;
  Tx << 0, -_t(2), _t(1),
    _t(2), 0, -_t(0),
    -_t(1), _t(0), 0;
  Eigen::Matrix3d sk; sk = Tx*R;

  res.block<3,3>(0,0) = R;
  res.block<3,3>(0,3).setZero();
  res.block<3,3>(3,0) = sk;
  res.block<3,3>(3,3) = R;
  ////////////////////////////


  sotDEBUG(15) << "scXw = " << res <<std::endl;

  sotDEBUGOUT(35);
  return res;
}

MatrixForce& ForceCompensation::
computeHandVsensor( const MatrixRotation & handRsensor,
		    MatrixForce& res )
{
  sotDEBUGIN(35);

  for( unsigned int i=0;i<3;++i )
    for( unsigned int j=0;j<3;++j )
      {
	res(i,j)=handRsensor(i,j);
	res(i+3,j+3)=handRsensor(i,j);
	res(i+3,j)=0.;
	res(i,j+3)=0.;
      }

  sotDEBUG(25) << "hVs" << res << std::endl;

  sotDEBUGOUT(35);
  return res;
}

MatrixForce& ForceCompensation::
computeSensorXhand( const MatrixRotation & /*handRsensor*/,
		    const dynamicgraph::Vector & transJointSensor,
		    MatrixForce& res )
{
  sotDEBUGIN(35);

  /* Force Matrix to pass from the joint frame (ie frame located
   * at the position of the motor, in which the acc is computed by Spong)
   * to the frame SensorHand where all the forces are expressed (ie
   * frame located at the sensor position bu oriented like the hand). */

  Eigen::Matrix3d Tx;
  Tx << 0, -transJointSensor(2), transJointSensor(1),
    transJointSensor(2), 0, -transJointSensor(0),
    -transJointSensor(1), transJointSensor(0), 0;

  res.block<3,3>(0,0).setIdentity();
  res.block<3,3>(0,3).setZero();
  res.block<3,3>(3,0) = Tx;
  res.block<3,3>(3,3).setIdentity();

  sotDEBUG(25) << "shXJ" << res << std::endl;

  sotDEBUGOUT(35);
  return res;
}

// dynamicgraph::Matrix& ForceCompensation::
// computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint,
// 		      const MatrixForce& sensorXhand,
// 		      dynamicgraph::Matrix& res )
// {
//   sotDEBUGIN(35);

//   /* Inertia felt at the sensor position, expressed in the orientation
//    * of the hand. */

//   res.resize(6,6);
//   sensorXhand.multiply( inertiaJoint,res );

//   sotDEBUGOUT(35);
//   return res;
// }


dynamicgraph::Vector& ForceCompensation::
computeTorsorCompensated( const dynamicgraph::Vector& torqueInput,
			  const dynamicgraph::Vector& torquePrecompensation,
			  const dynamicgraph::Vector& gravity,
			  const MatrixForce& handXworld,
			  const MatrixForce& handVsensor,
			  const dynamicgraph::Matrix& gainSensor,
			  const dynamicgraph::Vector& momentum,
			  dynamicgraph::Vector& res )

{
  sotDEBUGIN(35);
  /* Torsor in the sensor frame: K*(-torsred-gamma)+sVh*hXw*mg  */
  /* Torsor in the hand frame: hVs*K*(-torsred-gamma)+hXw*mg  */
  /* With gamma expressed in the sensor frame  (gamma_s = sVh*gamma_h) */

  sotDEBUG(25) << "t_nc = " << torqueInput;
  dynamicgraph::Vector torquePrecompensated(6);
  //if( usingPrecompensation )
  torquePrecompensated = torqueInput+torquePrecompensation;
  //else { torquePrecompensated = torqueInput; }
  sotDEBUG(25) << "t_pre = " << torquePrecompensated;

  dynamicgraph::Vector torqueS(6), torqueRH(6);
  torqueS = gainSensor*torquePrecompensated;
  res = handVsensor*torqueS;
  sotDEBUG(25) << "t_rh = " << res;

  dynamicgraph::Vector grh(6);
  grh = handXworld*gravity;
  grh *= -1;
  sotDEBUG(25) << "g_rh = " << grh;

  res+=grh;
  sotDEBUG(25) << "fcomp = " << res;

  res+=momentum;
  sotDEBUG(25) << "facc = " << res;


  /* TODO res += m xddot */

  sotDEBUGOUT(35);
  return res;
}

dynamicgraph::Vector& ForceCompensation::
crossProduct_V_F( const dynamicgraph::Vector& velocity,
		  const dynamicgraph::Vector& force,
		  dynamicgraph::Vector& res )
{
  /* [ v;w] x [ f;tau ] = [ w x f; v x f + w x tau ] */
  Eigen::Vector3d v,w,f,tau;
  for( unsigned int i=0;i<3;++i )
    {
      v(i)=velocity(i); w(i) = velocity(i+3);
      f(i) = force(i); tau(i) = force(i+3);
    }
  Eigen::Vector3d res1(3),res2a(3),res2b(3);
  res1 = w.cross(f);
  res2a = v.cross(f);
  res2b = w.cross(tau);
  res2a += res2b;

  res.resize(6);
  for( unsigned int i=0;i<3;++i )
    {
      res(i)=res1(i); res(i+3)=res2a(i);
    }
  return res;
}


dynamicgraph::Vector& ForceCompensation::
computeMomentum( const dynamicgraph::Vector& velocity,
		 const dynamicgraph::Vector& acceleration,
		 const MatrixForce& sensorXhand,
		 const dynamicgraph::Matrix& inertiaJoint,
		 dynamicgraph::Vector& res )
{
  sotDEBUGIN(35);

  /* Fs + Fext = I acc + V x Iv */
  dynamicgraph::Vector Iacc(6); Iacc = inertiaJoint* acceleration;
  res.resize(6); res = sensorXhand*Iacc;

  dynamicgraph::Vector Iv(6); Iv = inertiaJoint*velocity;
  dynamicgraph::Vector vIv(6); crossProduct_V_F( velocity,Iv,vIv );
  dynamicgraph::Vector XvIv(6); XvIv = sensorXhand*vIv;
  res+= XvIv;

  sotDEBUGOUT(35);
  return res;
}

dynamicgraph::Vector& ForceCompensation::
computeDeadZone( const dynamicgraph::Vector& torqueInput,
		 const dynamicgraph::Vector& deadZone,
		 dynamicgraph::Vector& res )
{
  sotDEBUGIN(35);

  if( torqueInput.size()>deadZone.size() ) return res;
  res.resize(torqueInput.size());
  for( int i=0;i<torqueInput.size();++i )
    {
      const double th = fabs(deadZone(i));
      if( (torqueInput(i)<th)&&(torqueInput(i)>-th) )
	{ res(i)=0; }
      else if(torqueInput(i)<0) res(i)=torqueInput(i)+th;
      else res(i)=torqueInput(i)-th;
    }

  sotDEBUGOUT(35);
  return res;
}


ForceCompensationPlugin::sotDummyType& ForceCompensationPlugin::
calibrationTriger( ForceCompensationPlugin::sotDummyType& dummy,int /*time*/ )
{
  //   sotDEBUGIN(45);
  //   if(! calibrationStarted ) { sotDEBUGOUT(45); return dummy=0; }

  //   addCalibrationValue( torsorSIN(time),worldRhandSIN(time) );
  //   sotDEBUGOUT(45);
  return dummy=1;
}