model: add nqs, idx_qs, nvs and idx_vs fields to ModelTpl

src/algorithm/check.hxx

@@ -127,6 +127,16 @@ namespace pinocchio
     CHECK_DATA( (int)data.parents_fromRow.size()   == model.nv );
     CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );
 
+    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+    {
+      const typename Model::JointModel & jmodel = model.joints[joint_id];
+      
+      CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
+      CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
+      CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
+      CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
+    }
+    
     for( JointIndex j=1;int(j)<model.njoints;++j )
     {
       JointIndex c = (JointIndex)data.lastChild[j];

src/multibody/model.hpp

@@ -94,6 +94,18 @@ namespace pinocchio
     /// \brief Model of joint *i*, encapsulated in a JointModelAccessor.
     JointModelVector joints;
     
+    /// \brief Starting index of the joint *i* in the configuration space
+    std::vector<int> idx_qs;
+    
+    /// \brief Dimension of the joint *i* configuration subspace
+    std::vector<int> nqs;
+    
+    /// \brief Starting index of the joint *i* in the tangent configuration space
+    std::vector<int> idx_vs;
+    
+    /// \brief Dimension of the joint *i* tangent subspace
+    std::vector<int> nvs;
+    
     /// \brief Joint parent of joint *i*, denoted *li* (li==parents[i]).
     std::vector<JointIndex> parents;
 
@@ -157,6 +169,10 @@ namespace pinocchio
     , inertias(1, Inertia::Zero())
     , jointPlacements(1, SE3::Identity())
     , joints(1)
+    , idx_qs(1,0)
+    , nqs(1,0)
+    , idx_vs(1,0)
+    , nvs(1,0)
     , parents(1, 0)
     , names(1)
     , supports(1,IndexVector(1,0))
@@ -187,8 +203,13 @@ namespace pinocchio
       res.gravity = gravity.template cast<NewScalar>();
       res.name = name;
       
-      /// Eigen Vectors
-      /// TODO: remove this pragma when neutralConfiguration will be removed
+      res.idx_qs = idx_qs;
+      res.nqs = nqs;
+      res.idx_vs = idx_vs;
+      res.nvs = nvs;
+      
+      // Eigen Vectors
+      // TODO: remove this pragma when neutralConfiguration will be removed
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
       res.neutralConfiguration = neutralConfiguration.template cast<NewScalar>();
@@ -209,8 +230,7 @@ namespace pinocchio
         rit++;
       }
         
-
-      /// reserve vectors
+      // reserve vectors
       res.inertias.resize(inertias.size());
       res.jointPlacements.resize(jointPlacements.size());
       res.joints.resize(joints.size());
@@ -250,7 +270,13 @@ namespace pinocchio
       && other.subtrees == subtrees
       && other.gravity == gravity
       && other.name == name;
-
+      
+      res &=
+         other.idx_qs == idx_qs
+      && other.nqs == nqs
+      && other.idx_vs == idx_vs
+      && other.nvs == nvs;
+      
       /// TODO: remove this pragma when neutralConfiguration will be removed
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
@@ -468,7 +494,8 @@ namespace pinocchio
     ///
     /// \return Name of the joint.
     ///
-    PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
+    PINOCCHIO_DEPRECATED
+    const std::string & getJointName(const JointIndex index) const;
 
     ///
     /// \brief Returns the index of a frame given by its name.
@@ -508,7 +535,7 @@ namespace pinocchio
     ///
     int addFrame(const Frame & frame);
 
-    /// Check the validity of the attributes of Model with respect to the specification of some
+    /// \brief Check the validity of the attributes of Model with respect to the specification of some
     /// algorithms.
     ///
     /// The method is a template so that the checkers can be defined in each algorithms.
@@ -522,7 +549,7 @@ namespace pinocchio
     /// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
     inline bool check() const;
     
-    /// Run checkData on data and current model.
+    /// \brief Run checkData on data and current model.
     ///
     /// \param[in] data to be checked wrt *this.
     ///

src/multibody/model.hxx

@@ -83,17 +83,23 @@ namespace pinocchio
     JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
     jmodel.setIndexes(idx,nq,nv);
     
-    assert(jmodel.idx_q() >= 0);
-    assert(jmodel.idx_v() >= 0);
+    const int joint_nq = jmodel.nq();
+    const int joint_idx_q = jmodel.idx_q();
+    const int joint_nv = jmodel.nv();
+    const int joint_idx_v = jmodel.idx_v();
+    
+    assert(joint_idx_q >= 0);
+    assert(joint_idx_v >= 0);
 
     inertias       .push_back(Inertia::Zero());
     parents        .push_back(parent);
     jointPlacements.push_back(joint_placement);
     names          .push_back(joint_name);
-    nq += joint_model.nq();
-    nv += joint_model.nv();
+    
+    nq += joint_nq; nqs.push_back(joint_nq); idx_qs.push_back(joint_idx_q);
+    nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v);
 
-    if(joint_model.nq() > 0 && joint_model.nv() > 0)
+    if(joint_nq > 0 && joint_nv > 0)
     {
       effortLimit.conservativeResize(nv);
       jmodel.jointVelocitySelector(effortLimit) = max_effort;
@@ -104,7 +110,7 @@ namespace pinocchio
       upperPositionLimit.conservativeResize(nq);
       jmodel.jointConfigSelector(upperPositionLimit) = max_config;
       
-      /// TODO: remove this pragma when neutralConfiguration will be removed
+      // TODO: remove this pragma when neutralConfiguration will be removed
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
       neutralConfiguration.conservativeResize(nq);

unittest/model.cpp

@@ -67,6 +67,24 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
     }
   }
 
+  BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions)
+  {
+    Model model;
+    buildModels::humanoidRandom(model);
+
+    // Check that i ends supports[i]
+    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
+    {
+      const Model::JointModel & jmodel = model.joints[joint_id];
+      
+      BOOST_CHECK(model.nqs[joint_id] == jmodel.nq());
+      BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q());
+      
+      BOOST_CHECK(model.nvs[joint_id] == jmodel.nv());
+      BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v());
+    }
+  }
+
   BOOST_AUTO_TEST_CASE(comparison)
   {
     Model model;