Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/feature-pose.hxx	Lines:	130	158	82.3 %
Date:	2023-03-13 12:09:37	Branches:	224	488	45.9 %


/*
 * Copyright 2019
 * Joseph Mirabel
 *
 * LAAS-CNRS
 *
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* --- SOT --- */
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command.h>

#include <Eigen/LU>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
#include <sot/core/feature-pose.hh>

namespace dynamicgraph {
namespace sot {

typedef pinocchio::CartesianProductOperation<
    pinocchio::VectorSpaceOperationTpl<3, double>,
    pinocchio::SpecialOrthogonalOperationTpl<3, double> >
    R3xSO3_t;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;

namespace internal {
template <Representation_t representation>
struct LG_t {
  typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
                                    R3xSO3_t>::type type;
};
}  // namespace internal

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());

template <Representation_t representation>
FeaturePose<representation>::FeaturePose(const std::string &pointName)
    : FeatureAbstract(pointName),
      oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
      jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
      oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
      jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
      jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
      jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"),
      faMfbDes(NULL,
               CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
      faNufafbDes(NULL,
                  CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"),
      faMfb(
          boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
          oMja << jaMfa << oMjb << jbMfb,
          CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"),
      q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
                          _2),
              faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
      q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
                             this, _1, _2),
                 faMfbDes,
                 CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
  oMja.setConstant(Id);
  jaMfa.setConstant(Id);
  jbMfb.setConstant(Id);
  faMfbDes.setConstant(Id);
  faNufafbDes.setConstant(Vector::Zero(6));

  jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);

  errorSOUT.addDependencies(q_faMfbDes << q_faMfb);

  signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
  signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes);

  errordotSOUT.setFunction(
      boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
  errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);

  // Commands
  //
  {
    using namespace dynamicgraph::command;
    addCommand("keep",
               makeCommandVoid1(
                   *this, &FeaturePose<representation>::servoCurrentPosition,
                   docCommandVoid1(
                       "modify the desired position to servo at current pos.",
                       "time")));
  }
}

template <Representation_t representation>
FeaturePose<representation>::~FeaturePose() {}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

template <Representation_t representation>
static inline void check(const FeaturePose<representation> &ft) {
  (void)ft;
  assert(ft.oMja.isPlugged());
  assert(ft.jaMfa.isPlugged());
  assert(ft.oMjb.isPlugged());
  assert(ft.jbMfb.isPlugged());
  assert(ft.faMfbDes.isPlugged());
  assert(ft.faNufafbDes.isPlugged());
}

template <Representation_t representation>
unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
                                                        int time) {
  sotDEBUG(25) << "# In {" << std::endl;

  const Flags &fl = selectionSIN.access(time);

  dim = 0;
  for (int i = 0; i < 6; ++i)
    if (fl(i)) dim++;

  sotDEBUG(25) << "# Out }" << std::endl;
  return dim;
}

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;
}

template <Representation_t representation>
Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
  typedef typename internal::LG_t<representation>::type LieGroup_t;

  check(*this);

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);

  const unsigned int &dim = dimensionSOUT(time);
  const Flags &fl = selectionSIN(time);

  const Matrix &_jbJjb = jbJjb(time);

  const MatrixHomogeneous &_jbMfb =
      (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);

  const Matrix::Index cJ = _jbJjb.cols();
  J.resize(dim, cJ);

  MatrixTwist X;
  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  buildFrom(_jbMfb.inverse(Eigen::Affine), X);
  MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
                         oMja.access(time).rotation().transpose() *
                         oMjb.access(time).rotation() * _jbMfb.rotation();
  if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
    X.topRows<3>().applyOnTheLeft(faRfb);
  LieGroup_t().template dDifference<pinocchio::ARG1>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);

  // Contribution of b:
  // J = Jminus * X * jbJjb;
  unsigned int rJ = 0;
  for (unsigned int r = 0; r < 6; ++r)
    if (fl((int)r)) J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;

  if (jaJja.isPlugged()) {
    const Matrix &_jaJja = jaJja(time);
    const MatrixHomogeneous &_jaMfa =
                                (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
                            _faMfb = faMfb.accessCopy();

    buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
    if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
      X.topRows<3>().applyOnTheLeft(faRfb);

    // J -= (Jminus * X) * jaJja(time);
    rJ = 0;
    for (unsigned int r = 0; r < 6; ++r)
      if (fl((int)r)) J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
  }

  return J;
}

