diff --git a/src/algorithm/finite-differences.hxx b/src/algorithm/finite-differences.hxx index d243ca6b3df2e3ede84f6b54af95e98b086573c1..a61503760d998ec1a34b469267088a0a257aeeeb 100644 --- a/src/algorithm/finite-differences.hxx +++ b/src/algorithm/finite-differences.hxx @@ -50,8 +50,9 @@ namespace se3 { using namespace se3::details; Eigen::VectorXd fd_increment(model.nv); - BOOST_FOREACH(const JointModel & jmodel,model.joints) + for(int k = 1; k < model.joints.size(); ++k) { + const JointModel & jmodel = model.joints[k]; FinitDiffEpsVisitor::run(jmodel,FinitDiffEpsVisitor::ArgsType(fd_increment)); } diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp index 6561fe2bfcf6ed7a401cf0a7a4c225e98809f5dd..4ce01748c8d9112356e1532f2b4410d738e3e2c8 100644 --- a/src/multibody/joint/joint-base.hpp +++ b/src/multibody/joint/joint-base.hpp @@ -22,6 +22,7 @@ #include "pinocchio/multibody/fwd.hpp" #include <Eigen/Core> +#include <limits> namespace se3 { @@ -426,12 +427,14 @@ namespace se3 /// Default constructor: protected. /// /// Prevent the construction of stand-alone JointModelBase. - inline JointModelBase() {} // TODO: default value should be set to -1 + inline JointModelBase() : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {} + /// Copy constructor: protected. /// /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting /// objects. Copy is done by calling copy operator. inline JointModelBase( const JointModelBase& clone) { *this = clone; } + /// Copy operator: protected. /// /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting diff --git a/unittest/joint.cpp b/unittest/joint.cpp index c932c1c2e60ff7cdd67302a30b1d523c6839032d..cdf4bac94e0b3a28b8aec876d619775e0ec5e941 100644 --- a/unittest/joint.cpp +++ b/unittest/joint.cpp @@ -21,6 +21,8 @@ #include <boost/test/unit_test.hpp> #include <boost/utility/binary.hpp> +using namespace se3; + template <typename T> void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata) { @@ -127,7 +129,21 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) BOOST_AUTO_TEST_CASE ( test_all_joints ) { - using namespace se3; boost::mpl::for_each<JointModelVariant::types>(TestJoint()); } + +BOOST_AUTO_TEST_CASE(test_empty_model) +{ + JointModel jmodel; + std::cout << "nq " << jmodel.nq() << std::endl; + std::cout << "nv " << jmodel.nv() << std::endl; + std::cout << "idx_q " << jmodel.idx_q() << std::endl; + std::cout << "idx_v " << jmodel.idx_v() << std::endl; + std::cout << "id " << jmodel.id() << std::endl; + std::cout << "name " << jmodel.shortname() << std::endl; + + BOOST_CHECK(jmodel.idx_q() == -1); + BOOST_CHECK(jmodel.idx_v() == -1); +} + BOOST_AUTO_TEST_SUITE_END ()