Commit df9a3490 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add FeatureTransformation with unit-test

parent 0564b2fc
/*
* 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:
*/
......@@ -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();
......
......@@ -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
......
/*
* 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."; }
}
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment