dynamic-integrator.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/dynamic-integrator.h>
#include <sot-core/debug.h>
#include <dynamic-graph/factory.h>

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

#include <sot-dyninv/mal-to-eigen.h>
#include <soth/Algebra.hpp>

/** This class proposes to integrate the acceleration given in input
 * to produce both velocity and acceleration. Initial conditions have to
 * be provided using the setters of position and velocity. The integration
 * has to be explicitely triggered by calling the command 'inc' (increments).
 */

namespace sot
{
  namespace dyninv
  {

    namespace dg = ::dynamicgraph;
    using namespace dg;

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

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

      ,CONSTRUCT_SIGNAL_IN(acceleration,ml::Vector)
      ,CONSTRUCT_SIGNAL_IN(dt,double)

      ,CONSTRUCT_SIGNAL_OUT(velocity,ml::Vector,sotNOSIGNAL)
      ,CONSTRUCT_SIGNAL_OUT(position,ml::Vector,sotNOSIGNAL)
   {
     Entity::signalRegistration( accelerationSIN << dtSIN
				 << velocitySOUT << positionSOUT );

     addCommand("inc",
		makeCommandVoid0(*this,&DynamicIntegrator::integrateFromSignals,
				  docCommandVoid0("Integrate acc, update v and p.")));

      addCommand("setPosition",
		 makeDirectSetter(*this,&position,
				  docDirectSetter("position","vector")));
      addCommand("setVelocity",
		 makeDirectSetter(*this,&velocity,
				  docDirectSetter("velocity","vector")));

    }

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

    ml::Vector& DynamicIntegrator::
    velocitySOUT_function( ml::Vector& mlv,int )
    {
      mlv = velocity;
      return mlv;
    }

    ml::Vector& DynamicIntegrator::
    positionSOUT_function( ml::Vector& mlp,int )
    {
      mlp = position;
      return mlp;
    }

    /* --- PROXYS ----------------------------------------------------------- */
    void DynamicIntegrator::
    integrateFromSignals( const int & time )
    {
      const ml::Vector & acc = accelerationSIN(time);
      const double & dt = dtSIN(time);

      integrate( acc,dt, velocity,position );
      velocitySOUT.setReady();
      positionSOUT.setReady();
    }

    void DynamicIntegrator::
    integrateFromSignals( void )
    {
      integrateFromSignals( accelerationSIN.getTime() + 1 );
    }

    void DynamicIntegrator::
    setPosition( const ml::Vector& p )
    {
      position = p;
      positionSOUT.setReady();
    }

    void DynamicIntegrator::
    setVelocity( const ml::Vector& v )
    {
      velocity = v;
      velocitySOUT.setReady();
    }

    void DynamicIntegrator::
    setState( const ml::Vector& p,const ml::Vector& v )
    {
      sotDEBUG(5) << "State: " << p << v << std::endl;
      position = p;
      velocity = v;
      velocitySOUT.setReady();
      positionSOUT.setReady();
    }

    /* --- COMMANDS ---------------------------------------------------------- */
    /* --- COMMANDS ---------------------------------------------------------- */
    /* --- COMMANDS ---------------------------------------------------------- */

    using namespace Eigen;

    namespace DynamicIntegratorStatic
    {
      template< typename D1 >
      static Matrix3d
      computeRotationMatrixFromEuler(const MatrixBase<D1> & euler)
      {
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);

	double Rpsi	= euler[0];
	double Rtheta = euler[1];
	double Rphy	= euler[2];

	double cosPhy = cos(Rphy);
	double sinPhy = sin(Rphy);

	double cosTheta = cos(Rtheta);
	double sinTheta = sin(Rtheta);

	double cosPsi = cos(Rpsi);
	double sinPsi = sin(Rpsi);

	Matrix3d rotation;

	rotation(0, 0) =  cosPhy * cosTheta;
	rotation(1, 0) =  sinPhy * cosTheta;
	rotation(2, 0) = -sinTheta;

	double   sinTheta__sinPsi = sinTheta * sinPsi;
	double   sinTheta__cosPsi = sinTheta * cosPsi;

	rotation(0, 1) = cosPhy * sinTheta__sinPsi - sinPhy * cosPsi;
	rotation(1, 1) = sinPhy * sinTheta__sinPsi + cosPhy * cosPsi;
	rotation(2, 1) = cosTheta * sinPsi;

