Added JointComposite and partial unittest.

[C++] Make JointComposite part of the JointVariant. Reoganized some include to avoid circular dependencies

CMakeLists.txt

@@ -156,6 +156,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint.hpp
   multibody/joint/joint-basic-visitors.hpp
   multibody/joint/joint-basic-visitors.hxx
+  multibody/joint/joint-composite.hpp
   ) 
 
 SET(${PROJECT_NAME}_MULTIBODY_HEADERS

src/algorithm/center-of-mass.hxx

@@ -225,14 +225,37 @@ namespace se3
       ColBlock Jcols = jmodel.jointCols(data.J);
       Jcols = data.oMi[i].act(jdata.S());
       
-      if( JointModel::NV==1 )
+      if (JointModel::NV == -1)
+      {
+        if( jmodel.nv() ==1 )
+        {
         data.Jcom.col(jmodel.idx_v())
         = data.mass[i] * Jcols.template topLeftCorner<3,1>()
         - data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
+        }
+        else
+        {
+          jmodel.jointCols(data.Jcom)
+          = data.mass[i] * Jcols.template topRows<3>()
+          - skew(data.com[i]) * Jcols.template bottomRows<3>();
+        }
+      }
       else
-        jmodel.jointCols(data.Jcom)
-        = data.mass[i] * Jcols.template topRows<3>()
-        - skew(data.com[i]) * Jcols.template bottomRows<3>();
+      {
+        if( JointModel::NV ==1 )
+        {
+        data.Jcom.col(jmodel.idx_v())
+        = data.mass[i] * Jcols.template topLeftCorner<3,1>()
+        - data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
+        }
+        else
+        {
+          jmodel.jointCols(data.Jcom)
+          = data.mass[i] * Jcols.template topRows<3>()
+          - skew(data.com[i]) * Jcols.template bottomRows<3>();
+        }
+      }
+      
     
       if(computeSubtreeComs)
         data.com[i] /= data.mass[i];

src/algorithm/crba.hxx

@@ -161,7 +161,7 @@ namespace se3
                      const se3::Model & model,
                      se3::Data & data)
     {
-      typedef typename Data::Matrix6x::NColsBlockXpr<JointModel::NV>::Type ColsBlock;
+      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
       
       const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
       const Model::Index & parent = model.parents[i];
@@ -169,8 +169,16 @@ namespace se3
       data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
       
       jdata.U() = data.Ycrb[i] * jdata.S();
-      ColsBlock jF
-      = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
+      if (JointModel::NV == -1)
+      {
+        ColsBlock jF
+        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
+      }
+      else
+      {
+        ColsBlock jF
+        = data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
+      }
       forceSet::se3Action(data.oMi[i],jdata.U(),jF);
     }
     

src/algorithm/joint-configuration.hpp

@@ -146,7 +146,6 @@ namespace se3
                      const Eigen::VectorXd & v,
                      Eigen::VectorXd & result) 
     {
-     
       jmodel.jointConfigSelector(result) = jmodel.integrate(q, v);
     }
 

src/multibody/constraint.hpp

@@ -181,6 +181,11 @@ namespace se3
       return (m.toActionMatrix()*S).eval();
     }
     
+    DenseBase se3ActionInverse(const SE3 & m) const
+    {
+      return (m.inverse().toActionMatrix()*S).eval();
+    }
+
     void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
 
   protected:

