Unverified Commit b22c502e authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1443 from jcarpent/topic/joints

Enhance comparisons of Joint variants
parents 0277fa4f a2922812
Pipeline #14252 passed with stage
in 126 minutes and 39 seconds
......@@ -18,7 +18,7 @@ namespace pinocchio
template<class JointModelDerived>
struct JointModelDerivedPythonVisitor
: public boost::python::def_visitor< JointModelDerivedPythonVisitor<JointModelDerived> >
: public boost::python::def_visitor< JointModelDerivedPythonVisitor<JointModelDerived> >
{
public:
......@@ -26,14 +26,21 @@ namespace pinocchio
void visit(PyClass& cl) const
{
cl
.def(bp::init<>(bp::arg("self")))
// All are add_properties cause ReadOnly
.add_property("id",&get_id)
.add_property("idx_q",&get_idx_q)
.add_property("idx_v",&get_idx_v)
.add_property("nq",&get_nq)
.add_property("nv",&get_nv)
.def("setIndexes",&JointModelDerived::setIndexes)
.def("shortname",&JointModelDerived::shortname)
.def("setIndexes",
&JointModelDerived::setIndexes,
bp::args("self","id","idx_q","idx_v"))
.def("hasSameIndexes",
&JointModelDerived::template hasSameIndexes<JointModelDerived>,
bp::args("self","other"),
"Check if this has same indexes than other.")
.def("shortname",&JointModelDerived::shortname,bp::arg("self"))
.def(bp::self == bp::self)
.def(bp::self != bp::self)
......@@ -51,14 +58,11 @@ namespace pinocchio
static int get_nv(const JointModelDerived & self)
{ return self.nv(); }
static void expose()
{}
};
template<class JointDataDerived>
struct JointDataDerivedPythonVisitor
: public boost::python::def_visitor< JointDataDerivedPythonVisitor<JointDataDerived> >
: public boost::python::def_visitor< JointDataDerivedPythonVisitor<JointDataDerived> >
{
public:
......
......@@ -31,8 +31,14 @@ namespace pinocchio
.add_property("idx_v",&getIdx_v)
.add_property("nq",&getNq)
.add_property("nv",&getNv)
.def("setIndexes",&JointModel::setIndexes)
.def("shortname",&JointModel::shortname)
.def("setIndexes",
&JointModel::setIndexes,
bp::args("self","id","idx_q","idx_v"))
.def("hasSameIndexes",
&JointModel::hasSameIndexes<JointModel>,
bp::args("self","other"),
"Check if this has same indexes than other.")
.def("shortname",&JointModel::shortname,bp::arg("self"))
.def(bp::self == bp::self)
.def(bp::self != bp::self)
......
......@@ -57,7 +57,7 @@ namespace pinocchio
expose_joint_model<T>(
bp::class_<T>(T::classname().c_str(),
T::classname().c_str(),
bp::init<>())
bp::no_init)
.def(JointModelDerivedPythonVisitor<T>())
.def(PrintableVisitor<T>())
);
......
//
// Copyright (c) 2016-2020 CNRS INRIA
// Copyright (c) 2016-2021 CNRS INRIA
//
#ifndef __pinocchio_algorithm_joint_configuration_hpp__
......@@ -350,6 +350,7 @@ namespace pinocchio
*
* @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
* to the tangent space at \f$ q \f$.
* It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations.
* In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
* A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
......@@ -378,6 +379,7 @@ namespace pinocchio
*
* @details This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \f$ q \oplus v \f$,
* to the tangent space at \f$ q \f$.
* It performs the product with the Jacobian of integrate by exploiting at best the sparsity of the underlying operations.
* In other words, this functions transforms a tangent vector expressed at \f$ q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of configuration between
* \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent vector.
* A typical example of parallel transportation is the action operated by a rigid transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$.
......
......@@ -174,6 +174,31 @@ namespace pinocchio
typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type
cast_joint(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/**
* @brief Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived
*
* @param[in] jmodel_generic The generic joint model containing a variant.
* @param[in] jmodel The other joint model for the comparison.
*
* @return True if the two joint models are equal.
*/
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool isEqual(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel);
/**
* @brief Check whether JointModelTpl<Scalar,...> has the indexes than another JointModelDerived
*
* @param[in] jmodel_generic The generic joint model containing a variant.
* @param[in] jmodel The other joint modelto compare with
*
* @return True if the two joints have the same indexes.
*/
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel);
//
// Visitors on JointDatas
......@@ -263,6 +288,18 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
udinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/**
* @brief Visit a JointDataTpl<Scalar,...> to compare it to another JointData
*
* @param[in] jdata_generic The generic joint data containing a variant.
* @param[in] jdata The other joint data for the comparison.
*
* @return True if the two joints data are equal.
*/
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool isEqual(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointDataBase<JointDataDerived> & jmodel);
} // namespace pinocchio
......
......@@ -301,6 +301,52 @@ namespace pinocchio
return Algo::run(jmodel);
}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
struct JointModelComparisonOperatorVisitor
: fusion::JointUnaryVisitorBase< JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool>
{
typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
template<typename JointModel>
static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
const JointModelDerived & jmodel_rhs)
{
return jmodel_lhs.derived() == jmodel_rhs;
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool isEqual(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel)
{
typedef JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo;
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
struct JointModelHasSameIndexesVisitor
: fusion::JointUnaryVisitorBase< JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool>
{
typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
template<typename JointModel>
static bool algo(const JointModelBase<JointModel> & jmodel_lhs,
const JointModelDerived & jmodel_rhs)
{
return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs);
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic,
const JointModelBase<JointModelDerived> & jmodel)
{
typedef JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo;
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived())));
}
//
// Visitors on JointDatas
//
......@@ -520,6 +566,30 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline std::string shortname(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata)
{ return JointDataShortnameVisitor::run(jdata);}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
struct JointDataComparisonOperatorVisitor
: fusion::JointUnaryVisitorBase< JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived>,bool>
{
typedef boost::fusion::vector<const JointDataDerived &> ArgsType;
template<typename JointData>
static bool algo(const JointDataBase<JointData> & jdata_lhs,
const JointDataDerived & jdata_rhs)
{
return jdata_lhs.derived() == jdata_rhs;
}
};
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool isEqual(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata_generic,
const JointDataBase<JointDataDerived> & jdata)
{
typedef JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived> Algo;
return Algo::run(jdata_generic,typename Algo::ArgsType(boost::ref(jdata.derived())));
}
/// @endcond
......
//
// Copyright (c) 2016-2020 CNRS INRIA
// Copyright (c) 2016-2021 CNRS INRIA
//
#ifndef __pinocchio_joint_composite_hpp__
......@@ -156,13 +156,14 @@ namespace pinocchio
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelVariant;
typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
typedef JointModel JointModelVariant;
typedef SE3Tpl<Scalar,Options> SE3;
typedef MotionTpl<Scalar,Options> Motion;
typedef InertiaTpl<Scalar,Options> Inertia;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
using Base::id;
using Base::idx_q;
......@@ -325,6 +326,7 @@ namespace pinocchio
using Base::isEqual;
bool isEqual(const JointModelCompositeTpl & other) const
{
std::cout << "JointModelCompositeTpl::isEqual" << std::endl;
return Base::isEqual(other)
&& nq() == other.nq()
&& nv() == other.nv()
......
......@@ -120,13 +120,14 @@ namespace pinocchio
return os;
}
bool operator==(const JointDataBase<Derived> & other) const
template<typename OtherDerived>
bool operator==(const JointDataBase<OtherDerived> & other) const
{
return derived().isEqual(other.derived());
}
/// \brief Default operator== implementation
bool isEqual(const JointDataBase<Derived> & other) const
bool isEqual(const JointDataBase & other) const
{
return S() == other.S()
&& M() == other.M()
......@@ -138,6 +139,14 @@ namespace pinocchio
;
}
/// \brief Default operator== implementation
template<typename OtherDerived>
bool isEqual(const JointDataBase<OtherDerived> & /*other*/) const
{
return false;
;
}
bool operator!=(const JointDataBase<Derived> & other) const
{
return derived().isNotEqual(other.derived());
......
//
// Copyright (c) 2016-2020 CNRS INRIA
// Copyright (c) 2016-2021 CNRS INRIA
//
#ifndef __pinocchio_joint_generic_hpp__
......@@ -128,10 +128,25 @@ namespace pinocchio
static std::string classname() { return "JointData"; }
std::string shortname() const { return ::pinocchio::shortname(*this); }
template<typename JointDataDerived>
bool isEqual(const JointDataBase<JointDataDerived> & other) const
{
return ::pinocchio::isEqual(*this,other.derived());
}
bool isEqual(const JointDataTpl & other) const
{
return Base::isEqual(other)
&& toVariant() == other.toVariant();
return Base::isEqual(other) && toVariant() == other.toVariant();
}
bool operator==(const JointDataTpl & other) const
{
return isEqual(other);
}
bool operator!=(const JointDataTpl & other) const
{
return !(*this == other);
}
};
......@@ -167,7 +182,8 @@ namespace pinocchio
JointModelTpl(const JointModelVariant & jmodel_variant)
: JointCollection::JointModelVariant(jmodel_variant)
{}
{
}
template<typename JointModelDerived>
JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
......@@ -185,11 +201,31 @@ namespace pinocchio
JointDataDerived createData() const
{ return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); }
using Base::isEqual;
template<typename JointModelDerived>
bool isEqual(const JointModelBase<JointModelDerived> & other) const
{
return ::pinocchio::isEqual(*this,other.derived());;
}
template<typename JointModelDerived>
bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const
{
return ::pinocchio::hasSameIndexes(*this,other.derived());
}
bool isEqual(const JointModelTpl & other) const
{
return Base::isEqual(other)
&& toVariant() == other.toVariant();
return Base::isEqual(other) && toVariant() == other.toVariant();
}
bool operator==(const JointModelTpl & other) const
{
return isEqual(other);
}
bool operator!=(const JointModelTpl & other) const
{
return !(*this == other);
}
template<typename ConfigVector>
......@@ -219,7 +255,9 @@ namespace pinocchio
JointIndex id_impl() const { return ::pinocchio::id(*this); }
void setIndexes(JointIndex id, int nq, int nv)
{ ::pinocchio::setIndexes(*this,id, nq, nv); }
{
::pinocchio::setIndexes(*this, id, nq, nv);
}
/// \returns An expression of *this with the Scalar type casted to NewScalar.
template<typename NewScalar>
......@@ -232,6 +270,36 @@ namespace pinocchio
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool operator==(const JointDataBase<JointDataDerived> & joint_data,
const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
{
return joint_data_generic == joint_data.derived();
}
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived>
bool operator!=(const JointDataBase<JointDataDerived> & joint_data,
const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic)
{
return joint_data_generic != joint_data.derived();
}
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool operator==(const JointModelBase<JointModelDerived> & joint_model,
const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
{
return joint_model_generic == joint_model.derived();
}
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived>
bool operator!=(const JointModelBase<JointModelDerived> & joint_model,
const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic)
{
return joint_model_generic != joint_model.derived();
}
} // namespace pinocchio
#endif // ifndef __pinocchio_joint_generic_hpp__
//
// Copyright (c) 2019 INRIA
// Copyright (c) 2019-2021 INRIA
//
#ifndef __pinocchio_joint_mimic_hpp__
#define __pinocchio_joint_mimic_hpp__
#ifndef __pinocchio_multibody_joint_mimic_hpp__
#define __pinocchio_multibody_joint_mimic_hpp__
#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
......@@ -316,6 +316,7 @@ namespace pinocchio
return *this;
}
using Base::isEqual;
bool isEqual(const JointDataMimic & other) const
{
return Base::isEqual(other)
......@@ -635,4 +636,4 @@ namespace pinocchio
} // namespace pinocchio
#endif // ifndef __pinocchio_joint_mimic_hpp__
#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__
......@@ -159,6 +159,12 @@ namespace pinocchio
{ return false; }
bool isEqual(const JointModelBase<Derived> & other) const
{
return derived().hasSameIndexes(other.derived());
}
template <class OtherDerived>
bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
{
return other.id() == id()
&& other.idx_q() == idx_q()
......
//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_multibody_visitior_joint_unary_visitor_hpp__
......@@ -69,6 +69,14 @@ namespace pinocchio
return boost::apply_visitor(visitor,jmodel);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp>
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata,
ArgsTmp args)
{
InternalVisitorModel<ArgsTmp> visitor(args);
return boost::apply_visitor(visitor,jdata);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel)
{
......@@ -76,6 +84,13 @@ namespace pinocchio
return boost::apply_visitor(visitor,jmodel);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata)
{
InternalVisitorModel<NoArg> visitor;
return boost::apply_visitor(visitor,jdata);
}
template<typename JointModelDerived, typename ArgsTmp>
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel,
ArgsTmp args)
......@@ -84,6 +99,14 @@ namespace pinocchio
return visitor(jmodel.derived());
}
template<typename JointDataDerived, typename ArgsTmp>
static ReturnType run(const JointDataBase<JointDataDerived> & jdata,
ArgsTmp args)
{
InternalVisitorModel<ArgsTmp> visitor(args);
return visitor(jdata.derived());
}
template<typename JointModelDerived>
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel)
{
......@@ -91,6 +114,13 @@ namespace pinocchio
return visitor(jmodel.derived());
}
template<typename JointDataDerived>
static ReturnType run(const JointDataBase<JointDataDerived> & jdata)
{
InternalVisitorModel<NoArg> visitor;
return visitor(jdata.derived());
}
private:
template<typename JointModel, typename ArgType>
......@@ -155,6 +185,14 @@ namespace pinocchio
args));
}
template<typename JointDataDerived>
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
{
return bf::invoke(&JointVisitorDerived::template algo<JointDataDerived>,
bf::append(boost::ref(jdata.derived()),
args));
}
ReturnType operator()(const JointModelVoid) {return;}
ArgType args;
......@@ -171,6 +209,12 @@ namespace pinocchio
{
return JointVisitorDerived::template algo<JointModelDerived>(jmodel.derived());
}
template<typename JointDataDerived>
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
{
return JointVisitorDerived::template algo<JointDataDerived>(jdata.derived());
}
};
}; // struct JointUnaryVisitorBase
......
//
// Copyright(c) 2015-2020 CNRS INRIA
// Copyright(c) 2015-2021 CNRS INRIA
// Copyright(c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include <boost/test/unit_test.hpp>
#include <iostream>
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
......@@ -11,9 +14,6 @@
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename JointModel_> struct init;
......@@ -141,11 +141,11 @@ struct TestJointModelIsEqual : TestJointModel<TestJointModelIsEqual>
static void test(const JointModelBase<JointModel> & jmodel)
{
JointModel jmodel_copy = jmodel.derived();
BOOST_CHECK(jmodel_copy == jmodel.derived());
BOOST_CHECK(jmodel_copy == jmodel);
JointModel jmodel_any;
BOOST_CHECK(jmodel_any != jmodel.derived());
BOOST_CHECK(!jmodel_any.isEqual(jmodel.derived()));
BOOST_CHECK(jmodel_any != jmodel);
BOOST_CHECK(!jmodel_any.isEqual(jmodel));
}
};
......@@ -173,6 +173,8 @@ struct TestJointModelCast : TestJointModel<TestJointModelCast>
static void test(const JointModelBase<JointModel> & jmodel)
{
typedef typename JointModel::Scalar Scalar;
BOOST_CHECK(jmodel == jmodel);
BOOST_CHECK(jmodel.template cast<Scalar>().isEqual(jmodel));
BOOST_CHECK(jmodel.template cast<Scalar>() == jmodel);