	rotation(0, 2) = cosPhy * sinTheta__cosPsi + sinPhy * sinPsi;
	rotation(1, 2) = sinPhy * sinTheta__cosPsi - cosPhy * sinPsi;
	rotation(2, 2) = cosTheta * cosPsi;

	return rotation;
      }

      template< typename D1 >
      Vector3d computeEulerFromRotationMatrix ( const MatrixBase<D1> & rotation )
      {
	Vector3d euler;

	double rotationMatrix00 = rotation(0,0);
	double rotationMatrix10 = rotation(1,0);
	double rotationMatrix20 = rotation(2,0);
	double rotationMatrix01 = rotation(0,1);
	double rotationMatrix11 = rotation(1,1);
	double rotationMatrix21 = rotation(2,1);
	double rotationMatrix02 = rotation(0,2);
	double rotationMatrix12 = rotation(1,2);
	double rotationMatrix22 = rotation(2,2);

	double cosTheta = sqrt(0.5 * ( rotationMatrix00*rotationMatrix00
				       + rotationMatrix10*rotationMatrix10
				       + rotationMatrix21*rotationMatrix21
				       + rotationMatrix22*rotationMatrix22 ));
	double sinTheta = -rotationMatrix20;
	euler[1] = atan2 (sinTheta, cosTheta);

	double cosTheta_cosPhi = 0.5 * (rotationMatrix22 * rotationMatrix11
					- rotationMatrix21 * rotationMatrix12);
	double cosTheta_sinPhi = 0.5 * (rotationMatrix21 * rotationMatrix02
					- rotationMatrix22 * rotationMatrix01);
	double cosTheta_cosPsi = 0.5 * (rotationMatrix00 * rotationMatrix11
					- rotationMatrix01 * rotationMatrix10);
	double cosTheta_sinPsi = 0.5 * (rotationMatrix10 * rotationMatrix02
					- rotationMatrix00 * rotationMatrix12);

	//if cosTheta == 0
	if (fabs(cosTheta) < 1e-9 )
	  {
	    if (sinTheta > 0.5) // sinTheta ~= 1
	      {
		//phi_psi = phi - psi
		double phi_psi = atan2(- rotationMatrix10, rotationMatrix11);
		double psi = euler[2];

		double phi = phi_psi + psi;
		euler[0] = phi;
	      }
	    else  //sinTheta  ~= -1
	      {
		//phi_psi = phi + psi
		double phi_psi = atan2(- rotationMatrix10,  rotationMatrix11);

		double psi = euler[2];

		double phi = phi_psi;
		euler[0] = phi - psi;
	      }
	  }
	else
	  {
	    double cosPsi = cosTheta_cosPsi / cosTheta;
	    double sinPsi = cosTheta_sinPsi / cosTheta;
	    euler[0] = atan2 (sinPsi, cosPsi);

	    double cosPhi = cosTheta_cosPhi / cosTheta;
	    double sinPhi = cosTheta_sinPhi / cosTheta;
	    euler[2] = atan2 (sinPhi, cosPhi);
	  }

	return euler;
      }

      template< typename D1 >
      Matrix3d skew( const MatrixBase<D1> & v )
      {
	EIGEN_STATIC_ASSERT_VECTOR_ONLY( MatrixBase<D1> );
	Matrix3d mat;
	mat(0,0) =  0   ;	mat(0,1)= -v[2];	mat(0,2)=  v[1];
	mat(1,0) =  v[2];	mat(1,1)=  0   ;	mat(1,2)= -v[0];
	mat(2,0) = -v[1];	mat(2,1)=  v[0];	mat(2,2)=  0   ;
	return mat;
      }

      /*  Convert data expressed at the origin of the joint - typicaly acc and vel
       * in Djj - to data expressed at the center of the world - typicaly acc
       * and vel in Amelif - For the waist dq/ddq(1:6) is equivalent to [ v/a
       * angular | v/a linear ] expressed at the origin of the joint.
       */
      template< typename D1,typename D2, typename D3 >
      void
      djj2amelif ( Vector3d & angAmelif, Vector3d & linAmelif,
		   const MatrixBase<D1> & angDjj, const MatrixBase<D2> & linDjj,
		   const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
      {
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
	assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );

	angAmelif = angDjj;
	linAmelif = linDjj + skew(pos)* angDjj;
	sotDEBUG(20) << "cross = " << skew(pos)* angDjj << std::endl;
	sotDEBUG(20) << "cross = " << skew(pos) << std::endl;
	sotDEBUG(20) << "cross = " <<  angDjj << std::endl;
      }