src/multibody/joint/joint-composite.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_joint_composite_hpp__
+#define __se3_joint_composite_hpp__
+
+#include "pinocchio/assert.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+
+namespace se3
+{
+
+  struct JointComposite;
+  struct JointModelComposite;
+  struct JointDataComposite;
+
+  template<>
+  struct traits<JointComposite>
+  {
+
+    enum {
+      NQ = Eigen::Dynamic,
+      NV = Eigen::Dynamic
+    };
+    typedef JointDataComposite JointData;
+    typedef JointModelComposite JointModel;
+    typedef ConstraintXd Constraint_t;
+    typedef SE3 Transformation_t;
+    typedef Motion Motion_t;
+    typedef Motion Bias_t;
+
+    typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
+    // [ABA]
+    typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
+    typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
+    typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;
+
+    typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
+    typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
+  };
+  template<> struct traits<JointDataComposite> { typedef JointComposite Joint; };
+  template<> struct traits<JointModelComposite> { typedef JointComposite Joint; };
+
+  struct JointDataComposite : public JointDataBase<JointDataComposite> 
+  {
+    typedef JointComposite Joint;
+    SE3_JOINT_TYPEDEF;
+
+    JointDataVector joints;
+    int nq_composite,nv_composite;
+
+    Constraint_t S;
+    Transformation_t M;
+    Motion_t v;
+    Bias_t c;
+
+    
+    // // [ABA] specific data
+    U_t U;
+    D_t Dinv;
+    UD_t UDinv;
+
+
+    // JointDataComposite()  {} // can become necessary if we want a vector of JointDataComposite ?
+    JointDataComposite( JointDataVector & joints, int nq, int nv )
+    : joints(joints)
+    , nq_composite(nq)
+    , nv_composite(nv)
+    , S(Eigen::MatrixXd::Zero(6, nv_composite))
+    , M(Transformation_t::Identity())
+    , v(Motion_t::Zero())
+    , c(Bias_t::Zero())
+    , U(Eigen::MatrixXd::Zero(6, nv_composite))
+    , Dinv(Eigen::MatrixXd::Zero(nv_composite, nv_composite))
+    , UDinv(Eigen::MatrixXd::Zero(6, nv_composite))
+    {}
+
+  };
+
+  struct JointModelComposite : public JointModelBase<JointModelComposite> 
+  {
+    typedef JointComposite Joint;
+    SE3_JOINT_TYPEDEF;
+    SE3_JOINT_USE_INDEXES;
+    using JointModelBase<JointModelComposite>::id;
+    using JointModelBase<JointModelComposite>::setIndexes;
+
+    std::size_t max_joints;
+    JointModelVector joints;
+    int nq_composite,nv_composite;
+
+    // Same as JointModelComposite(1)
+    JointModelComposite() : max_joints(1)
+                          , joints(0)
+                          , nq_composite(0)
+                          , nv_composite(0)
+                          {} 
+    JointModelComposite(std::size_t max_number_of_joints) : max_joints(max_number_of_joints)
+                                                          , joints(0)
+                                                          , nq_composite(0)
+                                                          , nv_composite(0)
+                                                          {} 
+    
+    template <typename D1>
+    JointModelComposite(const JointModelBase<D1> & jmodel1) : max_joints(1)
+                                                            , joints(0)
+                                                            , nq_composite(jmodel1.nq())
+                                                            , nv_composite(jmodel1.nv())
+    {
+        joints.push_back(jmodel1.derived());
+    }
+
+    template <typename D1, typename D2 >
+    JointModelComposite(const JointModelBase<D1> & jmodel1, const JointModelBase<D2> & jmodel2)
+    : max_joints(2)
+    , joints(0)
+    , nq_composite(jmodel1.nq() + jmodel2.nq())
+    , nv_composite(jmodel1.nv() + jmodel2.nv())
+    {
+      joints.push_back(jmodel1.derived());
+      joints.push_back(jmodel2.derived());
+    }
+    
+    template <typename D1, typename D2, typename D3 >
+    JointModelComposite(const JointModelBase<D1> & jmodel1,
+                        const JointModelBase<D2> & jmodel2,
+                        const JointModelBase<D3> & jmodel3)
+    : max_joints(3)
+    , joints(0)
+    , nq_composite(jmodel1.nq() + jmodel2.nq() + jmodel3.nq())
+    , nv_composite(jmodel1.nv() + jmodel2.nv() + jmodel3.nv())
+    {
+      joints.push_back(jmodel1.derived());
+      joints.push_back(jmodel2.derived());
+      joints.push_back(jmodel3.derived());
+    }
+
+    // JointModelComposite( const JointModelVector & models ) : max_joints(models.size()) , joints(models) {}
+    
+    template < typename D>
+    std::size_t addJointModel(const JointModelBase<D> & jmodel)
+    {
+      std::size_t nb_joints = joints.size();
+      if (!isFull())
+      {
+        joints.push_back(jmodel.derived());
+        nq_composite += jmodel.nq();
+        nv_composite += jmodel.nv();
+        nb_joints = joints.size();
+      }
+      return nb_joints;
+    }
+
+    bool isFull() const
+    {
+      return joints.size() == max_joints ; 
+    }
+
+    bool isEmpty() const
+    {
+      return joints.size() <= 0 ; 
+    }
+    JointData createData() const
+    {
+      JointDataVector res;
+      for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        res.push_back(::se3::createData(*i));
+      }
+      return JointData(res, nq_composite, nv_composite);
+    }
+
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs) const
+    {
+      data.M.setIdentity();
+      for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+      {
+        JointDataVector::iterator::difference_type index = i - data.joints.begin();
+        calc_zero_order(joints[(std::size_t)index], *i, qs);
+        data.M =  data.M * joint_transform(*i);
+      }
+    }
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs,
+               const Eigen::VectorXd & vs ) const
+    {
+      data.M.setIdentity();
+      data.v.setZero();
+      for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+      {
+        JointDataVector::iterator::difference_type index = i - data.joints.begin();
+        calc_first_order(joints[(std::size_t)index], *i, qs, vs);
+        data.M =  data.M * joint_transform(*i);
+      }
+
+      SE3 iMcomposite(SE3::Identity());
+      data.v = motion(data.joints[joints.size()-1]);
+      int start_col = nv_composite;
+      int sub_constraint_dimension = (int)constraint_xd(data.joints[joints.size()-1]).matrix().cols();
+
+      for (JointDataVector::reverse_iterator i = data.joints.rbegin()+1; i != data.joints.rend(); ++i)
+      {
+        sub_constraint_dimension = (int)constraint_xd(*i).matrix().cols();
+        start_col -= sub_constraint_dimension;
+
+        iMcomposite = joint_transform(*(i-1)) * iMcomposite;
+        data.v += iMcomposite.actInv(motion(*i));
+        data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*i));
+
+        // Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
+        // data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
+      }
+    }
+   
+    
+    void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
+    {
+    // calc has been called previously in abaforwardstep1 so data.M, data.v are up to date
+      data.U.setZero();
+      data.Dinv.setZero();
+      data.UDinv.setZero();
+      for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
+      {
+        JointDataVector::iterator::difference_type index = i - data.joints.begin();
+        ::se3::calc_aba(joints[(std::size_t)index], *i, I, false);
+      }
+      data.U = I * data.S;
+      Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
+      data.Dinv = tmp.inverse();
+      data.UDinv = data.U * data.Dinv;
+
+      if (update_I)
+        I -= data.UDinv * data.U.transpose();
+    }
+
+    ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
+    {
+      ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+      for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::integrate(*i,qs,vs);
+      }
+      return result;
+    }
+
+    ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
+    {
+      ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+      for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::interpolate(*i,q0,q1,u);
+      }
+      return result;
+    }
+
+    ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
+    { 
+      ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+      for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        result.segment(::se3::idx_q(*i),::se3::nq(*i)) += 
+                                ::se3::randomConfiguration(*i,
+                                                          lower_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i)),
+                                                          upper_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i))
+                                                          );
+      }
+      return result;
+    } 
+
+    TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+    { 
+      TangentVector_t result(Eigen::VectorXd::Zero(nv_composite));
+      for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        result.segment(::se3::idx_v(*i),::se3::nv(*i)) += ::se3::difference(*i,q0,q1);
+      }
+      return result;
+    } 
+
+    double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+    { 
+      return difference_impl(q0,q1).norm();
+    }
+
+    int     nv_impl() const { return nv_composite; }
+    int     nq_impl() const { return nq_composite; }
+
+
+    // Be sure to call it AFTER setIndexes
+    void updateComponentsIndexes()
+    {
+      int current_idx_q, current_idx_v;
+      int next_idx_q = idx_q();
+      int next_idx_v = idx_v();
+
+      for (JointModelVector::iterator i = joints.begin(); i != joints.end(); ++i)
+      {
+        current_idx_q = next_idx_q;
+        current_idx_v = next_idx_v;
+        ::se3::setIndexes(*i,id(),current_idx_q, current_idx_v);
+        next_idx_q = current_idx_q + ::se3::nq(*i);
+        next_idx_v = current_idx_v + ::se3::nv(*i);
+      }
+    }
+
+    static const std::string shortname()
+    {
+      return std::string("JointModelComposite");
+    }
+
+    template <class D>
+    bool operator == (const JointModelBase<D> &) const
+    {
+      return false;
+    }
+    
+    bool operator == (const JointModelBase<JointModelComposite> & jmodel) const
+    {
+      return jmodel.id() == id()
+              && jmodel.idx_q() == idx_q()
+              && jmodel.idx_v() == idx_v();
+    }
+
+
+template<typename D>
+    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
+    jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+    template<typename D>
+    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
+    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
+    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite);  }
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::Type
+    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite);  }
+
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
+    jointCols(const Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite);  }
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::Type 
+    jointCols(Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite);  }
+
+template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+  jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+  jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+  jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+  jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType 
+  jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type 
+  jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+
+  };
+  
+
+    inline std::ostream & operator << (std::ostream & os, const JointModelComposite & jmodel)
+    {
+      os << "JointModelComposite containing following models:\n" ;
+      for (JointModelVector::const_iterator i = jmodel.joints.begin(); i != jmodel.joints.end(); ++i)
+      {
+        os << shortname(*i) << std::endl;
+      }
+      return os;
+    }
+
+} // namespace se3
+
+
+#endif // ifndef __se3_joint_composite_hpp__