template <Representation_t representation>
MatrixHomogeneous &FeaturePose<representation>::computefaMfb(
    MatrixHomogeneous &res, int time) {
  check(*this);

  res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
        jbMfb(time);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
  check(*this);

  toVector(faMfb(time), res);
  return res;
}

template <Representation_t representation>
Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
  check(*this);

  toVector(faMfbDes(time), res);
  return res;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
  typedef typename internal::LG_t<representation>::type LieGroup_t;
  check(*this);

  const Flags &fl = selectionSIN(time);

  Eigen::Matrix<double, 6, 1> v;
  LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);

  error.resize(dimensionSOUT(time));
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl((int)i)) error(cursor++) = v(i);

  return error;
}

// This function is responsible of converting the input velocity expressed with
// SE(3) convention onto a velocity expressed with the convention of this
// feature (R^3xSO(3) or SE(3)), in the right frame.
template <>
Vector6d convertVelocity<SE3_t>(const MatrixHomogeneous &M,
                                const MatrixHomogeneous &Mdes,
                                const Vector &faNufafbDes) {
  (void)M;
  MatrixTwist X;
  buildFrom(Mdes.inverse(Eigen::Affine), X);
  return X * faNufafbDes;
}
template <>
Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
                                   const MatrixHomogeneous &Mdes,
                                   const Vector &faNufafbDes) {
  Vector6d nu;
  nu.head<3>() =
      faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
  nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
  return nu;
}