      /*  Convert data expressed at the center of the world - typicaly acc and vel
       * in Amelif - to data expressed at the origin of the joint - typicaly acc
       * and vel in Djj - For the waist dq/ddq(1:6) is equivalent to [ v/a
       * angular | v/a linear ] expressed at the origin of the joint
       */
      template< typename D1,typename D2, typename D3 >
      void
      amelif2djj ( MatrixBase<D1> & angDjj, MatrixBase<D2> & linDjj,
		   const Vector3d & angAmelif, const Vector3d & linAmelif,
		   const MatrixBase<D3> & pos, const Matrix3d & /*orient*/ )
      {
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D1>);
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D2>);
	EIGEN_STATIC_ASSERT_VECTOR_ONLY(Eigen::MatrixBase<D3>);
	assert( angDjj.size() == 3 && linDjj.size() == 3 && pos.size() == 3 );

	angDjj = angAmelif;
	linDjj = linAmelif - skew(pos)* angAmelif;
      }

    }

    /* -------------------------------------------------------------------------- */
    void DynamicIntegrator::
    integrate( const ml::Vector& mlacceleration,
	       const double & dt,
	       ml::Vector & mlvelocity,
	       ml::Vector & mlposition )
    {
      using namespace DynamicIntegratorStatic;
      using soth::MATLAB;
      sotDEBUGIN(15);

      /* --- Convert acceleration, velocity and position to amelif style  ------- */
      EIGEN_VECTOR_FROM_SIGNAL( acceleration,mlacceleration );
      EIGEN_VECTOR_FROM_SIGNAL( velocity,mlvelocity );
      EIGEN_VECTOR_FROM_SIGNAL( position,mlposition );


      sotDEBUG(1) << "acceleration = " << (MATLAB)acceleration << std::endl;
      sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
      sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;

      VectorBlock< Map<VectorXd> > fftrans = position.head(3);
      VectorBlock< Map<VectorXd> > ffeuler = position.segment(3,3);
      Matrix3d ffrot = computeRotationMatrixFromEuler(ffeuler);
      sotDEBUG(15) << "Rff_start = " << (MATLAB)ffrot << std::endl;
      sotDEBUG(15) << "tff_start = " << (MATLAB)fftrans << std::endl;

      const VectorBlock< Map<VectorXd> > ffvtrans = velocity.head(3);
      const VectorBlock< Map<VectorXd> > ffvrot = velocity.segment(3,3);
      Vector3d v_lin,v_ang;
      djj2amelif( v_ang,v_lin,ffvrot,ffvtrans,fftrans,ffrot );
      sotDEBUG(15) << "vff_start = " << (MATLAB)v_lin << std::endl;
      sotDEBUG(15) << "wff_start = " << (MATLAB)v_ang << std::endl;

      const VectorBlock< Map<VectorXd> > ffatrans = acceleration.head(3);
      const VectorBlock< Map<VectorXd> > ffarot = acceleration.segment(3,3);
      Vector3d a_lin,a_ang;
      djj2amelif( a_ang,a_lin,ffarot,ffatrans,fftrans,ffrot );
      sotDEBUG(15) << "alff_start = " << (MATLAB)a_lin << std::endl;
      sotDEBUG(15) << "aaff_start = " << (MATLAB)a_ang << std::endl;

      /* --- Integrate velocity ------------------------------------------------- */
      {
	/* Acceleration, velocity and position of the FF. */
	Matrix3d finalBodyOrientation;
	Vector3d finalBodyPosition;

	double norm_v_ang = v_ang.norm();

	/* If there's no angular velocity : no rotation. */
	if (norm_v_ang < 1e-8 )
	  {
	    finalBodyPosition = v_lin*dt + fftrans;
	    finalBodyOrientation = ffrot;
	  }
	else
	  {
	    const double th  = norm_v_ang * dt;
	    double sth  =  sin(th), cth  =  1.0 - cos(th);
	    Vector3d wn  = v_ang / norm_v_ang;
	    Vector3d vol = v_lin / norm_v_ang;

	    /* drot = wnX * sin(th) + wnX * wnX * (1 - cos (th)). */
	    const Matrix3d w_wedge = skew(wn);

	    Matrix3d drot = w_wedge * cth;
	    drot += Matrix3d::Identity()*sth;
	    drot = w_wedge * drot;

	    //rot = drot + id
	    Matrix3d rot(drot);
	    rot += Matrix3d::Identity();
	    sotDEBUG(1) << "Rv = " << (MATLAB)rot << std::endl;

	    /* Update the body rotation for the body. */
	    finalBodyOrientation = rot * ffrot;

	    /* Update the body position for the body
	     * pos = rot * pos - drot * (wn ^ vol) + th* wn *T(wn) * vol */
	    finalBodyPosition = rot * fftrans;

	    // Calculation of "- drot * crossProduct(wn, vol)"
	    VectorXd tmp1 = (w_wedge*vol);
	    VectorXd tmp2 = w_wedge*tmp1;
	    VectorXd tmp3 = w_wedge*tmp2;
	    tmp2 *= sth;
	    tmp3 *= cth;
	    finalBodyPosition -= tmp2;
	    finalBodyPosition -= tmp3;

	    // Calculation of "th * wn * T(wn) * vol"
	    double w0v0 = wn[0u] * vol[0u];
	    double w1v1 = wn[1u] * vol[1u];
	    double w2v2 = wn[2u] * vol[2u];
	    w0v0 += w1v1;
	    w0v0 += w2v2;
	    w0v0 *= th;

	    // pos += wn * wovo
	    finalBodyPosition[0u] += wn[0u] * w0v0;
	    finalBodyPosition[1u] += wn[1u] * w0v0;
	    finalBodyPosition[2u] += wn[2u] * w0v0;
	  }

	sotDEBUG(1) << "Rff_end = " << (MATLAB)finalBodyOrientation << std::endl;

	/* --- Convert position --------------------------------------------------- */
	Vector3d ffeuleur_fin = computeEulerFromRotationMatrix( finalBodyOrientation );

	position.head(3) = finalBodyPosition;
	position.segment(3,3) = ffeuleur_fin;
	position.tail( position.size()-6 ) += velocity.tail( position.size()-6 ) * dt;
      }

      /* --- Integrate acceleration --------------------------------------------- */

      v_lin += a_lin*dt;
      v_ang += a_ang*dt;

      Vector3d vdjj_ang,vdjj_lin;
      amelif2djj( vdjj_ang,vdjj_lin,v_ang,v_lin,fftrans,ffrot);
      velocity.head(3) = vdjj_lin; velocity.segment(3,3) = vdjj_ang;
      //amelif2djj( ffvrot,ffvtrans,v_ang,v_lin,fftrans,ffrot);
      velocity.tail( velocity.size()-6 ) += acceleration.tail( acceleration.size()-6 )*dt;

      sotDEBUG(15) << "vff_end = " << (MATLAB)v_lin << std::endl;
      sotDEBUG(15) << "wff_end = " << (MATLAB)v_ang << std::endl;


      sotDEBUG(1) << "velocity = " << (MATLAB)velocity << std::endl;
      sotDEBUG(1) << "position = " << (MATLAB)position << std::endl;
      sotDEBUGOUT(15);
    }

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

    void DynamicIntegrator::
    display( std::ostream& os ) const
    {
      os << "DynamicIntegrator "<<getName() << ". " << std::endl;
    }

    void DynamicIntegrator::
    commandLine( const std::string& cmdLine,
		 std::istringstream& cmdArgs,
		 std::ostream& os )
    {
      if( cmdLine == "help" )
	{
	  os << "DynamicIntegrator:" << std::endl
	     << " - inc [dt]" << std::endl;
	}
      else if( cmdLine == "inc" )
	{
	  if( cmdArgs >> std::ws, cmdArgs.good() )
	    {
	      double dt; cmdArgs >> dt; dtSIN = dt;
	    }
	  integrateFromSignals();
	}
      else
	{
	  Entity::commandLine( cmdLine,cmdArgs,os );
	}
    }

  } // namespace dyninv
} // namespace sot