Merge pull request #358 from jcarpent/devel

Add default value to JointModelBase indexes

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));
     }
     

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

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 ()