src/multibody/joint/joint-variant.hpp

@@ -33,14 +33,27 @@
 
 #include <Eigen/StdVector>
 #include <boost/variant.hpp>
+#include <boost/variant/recursive_wrapper.hpp>
 
 namespace se3
 {
   enum { MAX_JOINT_NV = 6 };
 
-  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation, JointModelDense<-1,-1>, JointModelRUBX, JointModelRUBY, JointModelRUBZ > JointModelVariant;
-  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1>, JointDataRUBX, JointDataRUBY, JointDataRUBZ > JointDataVariant;
-  
+  struct JointComposite;
+  struct JointModelComposite;
+  struct JointDataComposite;
+
+  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical,
+                          JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ,
+                          JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation,
+                          JointModelDense<-1,-1> ,JointModelRUBX, JointModelRUBY, JointModelRUBZ,
+                          boost::recursive_wrapper<JointModelComposite> >JointModelVariant;
+  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical,
+                          JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned,
+                          JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1>,
+                          JointDataRUBX, JointDataRUBY, JointDataRUBZ,
+                          boost::recursive_wrapper<JointDataComposite> > JointDataVariant;
+
 } // namespace se3
 
 #endif // ifndef __se3_joint_variant_hpp__

src/multibody/model.hpp

@@ -27,7 +27,9 @@
 #include "pinocchio/multibody/fwd.hpp"
 #include "pinocchio/multibody/frame.hpp"
 #include "pinocchio/multibody/joint/joint.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
 #include "pinocchio/deprecated.hh"
