test_feature_generic.cpp

Go to the documentation of this file.

/*
 * Copyright 2019,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <iostream>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/sample-models.hpp>

#define BOOST_TEST_MODULE features
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>

#include <Eigen/SVD>
#include <boost/test/unit_test.hpp>
#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-generic.hh>
#include <sot/core/feature-pose.hh>
#include <sot/core/gain-adaptive.hh>
#include <sot/core/sot.hh>
#include <sot/core/task.hh>

namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;

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
}  // namespace sot
}  // namespace dynamicgraph

using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;

#define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision)         \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision),       \
                      "check " #Va ".isApprox(" #Vb       \
                      ") failed "                         \
                      "[\n"                               \
                          << (Va).transpose() << "\n!=\n" \
                          << (Vb).transpose() << "\n]")
#define EIGEN_MATRIX_IS_APPROX(Va, Vb, precision)                           \
  BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), "check " #Va            \
                                                    ".isApprox(" #Vb        \
                                                    ") failed "             \
                                                    "[\n"                   \
                                                        << (Va) << "\n!=\n" \
                                                        << (Vb) << "\n]")

class FeatureTestBase {
 public:
  Task task_;
  int time_;
  dynamicgraph::Vector expectedTaskOutput_;

  FeatureTestBase(unsigned dim, const std::string &name)
      : task_("task" + name), time_(0) {
    expectedTaskOutput_.resize(dim);
  }

  virtual void init() {
    task_.addFeature(featureAbstract());
    task_.setWithDerivative(true);
  }

  void computeExpectedTaskOutput(const Vector &error,
                                 const Vector &errorDrift) {
    double gain = task_.controlGainSIN;
    expectedTaskOutput_ = -gain * error - errorDrift;
  }

  template <typename LG_t>
  void computeExpectedTaskOutput(const Vector &s, const Vector &sdes,
                                 const Vector &sDesDot, const LG_t &lg) {
    Vector s_sd(lg.nv());
    lg.difference(sdes, s, s_sd);

    Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(),
                                                                    lg.nv());
    lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);

    computeExpectedTaskOutput(s_sd, Jminus * sDesDot);
  }

  void checkTaskOutput() {
    BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(),
                        expectedTaskOutput_.size());
    Vector taskOutput(expectedTaskOutput_.size());
    for (int i = 0; i < expectedTaskOutput_.size(); ++i)
      taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
    EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
  }

  void setGain(double gain) {
    task_.controlGainSIN = gain;
    task_.controlGainSIN.access(time_);
    task_.controlGainSIN.setReady();
  }

  template <typename SignalType, typename ValueType>
  void setSignal(SignalType &sig, const ValueType &v) {
    sig = v;
    sig.access(time_);
    sig.setReady();
  }

  virtual FeatureAbstract &featureAbstract() = 0;

  virtual void setInputs() = 0;

  virtual void printInputs() {}

  virtual void recompute() {
    task_.taskSOUT.recompute(time_);

    // Check that recomputing went fine.
    FeatureAbstract &f(featureAbstract());
    BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime());
    BOOST_CHECK_EQUAL(time_, task_.errorTimeDerivativeSOUT.getTime());

    // TODO check that the output is correct
    // computeExpectedTaskOutput (s - sd, - vd);
  }

  virtual void printOutputs() {}

  virtual void checkValue() {
    time_++;
    setInputs();
    printInputs();
    recompute();
    printOutputs();
  }
};

class TestFeatureGeneric : public FeatureTestBase {
 public:
  FeatureGeneric feature_, featureDes_;
  int dim_;

  TestFeatureGeneric(unsigned dim, const std::string &name)
      : FeatureTestBase(dim, name),
        feature_("feature" + name),
        featureDes_("featureDes" + name),
        dim_(dim) {
    feature_.setReference(&featureDes_);
    feature_.selectionSIN = Flags(true);

    dynamicgraph::Matrix Jq(dim, dim);
    Jq.setIdentity();
    feature_.jacobianSIN.setReference(&Jq);

    init();
  }

  FeatureAbstract &featureAbstract() { return feature_; }

  void setInputs() {
    dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
    double gain;

    switch (time_) {
      case 0:
        BOOST_TEST_MESSAGE(" ----- Test Velocity -----");
        s.setZero();
        sd.setZero();
        vd.setZero();
        gain = 0.0;
        break;
      case 1:
        BOOST_TEST_MESSAGE(" ----- Test Position -----");
        s.setZero();
        sd.setConstant(2.);
        vd.setZero();
        gain = 1.0;
        break;
      case 2:
        BOOST_TEST_MESSAGE(" ----- Test both -----");
        s.setZero();
        sd.setConstant(2.);
        vd.setConstant(1.);
        gain = 3.0;
        break;
      default:
        s.setRandom();
        sd.setRandom();
        vd.setRandom();
        gain = 1.0;
    }

    feature_.errorSIN = s;
    feature_.errorSIN.access(time_);
    feature_.errorSIN.setReady();

    featureDes_.errorSIN = sd;
    featureDes_.errorSIN.access(time_);
    featureDes_.errorSIN.setReady();

    featureDes_.errordotSIN = vd;
    featureDes_.errordotSIN.access(time_);
    featureDes_.errordotSIN.setReady();

    setGain(gain);
  }

  void printInputs() {
    BOOST_TEST_MESSAGE("----- inputs -----");
    BOOST_TEST_MESSAGE(
        "feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE(
        "featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE("featureDes_.errordotSIN: "
                       << featureDes_.errordotSIN(time_).transpose());
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
  }

  void recompute() {
    FeatureTestBase::recompute();

    dynamicgraph::Vector s = feature_.errorSIN;
    dynamicgraph::Vector sd = featureDes_.errorSIN;
    dynamicgraph::Vector vd = featureDes_.errordotSIN;

    computeExpectedTaskOutput(s - sd, -vd);

    checkTaskOutput();
  }

  void printOutputs() {
    BOOST_TEST_MESSAGE("----- output -----");
    BOOST_TEST_MESSAGE("time: " << time_);
    BOOST_TEST_MESSAGE(
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
    BOOST_TEST_MESSAGE(
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
    // task_.errorTimeDerivativeSOUT(time_));
    BOOST_TEST_MESSAGE(
        "Expected task output: " << expectedTaskOutput_.transpose());
  }
};

BOOST_AUTO_TEST_SUITE(feature_generic)

BOOST_AUTO_TEST_CASE(check_value) {
  std::string srobot("test");
  unsigned int dim = 6;

  TestFeatureGeneric testFeatureGeneric(dim, srobot);

  for (int i = 0; i < 10; ++i) testFeatureGeneric.checkValue();
}

BOOST_AUTO_TEST_SUITE_END()  // feature_generic

MatrixHomogeneous randomM() {
  MatrixHomogeneous M;
  M.translation().setRandom();
  Eigen::Vector3d w(Eigen::Vector3d::Random());
  M.linear() = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix();
  return M;
}

typedef pinocchio::SE3 SE3;

Vector7 toVector(const pinocchio::SE3 &M) {
  Vector7 v;
  v.head<3>() = M.translation();
  QuaternionMap(v.tail<4>().data()) = M.rotation();
  return v;
}

Vector toVector(const std::vector<MultiBound> &in) {
  Vector out(in.size());
  for (int i = 0; i < (int)in.size(); ++i) out[i] = in[i].getSingleBound();
  return out;
}

template <Representation_t representation>
class TestFeaturePose : public FeatureTestBase {
 public:
  typedef typename dg::sot::internal::LG_t<representation>::type LieGroup_t;
  FeaturePose<representation> feature_;
  bool relative_;
  pinocchio::Model model_;
  pinocchio::Data data_;
  pinocchio::JointIndex ja_, jb_;
  pinocchio::FrameIndex fa_, fb_;

  TestFeaturePose(bool relative, const std::string &name)
      : FeatureTestBase(6, name),
        feature_("feature" + name),
        relative_(relative),
        data_(model_) {
    pinocchio::buildModels::humanoid(model_, true);  // use freeflyer
    model_.lowerPositionLimit.head<3>().setConstant(-1.);
    model_.upperPositionLimit.head<3>().setConstant(1.);
    jb_ = model_.getJointId("rarm_wrist2_joint");
    fb_ = model_.addFrame(pinocchio::Model::Frame(
        "frame_b", jb_,
        model_.getFrameId("rarm_wrist2_joint", pinocchio::JOINT),
        SE3::Identity(), pinocchio::OP_FRAME));
    if (relative) {
      ja_ = model_.getJointId("larm_wrist2_joint");
      fa_ = model_.addFrame(pinocchio::Model::Frame(
          "frame_a", ja_,
          model_.getFrameId("larm_wrist2_joint", pinocchio::JOINT),
          SE3::Identity(), pinocchio::OP_FRAME));
    } else {
      ja_ = 0;
      fa_ = model_.addFrame(pinocchio::Model::Frame(
          "frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME));
    }
    data_ = pinocchio::Data(model_);

    init();

    setJointFrame();
  }

  void _setFrame() {
    setSignal(
        feature_.jaMfa,
        MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix()));
    setSignal(
        feature_.jbMfb,
        MatrixHomogeneous(model_.frames[fb_].placement.toHomogeneousMatrix()));
  }

  void setJointFrame() {
    model_.frames[fa_].placement.setIdentity();
    model_.frames[fb_].placement.setIdentity();
    _setFrame();
  }

  void setRandomFrame() {
    model_.frames[fa_].placement.setRandom();
    model_.frames[fb_].placement.setRandom();
    _setFrame();
  }

  FeatureAbstract &featureAbstract() { return feature_; }

  void setInputs() {
    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    // Desired
    setSignal(feature_.faMfbDes, randomM());
    setSignal(feature_.faNufafbDes, Vector::Random(6));

    double gain = 0;
    // if (time_ % 5 != 0)
    // gain = 2 * (double)rand() / RAND_MAX;
    if (time_ % 2 != 0) gain = 1;
    setGain(gain);
  }

  void printInputs() {
    BOOST_TEST_MESSAGE("----- inputs -----");
    // BOOST_TEST_MESSAGE("feature_.errorSIN: " <<
    // feature_.errorSIN(time_).transpose());
    BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
  }

  void recompute() {
    FeatureTestBase::recompute();

    const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_],
              faMfb(feature_.faMfb.accessCopy().matrix()),
              faMfbDes(feature_.faMfbDes.accessCopy().matrix());
    const Vector &nu(feature_.faNufafbDes.accessCopy());

    computeExpectedTaskOutput(
        toVector(oMfa.inverse() * oMfb), toVector(faMfbDes),
        (boost::is_same<LieGroup_t, SE3_t>::value
             ? faMfbDes.toActionMatrixInverse() * nu
             : (Vector6d() << nu.head<3>() -
                                  faMfb.translation().cross(nu.tail<3>()),
                faMfbDes.rotation().transpose() * nu.tail<3>())
                   .finished()),
        LieGroup_t());

    checkTaskOutput();
  }

  void printOutputs() {
    BOOST_TEST_MESSAGE("----- output -----");
    BOOST_TEST_MESSAGE("time: " << time_);
    BOOST_TEST_MESSAGE(
        "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
    BOOST_TEST_MESSAGE(
        "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
    BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
    // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
    // task_.errorTimeDerivativeSOUT(time_));
    BOOST_TEST_MESSAGE(
        "Expected task output: " << expectedTaskOutput_.transpose());
  }

  virtual void checkJacobian() {
    time_++;
    // We want to check that e (q+dq, t) ~ e(q, t) + de/dq(q,t) dq

    setGain(1.);

    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    // Desired
    setSignal(feature_.faMfbDes, randomM());

    // Get task jacobian
    Vector e_q = task_.errorSOUT.access(time_);
    J = task_.jacobianSOUT.access(time_);

    Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "",
                                "[", "]\n");
    Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
                                "[", "]\n");

    Vector qdot(Vector::Zero(model_.nv));
    double eps = 1e-6;
    BOOST_TEST_MESSAGE(
        data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt)
        << data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt)
        << model_.frames[fa_].placement.toHomogeneousMatrix().format(
               PyMatrixFmt)
        << model_.frames[fb_].placement.toHomogeneousMatrix().format(
               PyMatrixFmt)
        << J.format(PyMatrixFmt));

    for (int i = 0; i < model_.nv; ++i) {
      time_++;
      qdot(i) = eps;

      // Update pose signals
      Vector q_qdot = pinocchio::integrate(model_, q, qdot);
      pinocchio::framesForwardKinematics(model_, data_, q_qdot);
      setSignal(feature_.oMjb,
                MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
      if (relative_) {
        setSignal(feature_.oMja,
                  MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
      }

      // Recompute output and check finite diff
      Vector e_q_dq = task_.errorSOUT.access(time_);
      BOOST_CHECK_MESSAGE(
          (((e_q_dq - e_q) / eps) - J.col(i)).maxCoeff() < 1e-4,
          "Jacobian col " << i << " does not match finite difference.\n"
                          << ((e_q_dq - e_q) / eps).format(PyVectorFmt) << '\n'
                          << J.col(i).format(PyVectorFmt) << '\n');

      qdot(i) = 0.;
    }
    time_++;
  }

  virtual void checkFeedForward() {
    setGain(0.);
    // random config
    // set inputs
    // 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 * faNufafbDes

    time_++;
    Vector q(pinocchio::randomConfiguration(model_));
    pinocchio::framesForwardKinematics(model_, data_, q);
    pinocchio::computeJointJacobians(model_, data_);

    SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]);

    // Poses
    setSignal(feature_.oMjb,
              MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
    if (relative_) {
      setSignal(feature_.oMja,
                MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
    }

    // Jacobians
    Matrix J(6, model_.nv);
    J.setZero();
    pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
    setSignal(feature_.jbJjb, J);
    if (relative_) {
      J.setZero();
      pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
      setSignal(feature_.jaJja, J);
    }

    Matrix faJfafb;
    J.setZero();
    pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J);
    faJfafb = faMfb.toActionMatrix() * J;
    J.setZero();
    pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J);
    faJfafb -= J;

    // Desired
    setSignal(feature_.faMfbDes, randomM());

    // Get task jacobian
    task_.jacobianSOUT.recompute(time_);
    J = task_.jacobianSOUT.accessCopy();
    Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Vector faNufafbDes(Vector::Zero(6));
    double eps = 1e-6;
    for (int i = 0; i < 6; ++i) {
      time_++;
      faNufafbDes(i) = 1.;
      setSignal(feature_.faNufafbDes, faNufafbDes);
      task_.taskSOUT.recompute(time_);

      Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy()));

      Vector faNufafb = faJfafb * qdot;
      EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps);

      faNufafbDes(i) = 0.;
    }
    time_++;
  }
};

template <typename TestClass>
void runTest(TestClass &runner, int N = 2)
// int N = 10)
{
  for (int i = 0; i < N; ++i) runner.checkValue();
  for (int i = 0; i < N; ++i) runner.checkJacobian();
  for (int i = 0; i < N; ++i) runner.checkFeedForward();
}

BOOST_AUTO_TEST_SUITE(feature_pose)

template <Representation_t representation>
void feature_pose_absolute_tpl(const std::string &repr) {
  BOOST_TEST_MESSAGE("absolute " << repr);
  TestFeaturePose<representation> testAbsolute(false, "abs" + repr);
  testAbsolute.setJointFrame();
  runTest(testAbsolute);

  testAbsolute.setRandomFrame();
  runTest(testAbsolute);
}

BOOST_AUTO_TEST_SUITE(absolute)

BOOST_AUTO_TEST_CASE(r3xso3) {
  feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3");
}

BOOST_AUTO_TEST_CASE(se3) {
  feature_pose_absolute_tpl<SE3Representation>("SE3");
}

BOOST_AUTO_TEST_SUITE_END()  // absolute

template <Representation_t representation>
void feature_pose_relative_tpl(const std::string &repr) {
  BOOST_TEST_MESSAGE("relative " << repr);
  TestFeaturePose<representation> testRelative(true, "rel" + repr);
  runTest(testRelative);

  testRelative.setRandomFrame();
  runTest(testRelative);
}

BOOST_AUTO_TEST_SUITE(relative)

BOOST_AUTO_TEST_CASE(r3xso3) {
  feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3");
}

BOOST_AUTO_TEST_CASE(se3) {
  feature_pose_relative_tpl<SE3Representation>("SE3");
}

BOOST_AUTO_TEST_SUITE_END()  // relative

BOOST_AUTO_TEST_SUITE_END()  // feature_pose