force-compensation.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#include <dynamic-graph/factory.h>
#include <sot/dynamic-pinocchio/force-compensation.h>
 
#include <sot/core/debug.hh>
#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, sigtime_t /*time*/) {
  //   sotDEBUGIN(45);
  //   if(! calibrationStarted ) { sotDEBUGOUT(45); return dummy=0; }
 
  //   addCalibrationValue( torsorSIN(time),worldRhandSIN(time) );
  //   sotDEBUGOUT(45);
  return dummy = 1;
}