Commit 9505b8f3 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Rename FeatureTransformation into FeaturePose + update documentation.

parent 170b298b
......@@ -26,10 +26,21 @@ namespace dynamicgraph { namespace sot {
namespace dg = dynamicgraph;
/*!
\class FeaturePoint6d
\brief Class that defines point-6d control feature
\brief Feature that controls the relative (or absolute) pose between
two frames A (or world) and B.
Notations:
\li The frames are refered to with \c fa and \c fb.
\li Each frame is attached to a joint, which are refered to with \c ja and \c jb.
\li the difference operator is defined as \f[
\begin{array}{ccccc}
\ominus & : & SE(3)^2 & \to & \mathfrak{se}(3) \\
& & a, b & \mapsto & b \ominus a = \log(a^{-1} b) \\
\end{array}
\f]
\todo express error in R3xS03 so that the mask isn't surprising for user.
*/
class SOT_CORE_DLLAPI FeatureTransformation
class SOT_CORE_DLLAPI FeaturePose
: public FeatureAbstract
{
......@@ -41,36 +52,33 @@ class SOT_CORE_DLLAPI FeatureTransformation
/*! \name Input signals
@{
*/
/// Input transformation of <em>Joint A</em> wrt to world frame.
/// Input pose 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>.
/// Input pose 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.
/// Input pose 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>.
/// Input pose 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)
/// Jacobian of the input <em>Joint A</em>, expressed in <em>Joint A</em>
dg::SignalPtr< Matrix,int > jaJja;
/// Jacobians of the input <em>Joint B</em>.
/// \todo explicit the convention (local frame)
/// Jacobian of the input <em>Joint B</em>, expressed in <em>Joint B</em>
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;
/// The desired velocity of <em>Frame B</em> wrt to <em>Frame A</em>. The value is expressed in <em>Frame A</em>.
dg::SignalPtr< Vector, int > faNufafb;
/*! @} */
/*! \name Output signals
@{
*/
/// Transformation of <em>Frame B</em> wrt to <em>world frame</em>.
/// Pose of <em>Frame B</em> wrt to <em>world frame</em>.
/// It is expressed as a translation followed by a quaternion.
SignalTimeDependent< Vector7, int > q_oMfb;
/// Transformation of <em>Frame B*</em> wrt to <em>world frame</em>.
/// Pose of <em>Frame B*</em> wrt to <em>world frame</em>.
/// It is expressed as a translation followed by a quaternion.
SignalTimeDependent< Vector7, int > q_oMfbDes;
/*! @} */
......@@ -87,13 +95,16 @@ class SOT_CORE_DLLAPI FeatureTransformation
/*! @} */
public:
FeatureTransformation( const std::string& name );
virtual ~FeatureTransformation( void ) {}
FeaturePose( const std::string& name );
virtual ~FeaturePose( void ) {}
virtual unsigned int& getDimension( unsigned int & dim, int time );
/// Computes \f$ {}^oM_{fb} \ominus {}^oM_{fa} {}^{fa}M^*_{fb} \f$
virtual dg::Vector& computeError( dg::Vector& res,int time );
/// Computes \f$ \frac{\partial\ominus}{\partial b} {{}^{fb^*}X_{fa}} {}^{fa}\nu^*_{fafb} \f$
virtual dg::Vector& computeErrorDot( dg::Vector& res,int time );
/// Computes \f$ \frac{\partial\ominus}{\partial b} {{}^{fb}X_{jb}} {}^{jb}J_{jb} + \frac{\partial\ominus}{\partial a} {{}^{fb^*}X_{ja}} {}^{ja}J_{ja} \f$
virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
/** Static Feature selection. */
......
......@@ -44,7 +44,7 @@ SET(plugins
task/task-unilateral
feature/feature-point6d
feature/feature-transformation
feature/feature-pose
feature/feature-vector3
feature/feature-generic
feature/feature-joint-limits
......
......@@ -23,7 +23,7 @@
#include <Eigen/LU>
#include <sot/core/debug.hh>
#include <sot/core/feature-transformation.hh>
#include <sot/core/feature-pose.hh>
using namespace std;
using namespace dynamicgraph;
......@@ -36,7 +36,7 @@ using namespace dynamicgraph::sot;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> LieGroup_t;
#include <sot/core/factory.hh>
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTransformation,"FeatureTransformation");
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose,"FeaturePose");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......@@ -44,31 +44,31 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTransformation,"FeatureTransformation"
static const MatrixHomogeneous Id (MatrixHomogeneous::Identity());
FeatureTransformation::
FeatureTransformation( const string& pointName )
FeaturePose::
FeaturePose( 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")
, oMja ( NULL,"FeaturePose("+name+")::input(matrixHomo)::oMja" )
, jaMfa ( NULL,"FeaturePose("+name+")::input(matrixHomo)::jaMfa")
, oMjb ( NULL,"FeaturePose("+name+")::input(matrixHomo)::oMjb" )
, jbMfb ( NULL,"FeaturePose("+name+")::input(matrixHomo)::jbMfb")
, jaJja ( NULL,"FeaturePose("+name+")::input(matrix)::jaJja")
, jbJjb ( NULL,"FeaturePose("+name+")::input(matrix)::jbJjb")
, faMfbDes ( NULL,"FeatureTransformation("+name+")::input(matrixHomo)::faMfbDes")
, faMfbDesDot ( NULL,"FeatureTransformation("+name+")::input(vector)::faMfbDesDot")
, faMfbDes ( NULL,"FeaturePose("+name+")::input(matrixHomo)::faMfbDes")
, faNufafb ( NULL,"FeaturePose("+name+")::input(vector)::faNufafb")
, q_oMfb (boost::bind (&FeatureTransformation::computeQoMfb, this, _1, _2),
, q_oMfb (boost::bind (&FeaturePose::computeQoMfb, this, _1, _2),
oMjb << jbMfb,
"FeatureTransformation("+name+")::output(vector7)::q_oMfb")
, q_oMfbDes (boost::bind (&FeatureTransformation::computeQoMfbDes, this, _1, _2),
"FeaturePose("+name+")::output(vector7)::q_oMfb")
, q_oMfbDes (boost::bind (&FeaturePose::computeQoMfbDes, this, _1, _2),
oMja << jaMfa << faMfbDes,
"FeatureTransformation("+name+")::output(vector7)::q_oMfbDes")
"FeaturePose("+name+")::output(vector7)::q_oMfbDes")
{
oMja.setConstant (Id);
jaMfa.setConstant (Id);
jbMfb.setConstant (Id);
faMfbDes.setConstant (Id);
faMfbDesDot.setConstant (Vector::Zero(6));
faNufafb.setConstant (Vector::Zero(6));
jacobianSOUT.addDependencies(q_oMfbDes << q_oMfb
<< jaJja << jbJjb );
......@@ -76,18 +76,18 @@ FeatureTransformation( const string& pointName )
errorSOUT.addDependencies( q_oMfbDes << q_oMfb );
signalRegistration( oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb );
signalRegistration (errordotSOUT << faMfbDes << faMfbDesDot);
signalRegistration (errordotSOUT << faMfbDes << faNufafb);
errordotSOUT.setFunction (boost::bind (&FeatureTransformation::computeErrorDot,
errordotSOUT.setFunction (boost::bind (&FeaturePose::computeErrorDot,
this, _1, _2));
errordotSOUT.addDependencies (q_oMfbDes << q_oMfb << faMfbDesDot);
errordotSOUT.addDependencies (q_oMfbDes << q_oMfb << faNufafb);
// Commands
//
{
using namespace dynamicgraph::command;
addCommand("keep",
makeCommandVoid0(*this,&FeatureTransformation::servoCurrentPosition,
makeCommandVoid0(*this,&FeaturePose::servoCurrentPosition,
docCommandVoid0("modify the desired position to servo at current pos.")));
}
}
......@@ -96,17 +96,17 @@ FeatureTransformation( const string& pointName )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
static inline void check (const FeatureTransformation& ft)
static inline void check (const FeaturePose& ft)
{
assert (ft. oMja .isPlugged() );
assert (ft. jaMfa.isPlugged() );
assert (ft. oMjb .isPlugged() );
assert (ft. jbMfb.isPlugged() );
assert (ft. faMfbDes .isPlugged() );
assert (ft. faMfbDesDot.isPlugged() );
assert (ft. faNufafb.isPlugged() );
}
unsigned int& FeatureTransformation::
unsigned int& FeaturePose::
getDimension( unsigned int & dim, int time )
{
sotDEBUG(25)<<"# In {"<<endl;
......@@ -133,7 +133,7 @@ Vector7 toVector (const MatrixHomogeneous& M)
return ret;
}
Matrix& FeatureTransformation::computeJacobian( Matrix& J,int time )
Matrix& FeaturePose::computeJacobian( Matrix& J,int time )
{
check(*this);
......@@ -181,7 +181,7 @@ Matrix& FeatureTransformation::computeJacobian( Matrix& J,int time )
return J;
}
Vector7& FeatureTransformation::computeQoMfb (Vector7& res, int time)
Vector7& FeaturePose::computeQoMfb (Vector7& res, int time)
{
check(*this);
......@@ -189,7 +189,7 @@ Vector7& FeatureTransformation::computeQoMfb (Vector7& res, int time)
return res;
}
Vector7& FeatureTransformation::computeQoMfbDes (Vector7& res, int time)
Vector7& FeaturePose::computeQoMfbDes (Vector7& res, int time)
{
check(*this);
......@@ -197,7 +197,7 @@ Vector7& FeatureTransformation::computeQoMfbDes (Vector7& res, int time)
return res;
}
Vector& FeatureTransformation::computeError( Vector& error,int time )
Vector& FeaturePose::computeError( Vector& error,int time )
{
check(*this);
......@@ -215,13 +215,13 @@ Vector& FeatureTransformation::computeError( Vector& error,int time )
return error ;
}
Vector& FeatureTransformation::computeErrorDot( Vector& errordot,int time )
Vector& FeaturePose::computeErrorDot( Vector& errordot,int time )
{
check(*this);
errordot.resize(dimensionSOUT(time));
const Flags &fl = selectionSIN(time);
if (!faMfbDesDot.isPlugged()) {
if (!faNufafb.isPlugged()) {
errordot.setZero();
return errordot;
}
......@@ -241,7 +241,7 @@ Vector& FeatureTransformation::computeErrorDot( Vector& errordot,int time )
unsigned int cursor = 0;
for( unsigned int i=0;i<6;++i )
if( fl(i) )
errordot(cursor++) = Jminus.row(i) * faMfbDesDot.accessCopy();
errordot(cursor++) = Jminus.row(i) * faNufafb.accessCopy();
return errordot;
}
......@@ -249,7 +249,7 @@ Vector& FeatureTransformation::computeErrorDot( Vector& errordot,int time )
/* 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::
void FeaturePose::
servoCurrentPosition( void )
{
check(*this);
......@@ -268,7 +268,7 @@ static const char * featureNames []
"RX",
"RY",
"RZ" };
void FeatureTransformation::
void FeaturePose::
display( std::ostream& os ) const
{
os <<"Point6d <"<<name<<">: (" ;
......
......@@ -50,7 +50,7 @@ SET(TEST_test_feature_point6d_LIBS
)
SET(TEST_test_feature_generic_LIBS
gain-adaptive feature-generic task feature-transformation
gain-adaptive feature-generic task feature-pose
)
SET(TEST_test_mailbox_LIBS
......
......@@ -27,7 +27,7 @@
#include <sot/core/sot.hh>
#include <sot/core/feature-generic.hh>
#include <sot/core/feature-transformation.hh>
#include <sot/core/feature-pose.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/debug.hh>
#include <sot/core/task.hh>
......@@ -298,17 +298,17 @@ Vector toVector (const std::vector<MultiBound>& in)
return out;
}
class TestFeatureTransformation : public FeatureTestBase
class TestFeaturePose : public FeatureTestBase
{
public:
FeatureTransformation feature_;
FeaturePose feature_;
bool relative_;
pinocchio::Model model_;
pinocchio::Data data_;
pinocchio::JointIndex ja_, jb_;
pinocchio::FrameIndex fa_, fb_;
TestFeatureTransformation (bool relative, const std::string &name):
TestFeaturePose (bool relative, const std::string &name):
FeatureTestBase (6, name),
feature_("feature"+name),
relative_ (relative),
......@@ -400,7 +400,7 @@ class TestFeatureTransformation : public FeatureTestBase
// Desired
setSignal (feature_.faMfbDes, randomM());
setSignal (feature_.faMfbDesDot, Vector::Random(6));
setSignal (feature_.faNufafb, Vector::Random(6));
double gain = 0;
//if (time_ % 5 != 0)
......@@ -429,7 +429,7 @@ class TestFeatureTransformation : public FeatureTestBase
computeExpectedTaskOutput (
toVector(oMfb),
toVector(oMfa * faMfbDes),
faMfbDes.toActionMatrixInverse() * feature_.faMfbDesDot.accessCopy(),
faMfbDes.toActionMatrixInverse() * feature_.faNufafb.accessCopy(),
LieGroup_t());
checkTaskOutput();
......@@ -455,7 +455,7 @@ class TestFeatureTransformation : public FeatureTestBase
// compute e = task_.taskSOUT and J = task_.jacobianSOUT
// check that e (q + eps*qdot) - e (q) ~= eps * J * qdot
// compute qdot = J^+ * e
// check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faMfbDesDot
// check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faNufafb
time_++;
Vector q (pinocchio::randomConfiguration (model_));
......@@ -498,19 +498,19 @@ class TestFeatureTransformation : public FeatureTestBase
J = task_.jacobianSOUT.accessCopy();
Eigen::JacobiSVD<Matrix> svd (J, Eigen::ComputeThinU | Eigen::ComputeThinV);
Vector faMfbDesDot (Vector::Zero(6));
Vector faNufafb (Vector::Zero(6));
double eps = 1e-6;
for (int i = 0; i < 6; ++i)
{
time_++;
faMfbDesDot(i) = eps;
setSignal (feature_.faMfbDesDot, faMfbDesDot);
faNufafb(i) = eps;
setSignal (feature_.faNufafb, faNufafb);
task_.taskSOUT.recompute(time_);
Vector qdot = svd.solve(toVector (task_.taskSOUT.accessCopy()));
Vector faVfafb = faJfafb * qdot;
EIGEN_VECTOR_IS_APPROX(faMfbDesDot, faVfafb, eps);
EIGEN_VECTOR_IS_APPROX(faNufafb, faVfafb, eps);
// Check with finite difference.
Vector q_qdot = pinocchio::integrate (model_, q, qdot);
......@@ -521,25 +521,38 @@ class TestFeatureTransformation : public FeatureTestBase
Vector diff (6);
LieGroup_t().difference (q_faMfb, q_faMfb_next, diff);
Vector faVfafb_fd = faMfb.toActionMatrix() * diff;
EIGEN_VECTOR_IS_APPROX(faMfbDesDot, faVfafb_fd, 1e-5);
EIGEN_VECTOR_IS_APPROX(faNufafb, faVfafb_fd, 1e-5);
faMfbDesDot(i) = 0.;
faNufafb(i) = 0.;
}
time_++;
}
};
BOOST_AUTO_TEST_CASE (feature_transformation_absolute)
BOOST_AUTO_TEST_CASE (feature_pose_absolute)
{
TestFeatureTransformation testAbsolute(false,"abs");
TestFeaturePose testAbsolute(false,"abs");
for (int i = 0; i < 10; ++i)
testAbsolute.runTest();
testAbsolute.checkFeedForward();
testAbsolute.setRandomFrame();
for (int i = 0; i < 10; ++i)
testAbsolute.runTest();
testAbsolute.checkFeedForward();
}
BOOST_AUTO_TEST_CASE (feature_transformation_relative)
BOOST_AUTO_TEST_CASE (feature_pose_relative)
{
TestFeatureTransformation testRelative(true ,"rel");
TestFeaturePose testRelative(true ,"rel");
for (int i = 0; i < 10; ++i)
testRelative.runTest();
testRelative.checkFeedForward();
testRelative.setRandomFrame();
for (int i = 0; i < 10; ++i)
testRelative.runTest();
testRelative.checkFeedForward();
......
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