Add FeatureTransformation with unit-test

include/sot/core/feature-transformation.hh

+/*
+ * Copyright 2019,
+ * Joseph Mirabel
+ *
+ * LAAS-CNRS
+ *
+ */
+
+#ifndef __SOT_FEATURE_TRANSFORMATION_HH__
+#define __SOT_FEATURE_TRANSFORMATION_HH__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* SOT */
+#include <sot/core/config.hh>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/matrix-geometry.hh>
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+namespace dynamicgraph { namespace sot {
+namespace dg = dynamicgraph;
+
+/*!
+  \class FeaturePoint6d
+  \brief Class that defines point-6d control feature
+*/
+class SOT_CORE_DLLAPI FeatureTransformation
+  : public FeatureAbstract
+{
+
+ public:
+  static const std::string CLASS_NAME;
+  virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+
+ public:
+  /*! \name Input signals
+    @{
+  */
+  /// Input transformation of <em>Joint A</em> wrt to world frame.
+  dg::SignalPtr< MatrixHomogeneous, int > oMja;
+  /// Input transformation of <em>Frame A</em> wrt to <em>Joint A</em>.
+  dg::SignalPtr< MatrixHomogeneous, int > jaMfa;
+  /// Input transformation of <em>Joint B</em> wrt to world frame.
+  dg::SignalPtr< MatrixHomogeneous, int > oMjb;
+  /// Input transformation of <em>Frame B</em> wrt to <em>Joint B</em>.
+  dg::SignalPtr< MatrixHomogeneous, int > jbMfb;
+  /// Jacobians of the input <em>Joint A</em>.
+  /// \todo explicit the convention (local frame)
+  dg::SignalPtr< Matrix,int > jaJja;
+  /// Jacobians of the input <em>Joint B</em>.
+  /// \todo explicit the convention (local frame)
+  dg::SignalPtr< Matrix,int > jbJjb;
+
+  /// The desired pose of <em>Frame B</em> wrt to <em>Frame A</em>.
+  dg::SignalPtr< MatrixHomogeneous, int > faMfbDes;
+  /// The desired velocity of <em>Frame B</em> wrt to <em>Frame A</em>.
+  /// \todo explicit in which frame this velocity is expressed.
+  dg::SignalPtr< Vector, int > faMfbDesDot;
+  /*! @} */
+
+  /*! \name Output signals
+    @{
+  */
+  /// Transformation of <em>Frame B</em> wrt to <em>Frame A</em>.
+  /// It is expressed as a translation followed by a quaternion.
+  SignalTimeDependent< Vector7, int > faMfb;
+
+  /// The Vector7 version of faMfbDes, for internal purposes.
+  SignalTimeDependent< Vector7, int > faMfbDes_q;
+  /*! @} */
+
+  using FeatureAbstract::selectionSIN;
+  // TODO Rename into dError_dq or Jerror
+  using FeatureAbstract::jacobianSOUT;
+  // TODO Rename into error
+  using FeatureAbstract::errorSOUT;
+
+  /*! \name Dealing with the reference value to be reach with this feature.
+    @{  */
+  DECLARE_NO_REFERENCE();
+  /*! @} */
+
+ public:
+  FeatureTransformation( const std::string& name );
+  virtual ~FeatureTransformation( void ) {}
+
+  virtual unsigned int& getDimension( unsigned int & dim, int time );
+
+  virtual dg::Vector& computeError( dg::Vector& res,int time );
+  virtual dg::Vector& computeErrorDot( dg::Vector& res,int time );
+  virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+
+  /** Static Feature selection. */
+  inline static Flags selectX( void )  { return FLAG_LINE_1; }
+  inline static Flags selectY( void )  { return FLAG_LINE_2; }
+  inline static Flags selectZ( void )  { return FLAG_LINE_3; }
+  inline static Flags selectRX( void ) { return FLAG_LINE_4; }
+  inline static Flags selectRY( void ) { return FLAG_LINE_5; }
+  inline static Flags selectRZ( void ) { return FLAG_LINE_6; }
+
+  inline static Flags selectTranslation( void ) { return Flags(7); }
+  inline static Flags selectRotation( void ) { return Flags(56); }
+
+  virtual void display( std::ostream& os ) const;
+
+ public:
+  void servoCurrentPosition( void );
+ private:
+  Vector7& computefaMfb (Vector7& res, int time);
+  Vector7& computefaMfbDes_q (Vector7& res, int time);
+
+  /// \todo Intermediate variables for internal computations
+} ;
+
+} /* namespace sot */} /* namespace dynamicgraph */
+
+#endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__
+
+/*
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */

