joint-mimic.cpp

Go to the documentation of this file.

//
// Copyright (c) 2019 INRIA
//
 
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
 
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
 
using namespace pinocchio;
 
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6x;
 
template<typename JointModel>
void test_constraint_mimic(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::Constraint_t ConstraintType;
  typedef typename traits<Joint>::JointDataDerived JointData;
  typedef ScaledJointMotionSubspace<ConstraintType> ScaledJointMotionSubspaceType;
 
  JointData jdata = jmodel.createData();
 
  const double scaling_factor = 2.;
  ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S);
  ScaledJointMotionSubspaceType scaled_constraint(constraint_ref_shared, scaling_factor);
 
  BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv());
 
  typedef typename JointModel::TangentVector_t TangentVector_t;
  TangentVector_t v = TangentVector_t::Random();
 
  Motion m = scaled_constraint * v;
  Motion m_ref = scaling_factor * (Motion)(constraint_ref * v);
 
  BOOST_CHECK(m.isApprox(m_ref));
 
  {
    SE3 M = SE3::Random();
    typename ScaledJointMotionSubspaceType::DenseBase S = M.act(scaled_constraint);
    typename ScaledJointMotionSubspaceType::DenseBase S_ref =
      scaling_factor * M.act(constraint_ref);
 
    BOOST_CHECK(S.isApprox(S_ref));
  }
 
  {
    typename ScaledJointMotionSubspaceType::DenseBase S = scaled_constraint.matrix();
    typename ScaledJointMotionSubspaceType::DenseBase S_ref =
      scaling_factor * constraint_ref.matrix();
 
    BOOST_CHECK(S.isApprox(S_ref));
  }
 
  {
    Motion v = Motion::Random();
    typename ScaledJointMotionSubspaceType::DenseBase S = v.cross(scaled_constraint);
    typename ScaledJointMotionSubspaceType::DenseBase S_ref =
      scaling_factor * v.cross(constraint_ref);
 
    BOOST_CHECK(S.isApprox(S_ref));
  }
 
  // Test transpose operations
  {
    const Eigen::DenseIndex dim = 20;
    const Matrix6x Fin = Matrix6x::Random(6, dim);
    Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin;
    Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin);
    BOOST_CHECK(Fout.isApprox(Fout_ref));
 
    Force force_in(Force::Random());
    Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in);
    Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in);
    BOOST_CHECK(Stf_ref.isApprox(Stf));
  }
 
  // CRBA Y*S
  {
    Inertia Y = Inertia::Random();
    Eigen::MatrixXd YS = Y * scaled_constraint;
    Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref);
 
    BOOST_CHECK(YS.isApprox(YS_ref));
  }
}
 
struct TestJointConstraint
{
 
  template<typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0, 0, 0);
 
    test_constraint_mimic(jmodel);
  }
 
  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
  {
    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0, 0, 0);
 
    test_constraint_mimic(jmodel);
  }
 
  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
  {
    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0, 0, 0);
 
    test_constraint_mimic(jmodel);
  }
};
 
BOOST_AUTO_TEST_CASE(test_constraint)
{
  using namespace pinocchio;
  typedef boost::variant<
    JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX,
    JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY,
    JointModelRUBZ>
    Variant;
 
  boost::mpl::for_each<Variant::types>(TestJointConstraint());
}
 
template<typename JointModel>
void test_joint_mimic(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::JointDataDerived JointData;
 
  JointData jdata = jmodel.createData();
 
  const double scaling_factor = 1.;
  const double offset = 0.;
 
  typedef JointMimic<Joint> JointMimicType;
  typedef typename traits<JointMimicType>::JointModelDerived JointModelMimicType;
  typedef typename traits<JointMimicType>::JointDataDerived JointDataMimicType;
 
  // test constructor
  JointModelMimicType jmodel_mimic(jmodel.derived(), scaling_factor, offset);
  JointDataMimicType jdata_mimic = jmodel_mimic.createData();
 
  BOOST_CHECK(jmodel_mimic.nq() == 0);
  BOOST_CHECK(jmodel_mimic.nv() == 0);
 
  BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q());
  BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v());
 
  BOOST_CHECK(jmodel_mimic.idx_q() == 0);
  BOOST_CHECK(jmodel_mimic.idx_v() == 0);
 
  typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType;
  typedef typename LieGroup<JointModel>::type LieGroupType;
  ConfigVectorType q0 =
    LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones());
 
  jmodel.calc(jdata, q0);
  jmodel_mimic.calc(jdata_mimic, q0);
 
  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));
 
  typedef typename JointModelMimicType::TangentVector_t TangentVectorType;
 
  q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones());
  TangentVectorType v0 = TangentVectorType::Random();
  jmodel.calc(jdata, q0, v0);
  jmodel_mimic.calc(jdata_mimic, q0, v0);
 
  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));
  BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v()));
}
 
struct TestJointMimic
{
 
  template<typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0, 0, 0);
 
    test_joint_mimic(jmodel);
  }
 
  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
  {
    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0, 0, 0);
 
    test_joint_mimic(jmodel);
  }
 
  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
  {
    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0, 0, 0);
 
    test_joint_mimic(jmodel);
  }
};
 
BOOST_AUTO_TEST_CASE(test_joint)
{
  using namespace pinocchio;
  typedef boost::variant<
    JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX,
    JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY,
    JointModelRUBZ>
    Variant;
 
  boost::mpl::for_each<Variant::types>(TestJointMimic());
}
 
BOOST_AUTO_TEST_CASE(test_transform_linear_affine)
{
  typedef JointModelRX::ConfigVector_t ConfigVectorType;
  double scaling = 1., offset = 0.;
 
  ConfigVectorType q0 = ConfigVectorType::Random();
  ConfigVectorType q1;
  LinearAffineTransform::run(q0, scaling, offset, q1);
  BOOST_CHECK(q0 == q1);
 
  offset = 2.;
  LinearAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1);
  BOOST_CHECK(q1 == ConfigVectorType::Constant(offset));
}
 
BOOST_AUTO_TEST_CASE(test_transform_linear_revolute)
{
  typedef JointModelRUBX::ConfigVector_t ConfigVectorType;
  double scaling = 1., offset = 0.;
 
  ConfigVectorType q0 = ConfigVectorType::Random().normalized();
  ConfigVectorType q1;
  UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1);
  BOOST_CHECK(q0.isApprox(q1));
 
  offset = 2.;
  UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1);
  BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset), math::sin(offset)));
}
 
BOOST_AUTO_TEST_CASE(test_joint_generic_cast)
{
  JointModelRX jmodel_ref;
  jmodel_ref.setIndexes(1, 2, 3);
 
  JointModelMimic<JointModelRX> jmodel(jmodel_ref, 2., 1.);
  jmodel.setIndexes(1, -1, -1);
 
  BOOST_CHECK(jmodel.id() == jmodel_ref.id());
  BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q());
  BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v());
 
  JointModel jmodel_generic(jmodel);
  jmodel_generic.setIndexes(1, -2, -2);
 
  BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id());
}
BOOST_AUTO_TEST_SUITE_END()