+#include "pinocchio/tools/string-generator.hpp"
 
 #include <iostream>
 #include <Eigen/Cholesky>

src/multibody/model.hxx

@@ -67,14 +67,14 @@ namespace se3
     assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
            &&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
     assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
-    
+
     assert(max_effort.size() == joint_model.nv()
            && max_velocity.size() == joint_model.nv()
            && min_config.size() == joint_model.nq()
            && max_config.size() == joint_model.nq());
-    
+
     Model::JointIndex idx = (Model::JointIndex) (njoint++);
-    
+
     joints         .push_back(JointModel(joint_model.derived()));
     boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
     
@@ -84,11 +84,21 @@ namespace se3
     names          .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
     nq += joint_model.nq();
     nv += joint_model.nv();
-    
-    effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = max_effort;
-    velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = max_velocity;
-    lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = min_config;
-    upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = max_config;
+
+    if (D::NV == -1)
+    {
+      effortLimit.conservativeResize(nv);effortLimit.bottomRows(j.nv()) = max_effort;
+      velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(j.nv()) = max_velocity;
+      lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(j.nq()) = min_config;
+      upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(j.nq()) = max_config;
+    }
+    else
+    {
+      effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = max_effort;
+      velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = max_velocity;
+      lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = min_config;