template <Representation_t representation>
Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
                                                     int time) {
  typedef typename internal::LG_t<representation>::type LieGroup_t;
  check(*this);

  errordot.resize(dimensionSOUT(time));
  const Flags &fl = selectionSIN(time);
  if (!faNufafbDes.isPlugged()) {
    errordot.setZero();
    return errordot;
  }

  q_faMfb.recompute(time);
  q_faMfbDes.recompute(time);
  faNufafbDes.recompute(time);

  const MatrixHomogeneous &_faMfbDes = faMfbDes(time);

  Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;

  LieGroup_t().template dDifference<pinocchio::ARG0>(
      q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
  Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
                                            faNufafbDes.accessCopy());
  unsigned int cursor = 0;
  for (unsigned int i = 0; i < 6; ++i)
    if (fl((int)i)) errordot(cursor++) = Jminus.row(i) * nu;

  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. */
template <Representation_t representation>
void FeaturePose<representation>::servoCurrentPosition(const int &time) {
  check(*this);

  const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
                          _jaMfa =
                              (jaMfa.isPlugged() ? jaMfa.access(time) : Id),
                          _oMjb = oMjb.access(time),
                          _jbMfb =
                              (jbMfb.isPlugged() ? jbMfb.access(time) : Id);
  faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}

static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
template <Representation_t representation>
void FeaturePose<representation>::display(std::ostream &os) const {
  os << CLASS_NAME << "<" << 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.";
  }
}

}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2019
3			* Joseph Mirabel
4			*
5			* LAAS-CNRS
6			*
7			*/
8
9			/* --------------------------------------------------------------------- */
10			/* --- INCLUDE --------------------------------------------------------- */
11			/* --------------------------------------------------------------------- */
12
13			/* --- SOT --- */
14			#include <dynamic-graph/command-bind.h>
15			#include <dynamic-graph/command-getter.h>
16			#include <dynamic-graph/command-setter.h>
17			#include <dynamic-graph/command.h>
18
19			#include <Eigen/LU>
20			#include <boost/mpl/if.hpp>
21			#include <boost/type_traits/is_same.hpp>
22			#include <pinocchio/multibody/liegroup/liegroup.hpp>
23			#include <sot/core/debug.hh>
24			#include <sot/core/factory.hh>
25			#include <sot/core/feature-pose.hh>
26
27			namespace dynamicgraph {
28			namespace sot {
29
30			typedef pinocchio::CartesianProductOperation<
31			pinocchio::VectorSpaceOperationTpl<3, double>,
32			pinocchio::SpecialOrthogonalOperationTpl<3, double> >
33			R3xSO3_t;
34			typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
35
36			namespace internal {
37			template <Representation_t representation>
38			struct LG_t {
39			typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
40			R3xSO3_t>::type type;
41			};
42			} // namespace internal
43
44			/* --------------------------------------------------------------------- */
45			/* --- CLASS ----------------------------------------------------------- */
46			/* --------------------------------------------------------------------- */
47
48			static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());
49
50			template <Representation_t representation>
51		8	FeaturePose<representation>::FeaturePose(const std::string &pointName)
52			: FeatureAbstract(pointName),
53		8	oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
54		8	jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
55		8	oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
56		8	jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
57		8	jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
58		8	jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"),
59			faMfbDes(NULL,
60		8	CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
61			faNufafbDes(NULL,
62		8	CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"),
63			faMfb(
64			boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
65	✓✗✓✗ ✓✗	8	oMja << jaMfa << oMjb << jbMfb,
66		8	CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"),
67			q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
68			_2),
69		8	faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
70			q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
71			this, _1, _2),
72			faMfbDes,
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	16	CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
74	✓✗	8	oMja.setConstant(Id);
75	✓✗	8	jaMfa.setConstant(Id);
76	✓✗	8	jbMfb.setConstant(Id);
77	✓✗	8	faMfbDes.setConstant(Id);
78	✓✗✓✗ ✓✗	8	faNufafbDes.setConstant(Vector::Zero(6));
79
80	✓✗✓✗ ✓✗✓✗	8	jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);
81
82	✓✗✓✗	8	errorSOUT.addDependencies(q_faMfbDes << q_faMfb);
83
84	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	8	signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
85	✓✗✓✗ ✓✗✓✗	8	signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes);
86
87	✓✗✓✗ ✓✗	8	errordotSOUT.setFunction(
88			boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
89	✓✗✓✗ ✓✗	8	errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);
90
91			// Commands
92			//
93			{
94			using namespace dynamicgraph::command;
95	✓✗✓✗	8	addCommand("keep",
96	✓✗✓✗ ✓✗✓✗	16	makeCommandVoid1(
97			*this, &FeaturePose<representation>::servoCurrentPosition,
98			docCommandVoid1(
99			"modify the desired position to servo at current pos.",
100			"time")));
101			}
102		8	}
103
104			template <Representation_t representation>
105		8	FeaturePose<representation>::~FeaturePose() {}
106
107			/* --------------------------------------------------------------------- */
108			/* --------------------------------------------------------------------- */
109			/* --------------------------------------------------------------------- */
110
111			template <Representation_t representation>
112		3936	static inline void check(const FeaturePose<representation> &ft) {
113			(void)ft;
114	✗✓	3936	assert(ft.oMja.isPlugged());
115	✗✓	3936	assert(ft.jaMfa.isPlugged());
116	✗✓	3936	assert(ft.oMjb.isPlugged());
117	✗✓	3936	assert(ft.jbMfb.isPlugged());
118	✗✓	3936	assert(ft.faMfbDes.isPlugged());
119	✗✓	3936	assert(ft.faNufafbDes.isPlugged());
120		3936	}
121
122			template <Representation_t representation>
123		8	unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
124			int time) {
125			sotDEBUG(25) << "# In {" << std::endl;
126
127		8	const Flags &fl = selectionSIN.access(time);
128
129		8	dim = 0;
130	✓✓	56	for (int i = 0; i < 6; ++i)
131	✓✗✓✗	48	if (fl(i)) dim++;
132
133			sotDEBUG(25) << "# Out }" << std::endl;
134		8	return dim;
135			}
136
137		640	void toVector(const MatrixHomogeneous &M, Vector7 &v) {
138	✓✗✓✗	640	v.head<3>() = M.translation();
139	✓✗✓✗ ✓✗	640	QuaternionMap(v.tail<4>().data()) = M.linear();
140		640	}
141
142			Vector7 toVector(const MatrixHomogeneous &M) {
143			Vector7 ret;
144			toVector(M, ret);
145			return ret;
146			}
147
148			template <Representation_t representation>
149		64	Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
150			typedef typename internal::LG_t<representation>::type LieGroup_t;
151
152	✓✗	64	check(*this);
153
154	✓✗	64	q_faMfb.recompute(time);
155	✓✗	64	q_faMfbDes.recompute(time);
156
157	✓✗	64	const unsigned int &dim = dimensionSOUT(time);
158	✓✗	64	const Flags &fl = selectionSIN(time);
159
160	✓✗	64	const Matrix &_jbJjb = jbJjb(time);
161
162	✓✗	64	const MatrixHomogeneous &_jbMfb =
163	✓✗	128	(jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
164
165	✓✗	64	const Matrix::Index cJ = _jbJjb.cols();
166	✓✗	64	J.resize(dim, cJ);
167
168	✓✗	64	MatrixTwist X;
169	✓✗	64	Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
170
171	✓✗✓✗	64	buildFrom(_jbMfb.inverse(Eigen::Affine), X);
172	✓✗✓✗ ✓✗✓✗ ✓✗	128	MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
173	✓✗✓✗ ✓✗✓✗	128	oMja.access(time).rotation().transpose() *
174	✓✗✓✗ ✓✗✓✗	160	oMjb.access(time).rotation() * _jbMfb.rotation();
175			if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
176	✓✗✓✗	32	X.topRows<3>().applyOnTheLeft(faRfb);
177	✓✗✓✗	128	LieGroup_t().template dDifference<pinocchio::ARG1>(
178		64	q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
179
180			// Contribution of b:
181			// J = Jminus * X * jbJjb;
182		64	unsigned int rJ = 0;
183	✓✓	448	for (unsigned int r = 0; r < 6; ++r)
184	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	384	if (fl((int)r)) J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
185
186	✓✓	64	if (jaJja.isPlugged()) {
187	✓✗	32	const Matrix &_jaJja = jaJja(time);
188	✓✗	32	const MatrixHomogeneous &_jaMfa =
189	✓✗	64	(jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
190	✓✗	32	_faMfb = faMfb.accessCopy();
191
192	✓✗✓✗ ✓✗	32	buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
193			if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
194	✓✗✓✗	16	X.topRows<3>().applyOnTheLeft(faRfb);
195
196			// J -= (Jminus * X) * jaJja(time);
197		32	rJ = 0;
198	✓✓	224	for (unsigned int r = 0; r < 6; ++r)
199	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	192	if (fl((int)r)) J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
200			}
201
202		64	return J;
203			}
204
205			template <Representation_t representation>
206		1184	MatrixHomogeneous &FeaturePose<representation>::computefaMfb(
207			MatrixHomogeneous &res, int time) {
208		1184	check(*this);
209
210	✓✗✓✗ ✓✗✓✗	1184	res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
211			jbMfb(time);
212		1184	return res;
213			}
214
215			template <Representation_t representation>
216		1184	Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
217		1184	check(*this);
218
219		1184	toVector(faMfb(time), res);
220		1184	return res;
221			}
222
223			template <Representation_t representation>
224		96	Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
225		96	check(*this);
226
227		96	toVector(faMfbDes(time), res);
228		96	return res;
229			}
230
231			template <Representation_t representation>
232		1184	Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
233			typedef typename internal::LG_t<representation>::type LieGroup_t;
234	✓✗	1184	check(*this);
235
236	✓✗	1184	const Flags &fl = selectionSIN(time);
237
238	✓✗	1184	Eigen::Matrix<double, 6, 1> v;
239	✓✗✓✗ ✓✗✓✗	1184	LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);
240
241	✓✗✓✗	1184	error.resize(dimensionSOUT(time));
242		1184	unsigned int cursor = 0;
243	✓✓	8288	for (unsigned int i = 0; i < 6; ++i)
244	✓✗✓✗ ✓✗✓✗	7104	if (fl((int)i)) error(cursor++) = v(i);
245
246		1184	return error;
247			}
248
249			// This function is responsible of converting the input velocity expressed with
250			// SE(3) convention onto a velocity expressed with the convention of this
251			// feature (R^3xSO(3) or SE(3)), in the right frame.
252			template <>
253		56	Vector6d convertVelocity<SE3_t>(const MatrixHomogeneous &M,
254			const MatrixHomogeneous &Mdes,
255			const Vector &faNufafbDes) {
256			(void)M;
257	✓✗	56	MatrixTwist X;
258	✓✗✓✗	56	buildFrom(Mdes.inverse(Eigen::Affine), X);
259	✓✗✓✗	112	return X * faNufafbDes;
260			}
261			template <>
262		56	Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
263			const MatrixHomogeneous &Mdes,
264			const Vector &faNufafbDes) {
265		56	Vector6d nu;
266	✓✗	56	nu.head<3>() =
267	✓✗✓✗ ✓✗✓✗ ✓✗	112	faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
268	✓✗✓✗ ✓✗✓✗ ✓✗	56	nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
269		56	return nu;
270			}
271
272			template <Representation_t representation>
273		224	Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
274			int time) {
275			typedef typename internal::LG_t<representation>::type LieGroup_t;
276	✓✗	224	check(*this);
277
278	✓✗✓✗	224	errordot.resize(dimensionSOUT(time));
279	✓✗	224	const Flags &fl = selectionSIN(time);
280	✗✓	224	if (!faNufafbDes.isPlugged()) {
281			errordot.setZero();
282			return errordot;
283			}
284
285	✓✗	224	q_faMfb.recompute(time);
286	✓✗	224	q_faMfbDes.recompute(time);
287	✓✗	224	faNufafbDes.recompute(time);
288
289	✓✗	224	const MatrixHomogeneous &_faMfbDes = faMfbDes(time);
290
291	✓✗	224	Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
292
293	✓✗✓✗	448	LieGroup_t().template dDifference<pinocchio::ARG0>(
294		224	q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
295	✓✗	224	Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
296	✓✗✓✗	224	faNufafbDes.accessCopy());
297		224	unsigned int cursor = 0;
298	✓✓	1568	for (unsigned int i = 0; i < 6; ++i)
299	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1344	if (fl((int)i)) errordot(cursor++) = Jminus.row(i) * nu;
300
301		224	return errordot;
302			}
303
304			/* Modify the value of the reference (sdes) so that it corresponds
305			* to the current position. The effect on the servo is to maintain the
306			* current position and correct any drift. */
307			template <Representation_t representation>
308			void FeaturePose<representation>::servoCurrentPosition(const int &time) {
309			check(*this);
310
311			const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id),
312			_jaMfa =
313			(jaMfa.isPlugged() ? jaMfa.access(time) : Id),
314			_oMjb = oMjb.access(time),
315			_jbMfb =
316			(jbMfb.isPlugged() ? jbMfb.access(time) : Id);
317			faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
318			}
319
320			static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
321			template <Representation_t representation>
322			void FeaturePose<representation>::display(std::ostream &os) const {
323			os << CLASS_NAME << "<" << name << ">: (";
324
325			try {
326			const Flags &fl = selectionSIN.accessCopy();
327			bool first = true;
328			for (int i = 0; i < 6; ++i)
329			if (fl(i)) {
330			if (first) {
331			first = false;
332			} else {
333			os << ",";
334			}
335			os << featureNames[i];
336			}
337			os << ") ";
338			} catch (ExceptionAbstract e) {
339			os << " selectSIN not set.";
340			}
341			}
342
343			} // namespace sot
344			} // namespace dynamicgraph