5 #include "pinocchio/multibody/joint/joint-generic.hpp" 7 #include "pinocchio/multibody/liegroup/liegroup.hpp" 9 #include "pinocchio/multibody/model.hpp" 11 #include "pinocchio/algorithm/joint-configuration.hpp" 13 #include <boost/test/unit_test.hpp> 14 #include <boost/utility/binary.hpp> 18 template <
typename Jo
intModel>
24 std::cout <<
"Testing Joint over " << jmodel.
shortname() << std::endl;
27 q1 = LieGroupType().random();
28 q2 = LieGroupType().random();
31 v1(Eigen::VectorXd::Random(jdata.
S().nv())),
32 v2(Eigen::VectorXd::Random(jdata.
S().nv()));
37 bool update_I =
false;
44 BOOST_CHECK(jmodel == jma);
45 BOOST_CHECK(jma == jmodel);
49 BOOST_CHECK(jda == jdata);
50 BOOST_CHECK(jdata == jda);
52 jma.
calc(jda, q1, v1);
57 jma.
calc(jda_other, q2, v2);
58 jma.
calc_aba(jda_other, Ia2, update_I);
60 BOOST_CHECK(jda_other != jda);
61 BOOST_CHECK(jda != jda_other);
62 BOOST_CHECK(jda_other != jdata);
63 BOOST_CHECK(jdata != jda_other);
65 const std::string error_prefix(
"JointModel on " + jma.
shortname());
66 BOOST_CHECK_MESSAGE(jmodel.
nq() == jma.
nq() ,std::string(error_prefix +
" - nq "));
67 BOOST_CHECK_MESSAGE(jmodel.
nv() == jma.
nv() ,std::string(error_prefix +
" - nv "));
69 BOOST_CHECK_MESSAGE(jmodel.
idx_q() == jma.
idx_q() ,std::string(error_prefix +
" - Idx_q "));
70 BOOST_CHECK_MESSAGE(jmodel.
idx_v() == jma.
idx_v() ,std::string(error_prefix +
" - Idx_v "));
71 BOOST_CHECK_MESSAGE(jmodel.
id() == jma.
id() ,std::string(error_prefix +
" - JointId "));
73 BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.
S().matrix()),std::string(error_prefix +
" - ConstraintXd "));
74 BOOST_CHECK_MESSAGE( (jda.M()).
isApprox((jdata.
M())),std::string(error_prefix +
" - Joint transforms "));
76 BOOST_CHECK_MESSAGE((jda.c()) == (jdata.
c()),std::string(error_prefix +
" - Joint bias "));
78 BOOST_CHECK_MESSAGE((jda.U()).
isApprox(jdata.
U()),std::string(error_prefix +
" - Joint U inertia matrix decomposition "));
79 BOOST_CHECK_MESSAGE((jda.Dinv()).
isApprox(jdata.
Dinv()),std::string(error_prefix +
" - Joint DInv inertia matrix decomposition "));
80 BOOST_CHECK_MESSAGE((jda.UDinv()).
isApprox(jdata.
UDinv()),std::string(error_prefix +
" - Joint UDInv inertia matrix decomposition "));
83 typedef typename JointModel::Constraint_t Constraint_t;
84 typedef typename Constraint_t::DenseBase ConstraintDense;
87 ConstraintDense vxS(v.
cross(jdata.
S()));
90 BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix +
"- Joint vxS operation "));
94 const Inertia::Matrix6 Idense(Isparse.matrix());
96 const ConstraintDense IsparseS = Isparse * jdata.
S();
97 const ConstraintDense IdenseS = Idense * jdata.
S();
99 BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix +
"- Joint YS operation "));
103 template<
typename Jo
intModel_>
struct init;
105 template<
typename Jo
intModel_>
111 jmodel.setIndexes(0,0,0);
116 template<
typename Scalar,
int Options>
124 JointModel jmodel(Vector3::Random().normalized());
131 template<
typename Scalar,
int Options>
139 JointModel jmodel(Vector3::Random().normalized());
146 template<
typename Scalar,
int Options>
154 JointModel jmodel(Vector3::Random().normalized());
161 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
176 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
193 template<
typename Jo
intModel_>
202 JointModel jmodel(jmodel_ref,1.,0.);
211 template <
typename Jo
intModel>
216 typename JointModel::JointDataDerived jdata = jmodel.
createData();
231 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
struct JointTest;
232 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
struct JointModelTest;
233 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
struct JointDataTest;
235 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
239 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
243 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
261 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
U_t;
262 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options>
D_t;
263 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
UD_t;
269 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
272 ,
JointModelBase< JointModelTest<_Scalar,_Options,JointCollectionTpl> >
282 : VariantBase(jmodel)
301 std::vector< JointModelTest<double,0,JointCollectionDefaultTpl> > jmodel_test_vector;
304 std::vector<JointModelVariant> jmodel_variant_vector;
305 jmodel_variant_vector.push_back(jmodel_revolute_x);
307 std::vector<JointModel> jmodel_generic_vector;
308 jmodel_generic_vector.push_back((
JointModel)jmodel_revolute_x);
314 boost::mpl::for_each<JointModelVariant::types>(
TestJoint());
320 std::cout <<
"nq " << jmodel.
nq() << std::endl;
321 std::cout <<
"nv " << jmodel.
nv() << std::endl;
322 std::cout <<
"idx_q " << jmodel.
idx_q() << std::endl;
323 std::cout <<
"idx_v " << jmodel.
idx_v() << std::endl;
324 std::cout <<
"id " << jmodel.
id() << std::endl;
325 std::cout <<
"name " << jmodel.
shortname() << std::endl;
327 BOOST_CHECK(jmodel.
idx_q() == -1);
328 BOOST_CHECK(jmodel.
idx_v() == -1);
336 BOOST_CHECK(joint_revolutex != joint_revolutey);
342 BOOST_CHECK(jmodelx_copy == jmodelx);
343 BOOST_CHECK(jmodelx_copy == jmodelx.
derived());
348 BOOST_CHECK(jmodely != jmodelx);
358 BOOST_CHECK(jmodelx.
cast<
double>() == jmodelx);
359 BOOST_CHECK(jmodelx.
cast<
long double>().cast<
double>() == jmodelx);
365 template <
typename Jo
intModel>
369 typedef typename JointModel::JointDataDerived
JointData;
383 jmodel.
calc(jdata,q,v);
384 Inertia::Matrix6 I = Inertia::Matrix6::Identity();
389 template <
typename Jo
intModel>
395 template<
typename Jo
intData>
400 std::cout <<
"name: " << jdata_generic1.
shortname() << std::endl;
401 BOOST_CHECK(jdata_generic1 == jdata_generic1);
411 BOOST_AUTO_TEST_SUITE_END ()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
void test_joint_methods(JointModelBase< JointModel > &jmodel, JointDataBase< typename JointModel::JointDataDerived > &jdata)
JointTpl< Scalar, Options, JointCollectionTpl > JointDerived
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
pinocchio::JointModelMimic< JointModel_ > JointModel
void operator()(const pinocchio::JointModelMimic< JointModel > &) const
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void setIndexes(JointIndex id, int q, int v)
void operator()(const JointModelBase< JointModel > &) const
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
UDTypeConstRef UDinv() const
BiasTypeConstRef c() const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
static InertiaTpl Random()
DTypeConstRef Dinv() const
std::string shortname() const
static void test(const JointData &jdata)
NewScalar cast(const Scalar &value)
JointDataTpl< double > JointData
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
MotionTpl< Scalar, Options > Motion_t
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
TansformTypeConstRef M() const
MotionTypeConstRef v() const
void operator()(const pinocchio::JointModelComposite &) const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
JointModelTest(const JointModelVariant &jmodel)
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
MotionTpl< Scalar, Options > Bias_t
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
std::string shortname() const
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
JointCollection::JointModelVariant VariantBase
bool hasSameIndexes(const JointModelBase< JointModelDerived > &other) const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
JointTpl< Scalar, Options, JointCollectionTpl > JointDerived
void operator()(const JointModelBase< JointModel > &) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
JointCollection::JointModelVariant JointModelVariant
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
#define BOOST_TEST_MODULE
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I=false) const
SE3Tpl< Scalar, Options > Transformation_t
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
std::string shortname() const
void setIndexes(JointIndex id, int nq, int nv)
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
JointCollectionTpl< _Scalar, _Options > JointCollection
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Common traits structure to fully define base classes for CRTP.
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
ConstraintTypeConstRef S() const
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
JointCollectionDefault::JointModelVariant JointModelVariant
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
JointDataDerived createData() const
static MotionTpl Random()
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
ActionMatrixType toActionMatrix() const
JointCollectionTpl & model
JointTest< _Scalar, _Options, JointCollectionTpl > JointDerived
ConstraintTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)