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.hh>
#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 dynamicgraph
{
  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_CONST_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<SigVectorXd> fftrans = position.head(3);
	VectorBlock<SigVectorXd> 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;

	VectorBlock<SigVectorXd> ffvtrans = velocity.head(3);
	VectorBlock<SigVectorXd> 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<const_SigVectorXd> ffatrans = acceleration.head(3);
	const VectorBlock<const_SigVectorXd> 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
} // namespace dynamicgraph