include/sot/core/matrix-geometry.hh

@@ -80,6 +80,10 @@ namespace dynamicgraph {
     typedef Eigen::Matrix<double,6,6> SOT_CORE_EXPORT MatrixForce;
     typedef Eigen::Matrix<double,6,6> SOT_CORE_EXPORT MatrixTwist;
 
+    typedef Eigen::Matrix<double,7,1> SOT_CORE_EXPORT Vector7;
+    typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
+    typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
+
     inline void buildFrom (const MatrixHomogeneous& MH, MatrixTwist& MT) {
 
       Eigen::Vector3d _t = MH.translation();

src/CMakeLists.txt

@@ -44,6 +44,7 @@ SET(plugins
   task/task-unilateral
 
   feature/feature-point6d
+  feature/feature-transformation
   feature/feature-vector3
   feature/feature-generic
   feature/feature-joint-limits

src/feature/feature-transformation.cpp

+/*
+ * Copyright 2019
+ * Joseph Mirabel
+ *
+ * LAAS-CNRS
+ *
+ */
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* --- SOT --- */
+//#define VP_DEBUG
+//#define VP_DEBUG_MODE 45
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-bind.h>
+
+#include <pinocchio/multibody/liegroup/liegroup.hpp>
+
+#include <Eigen/LU>
+
+#include <sot/core/debug.hh>
+#include <sot/core/feature-transformation.hh>
+
+using namespace std;
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+//typedef pinocchio::CartesianProductOperation <
+//        pinocchio::VectorSpaceOperationTpl<3, double>,
+//        pinocchio::SpecialOrthogonalOperationTpl<3, double>
+//        > LieGroup_t;
+typedef pinocchio::SpecialEuclideanOperationTpl<3, double> LieGroup_t;
+
+#include <sot/core/factory.hh>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTransformation,"FeatureTransformation");
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+FeatureTransformation::
+FeatureTransformation( const string& pointName )
+  : FeatureAbstract( pointName )
+    , oMja  ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::oMja" )
+    , jaMfa ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::jaMfa")
+    , oMjb  ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::oMjb" )
+    , jbMfb ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::jbMfb")
+    , jaJja ( NULL,"FeatureTransformation("+name+")::input(matrix)::jaJja")
+    , jbJjb ( NULL,"FeatureTransformation("+name+")::input(matrix)::jbJjb")
+
+    , faMfbDes ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::faMfbDes")
+    , faMfbDesDot ( NULL,"FeatureTransformation("+name+")::input(vector)::faMfbDesDot")
+
+    , faMfb (boost::bind (&FeatureTransformation::computefaMfb, this, _1, _2),
+        oMja << jaMfa << oMjb << jbMfb,
+        "FeatureTransformation("+name+")::output(vector7)::faMfbDesDot")
+    , faMfbDes_q (boost::bind (&FeatureTransformation::computefaMfbDes_q, this, _1, _2),
+        faMfbDes,
+        "FeatureTransformation("+name+")::output(vector7)::faMfbDes_q")
+{
+  jacobianSOUT.addDependencies( faMfb << faMfbDes_q
+      << jaJja << jbJjb );
+
+  errorSOUT.addDependencies( faMfb << faMfbDes_q );
+
+  signalRegistration( oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb );
+  signalRegistration (errordotSOUT << faMfbDes << faMfbDesDot);
+
+  errordotSOUT.setFunction (boost::bind (&FeatureTransformation::computeErrorDot,
+					 this, _1, _2));
+  errordotSOUT.addDependencies (faMfbDesDot << faMfb << faMfbDes_q);
+
+  // Commands
+  //
+  {
+    using namespace dynamicgraph::command;
+    addCommand("keep",
+	       makeCommandVoid0(*this,&FeatureTransformation::servoCurrentPosition,
+				docCommandVoid0("modify the desired position to servo at current pos.")));
+  }
+}
+
+/* TODO Add this dependency in constructor.
+void FeaturePoint6d::
+addDependenciesFromReference( void )
+{
+  assert( isReferenceSet() );
+  errorSOUT.addDependency( getReference()->positionSIN );
+  jacobianSOUT.addDependency( getReference()->positionSIN );
+}
+
+void FeaturePoint6d::
+removeDependenciesFromReference( void )
+{
+  assert( isReferenceSet() );
+  errorSOUT.removeDependency( getReference()->positionSIN );
+  jacobianSOUT.removeDependency( getReference()->positionSIN );
+}
+*/
+
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+unsigned int& FeatureTransformation::
+getDimension( unsigned int & dim, int time )
+{
+  sotDEBUG(25)<<"# In {"<<endl;
+
+  const Flags &fl = selectionSIN.access(time);
+
+  dim = 0;
+  for( int i=0;i<6;++i ) if( fl(i) ) dim++;
+
+  sotDEBUG(25)<<"# Out }"<<endl;
+  return dim;
+}
+
+static const MatrixHomogeneous Id (MatrixHomogeneous::Identity());
+
+void toVector (const MatrixHomogeneous& M, Vector7& v)
+{
+  v.head<3>() = M.translation();
+  QuaternionMap(v.tail<4>().data()) = M.linear();
+}
+
+Vector7 toVector (const MatrixHomogeneous& M)
+{
+  Vector7 ret;
+  toVector (M, ret);
+  return ret;
+}
+
+void fromVector (const Vector7& v, MatrixHomogeneous& M)
+{
+  M.translation() = v.head<3>();
+  M.linear() = Eigen::Map<const Quaternion>(v.tail<4>().data()).toRotationMatrix();
+}
+
+Matrix& FeatureTransformation::computeJacobian( Matrix& J,int time )
+{
+  const int & dim = dimensionSOUT(time);
+  const Flags &fl = selectionSIN(time);
+
+  const Matrix & _jbJjb = jbJjb (time);
+  //const Vector7& _faMfb    = faMfb      (time),
+                 //_faMfbDes = faMfbDes_q (time);
+
+  const MatrixHomogeneous& _oMja  = (oMja .isPlugged() ? oMja (time) : Id),
+                           _jaMfa = (jaMfa.isPlugged() ? jaMfa(time) : Id),
+                           _oMjb  =                      oMjb (time),
+                           _jbMfb = (jbMfb.isPlugged() ? jbMfb(time) : Id),
+                           _faMfbDes = (faMfbDes.isPlugged() ? faMfbDes(time) : Id);
+
+  const Matrix::Index cJ = _jbJjb.cols();
+  J.resize(dim,cJ) ;
+  Matrix tmp (6,cJ);
+
+  MatrixTwist X;
+  Eigen::Matrix<double,6,6,Eigen::RowMajor> Jminus;
+
+  buildFrom (_jbMfb.inverse(Eigen::Affine), X);
+  LieGroup_t().dDifference<pinocchio::ARG1>(
+      toVector(_oMja * _jaMfa * _faMfbDes),
+      toVector(_oMjb * _jbMfb),
+      Jminus);
+  
+  tmp.noalias() = (Jminus * X) * _jbJjb;
+
+  if (jaJja.isPlugged()) {
+    LieGroup_t().dDifference<pinocchio::ARG0>(
+      toVector(_oMja * _jaMfa * _faMfbDes),
+      toVector(_oMjb * _jbMfb),
+      Jminus);
+    buildFrom ((_jaMfa *_faMfbDes).inverse(Eigen::Affine), X);
+
+    tmp.noalias() += (Jminus * X) * jaJja(time);
+  }
+
+  /* Select the active line of Jq. */
+  unsigned int rJ = 0;
+  for( unsigned int r=0;r<6;++r )
+    if( fl(r) )
+      J.row(rJ++) = tmp.row(r);
+
+  return J;
+}
+
+Vector7& FeatureTransformation::computefaMfb (Vector7& res, int time)
+{
+  const MatrixHomogeneous& _oMja  = (oMja .isPlugged() ? oMja (time) : Id),
+                           _jaMfa = (jaMfa.isPlugged() ? jaMfa(time) : Id),
+                           _oMjb  =                      oMjb (time),
+                           _jbMfb = (jbMfb.isPlugged() ? jbMfb(time) : Id);
+
+  MatrixHomogeneous _faMfb = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
+
+  toVector (_faMfb, res);
+  return res;
+}
+
+Vector7& FeatureTransformation::computefaMfbDes_q (Vector7& res, int time)
+{
+  if (faMfbDes.isPlugged()) {
+    const MatrixHomogeneous& _faMfbDes = faMfbDes(time);
+    toVector (_faMfbDes, res);
+  } else {
+    res.head<6>().setZero();
+    res[6] = 1.;
+  }
+  return res;
+}
+
+Vector& FeatureTransformation::computeError( Vector& error,int time )
+{
+  /*
+  const Vector7& _faMfb    = faMfb      (time),
+                 _faMfbDes = faMfbDes_q (time);
+
+  const Flags &fl = selectionSIN(time);
+
+  Eigen::Matrix<double,6,1> v;
+  LieGroup_t().difference (_faMfbDes, _faMfb, v); // _faMfb - _faMfbDes
+  */
+  const MatrixHomogeneous& _oMja  = (oMja .isPlugged() ? oMja (time) : Id),
+                           _jaMfa = (jaMfa.isPlugged() ? jaMfa(time) : Id),
+                           _oMjb  =                      oMjb (time),
+                           _jbMfb = (jbMfb.isPlugged() ? jbMfb(time) : Id),
+                           _faMfbDes = (faMfbDes.isPlugged() ? faMfbDes(time) : Id);
+
+  const Flags &fl = selectionSIN(time);
+
+  Eigen::Matrix<double,6,1> v;
+  LieGroup_t().difference (
+      toVector(_oMja * _jaMfa * _faMfbDes),
+      toVector(_oMjb * _jbMfb),
+      v);
+
+  error.resize(dimensionSOUT(time)) ;
+  unsigned int cursor = 0;
+  for( unsigned int i=0;i<6;++i )
+    if( fl(i) )
+      error(cursor++) = v(i);
+
+  return error ;
+}
+
+Vector& FeatureTransformation::computeErrorDot( Vector& errordot,int time )
+{
+  errordot.resize(dimensionSOUT(time));
+  const Flags &fl = selectionSIN(time);
+  if (!faMfbDesDot.isPlugged()) {
+    errordot.setZero();
+    return errordot;
+  }
+
+  const Vector& _faMfbDesDot = faMfbDesDot(time);
+  const MatrixHomogeneous& _oMja  = (oMja .isPlugged() ? oMja (time) : Id),
+                           _jaMfa = (jaMfa.isPlugged() ? jaMfa(time) : Id),
+                           _oMjb  =                      oMjb (time),
+                           _jbMfb = (jbMfb.isPlugged() ? jbMfb(time) : Id),
+                           _faMfbDes = (faMfbDes.isPlugged() ? faMfbDes(time) : Id);
+
+  Eigen::Matrix<double,6,6,Eigen::RowMajor> Jminus;
+
+  LieGroup_t().dDifference<pinocchio::ARG0>(
+      toVector(_oMja * _jaMfa * _faMfbDes),
+      toVector(_oMjb * _jbMfb),
+      Jminus);
+  // Assume _faMfbDesDot is expressed in fa
+  Jminus = Jminus * pinocchio::SE3(_faMfbDes.rotation(), _faMfbDes.translation()).toActionMatrixInverse();
+  // Assume _faMfbDesDot is expressed in fb*
+  // Jminus = Jminus
+  unsigned int cursor = 0;
+  for( unsigned int i=0;i<6;++i )
+    if( fl(i) )
+      errordot(cursor++) = Jminus.row(i) * _faMfbDesDot;
+
+  return errordot;
+}
+
+/* Modify the value of the reference (sdes) so that it corresponds
+ * to the current position. The effect on the servo is to maintain the
+ * current position and correct any drift. */
+void FeatureTransformation::
+servoCurrentPosition( void )
+{
+  MatrixHomogeneous M;
+  fromVector (faMfb.accessCopy(), M);
+  faMfbDes = M;
+}
+
+static const char * featureNames  []
+= { "X ",
+    "Y ",
+    "Z ",
+    "RX",
+    "RY",
+    "RZ"  };
+void FeatureTransformation::
+display( std::ostream& os ) const
+{
+  os <<"Point6d <"<<name<<">: (" ;
+
+  try{
+    const Flags &fl = selectionSIN.accessCopy();
+    bool first = true;
+    for( int i=0;i<6;++i )
+      if( fl(i) )
+	{
+	  if( first ) { first = false; } else { os << ","; }
+	  os << featureNames[i];
+	}
+    os<<") ";
+  }  catch(ExceptionAbstract e){ os<< " selectSIN not set."; }
+}

unitTesting/CMakeLists.txt

@@ -50,7 +50,7 @@ SET(TEST_test_feature_point6d_LIBS
   )
 
 SET(TEST_test_feature_generic_LIBS
-  gain-adaptive feature-generic task
+  gain-adaptive feature-generic task feature-transformation
   )
 
 SET(TEST_test_mailbox_LIBS