Merge pull request #162 from fvalenza/devel

Added JointAccessor + its unittests.

CMakeLists.txt

@@ -120,6 +120,9 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-free-flyer.hpp
   multibody/joint/joint-variant.hpp
   multibody/joint/joint-generic.hpp
+  multibody/joint/joint-accessor.hpp
+  multibody/joint/joint-basic-visitors.hpp
+  multibody/joint/joint-basic-visitors.hxx
   ) 
 
 SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS

src/assert.hpp

@@ -24,7 +24,14 @@ namespace se3
   {
 
     template<bool condition>
-    struct static_assertion {};
+    struct static_assertion
+    {
+      enum
+      {
+        THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS,
+        MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE
+      };
+    };
     
     template<>
     struct static_assertion<true>

src/multibody/joint/joint-accessor.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_accessor_hpp__
+#define __se3_joint_accessor_hpp__
+
+#include "pinocchio/assert.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+
+namespace se3
+{
+
+  struct JointAccessor;
+  struct JointModelAccessor;
+  struct JointDataAccessor;
+
+  template<>
+  struct traits<JointAccessor>
+  {
+
+    enum {
+      NQ = -1, // Dynamic because unknown at compilation
+      NV = -1
+    };
+    typedef JointDataAccessor JointData;
+    typedef JointModelAccessor 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<JointDataAccessor> { typedef JointAccessor Joint; };
+  template<> struct traits<JointModelAccessor> { typedef JointAccessor Joint; };
+
+  struct JointDataAccessor : public JointDataBase<JointDataAccessor> 
+  {
+    typedef JointAccessor Joint;
+    SE3_JOINT_TYPEDEF;
+
+
+    JointDataVariant & j_data_variant;
+
+    const Constraint_t      S() const  { return constraint_xd(j_data_variant); }
+    const Transformation_t  M() const  { return joint_transform(j_data_variant); } // featherstone book, page 78 (sXp)
+    const Motion_t          v() const  { return motion(j_data_variant); }
+    const Bias_t            c() const  { return bias(j_data_variant); }
+    
+    // // [ABA CCRBA]
+    const U_t               U()     const { return u_inertia(j_data_variant); }
+    U_t                     U()           { return u_inertia(j_data_variant); }
+    const D_t               Dinv()  const { return dinv_inertia(j_data_variant); }
+    const UD_t              UDinv() const { return udinv_inertia(j_data_variant); }
+
+
+    // JointDataAccessor()  {} // can become necessary if we want a vector of jointDataAccessor ?
+    JointDataAccessor( JointDataVariant & jdata ) : j_data_variant(jdata){}
+
+  };
+
+  struct JointModelAccessor : public JointModelBase<JointModelAccessor> 
+  {
+    typedef JointAccessor Joint;
+    SE3_JOINT_TYPEDEF;
+    SE3_JOINT_USE_INDEXES;
+    using JointModelBase<JointModelAccessor>::id;
+    using JointModelBase<JointModelAccessor>::setIndexes;
+
+    const JointModelVariant & j_model_variant;
+
+    JointDataVariant createData() 
+    {
+      return ::se3::createData(j_model_variant);
+    }
+
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs) const
+    {
+      calc_zero_order(j_model_variant, data.j_data_variant, qs);
+
+    }
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs,
+               const Eigen::VectorXd & vs ) const
+    {
+      calc_first_order(j_model_variant, data.j_data_variant, qs, vs);
+    }
+    
+    void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
+    {
+      ::se3::calc_aba(j_model_variant, data.j_data_variant, I, update_I);
+    }
+
+    ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
+    {
+      return ::se3::integrate(j_model_variant, qs, vs);
+    }
+
+    ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
+    {
+      return ::se3::interpolate(j_model_variant, q0, q1, u);
+    }
+
+    ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
+    { 
+      return ::se3::randomConfiguration(j_model_variant, lower_pos_limit, upper_pos_limit);
+    } 
+
+    TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+    { 
+      return ::se3::difference(j_model_variant, q0, q1);
+    } 
+
+    double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
+    { 
+      return ::se3::distance(j_model_variant, q0, q1);
+    }
+
+    // JointModelAccessor() : j_model_variant() {} // can become necessary if we want a vector of jointModelAccessor ?
+    JointModelAccessor( const JointModelVariant & model_variant ) : j_model_variant(model_variant)
+    {}
+
+    int     nq_impl() const { return ::se3::nq(j_model_variant); }
+    int     nv_impl() const { return ::se3::nv(j_model_variant); }
+
+    int     idx_q()   const { return ::se3::idx_q(j_model_variant); }
+    int     idx_v()   const { return ::se3::idx_v(j_model_variant); }
+
+    JointIndex     id()      const { return ::se3::id(j_model_variant); }
+
+    void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
+  };
+  
+
+} // namespace se3
+
+
+#endif // ifndef __se3_joint_accessor_hpp__

src/multibody/joint/joint-base.hpp

@@ -260,8 +260,8 @@ namespace se3
     /**
      * @brief      Integrate joint's configuration for a tangent vector during one unit time
      *
-     * @param[in]  q     initatial configuration  (size jmodel.nq)
-     * @param[in]  v     joint velocity (size jmodel.nv)
+     * @param[in]  q     initatial configuration  (size full model.nq)
+     * @param[in]  v     joint velocity (size full model.nv)
      *
      * @return     The configuration integrated
      */

src/multibody/joint/joint-basic-visitors.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_basic_visitors_hpp__
+#define __se3_joint_basic_visitors_hpp__
+
+#include <Eigen/StdVector>
+#include <boost/variant.hpp>
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+
+namespace se3
+{
+  
+  /**
+   * @brief      Visit a JointModelVariant through CreateData visitor to create a JointDataVariant
+   *
+   * @param[in]  jmodel  The JointModelVariant we want to create a data for
+   *
+   * @return     The created JointDataVariant
+   */
+  inline JointDataVariant createData(const JointModelVariant & jmodel);
+
+  
+  /**
+   * @brief      Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcZeroOrderVisitor
+   *             to compute the joint data kinematics at order zero
+   *
+   * @param[in]  jmodel  The corresponding JointModelVariant to the JointDataVariant we want to update
+   * @param      jdata   The JointDataVariant we want to update
+   * @param[in]  q       The full model's (in which the joint belongs to) configuration vector
+   */
+  inline void calc_zero_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q);
+
+  /**
+   * @brief      Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcFirstOrderVisitor
+   *             to compute the joint data kinematics at order one
+   *
+   * @param[in]  jmodel  The corresponding JointModelVariant to the JointDataVariant we want to update
+   * @param      jdata   The JointDataVariant we want to update
+   * @param[in]  q       The full model's (in which the joint belongs to) configuration vector
+   */
+  inline void calc_first_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q, const Eigen::VectorXd & v);
+  
+  
+  /**
+   * @brief      Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcAbaVisitor to
+   * 
+   *
+   * @param[in]  jmodel  The corresponding JointModelVariant to the JointDataVariant we want to update
+   * @param      jdata   The JointDataVariant we want to update
+   * @param      I       Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix
+   * @param[in]  update_I  If I should be updated or not
+   */
+  inline void calc_aba(const JointModelVariant & jmodel, JointDataVariant & jdata, Inertia::Matrix6 & I, const bool update_I);
+
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointIntegrateVisitor to integrate joint's configuration 
+   *             for a tangent vector during one unit time
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  q       Initatial configuration  (size full model.nq)
+   * @param[in]  v       Joints velocity (size full model.nv)
+   *
+   * @return     The configuration integrated
+   */
+  inline Eigen::VectorXd integrate(const JointModelVariant & jmodel,
+                                   const Eigen::VectorXd & q,
+                                   const Eigen::VectorXd & v);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointInterpolateVisitor to compute
+   *             the interpolation between two joint's configurations
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  q0    Initial configuration to interpolate
+   * @param[in]  q1    Final configuration to interpolate
+   * @param[in]  u     u in [0;1] position along the interpolation.
+   *
+   * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
+   */
+  inline Eigen::VectorXd interpolate(const JointModelVariant & jmodel,
+                                     const Eigen::VectorXd & q0,
+                                     const Eigen::VectorXd & q1,
+                                     const double u);
+
+  
+  /**
+   * @brief       Visit a JointModelVariant through JointRandomConfigurationVisitor to
+   *              generate a configuration vector uniformly sampled among provided limits
+   *
+   * @param[in]  jmodel           The JointModelVariant
+   * @param[in]  lower_pos_limit  lower joint limit
+   * @param[in]  upper_pos_limit  upper joint limit
+   *
+   * @return     The joint configuration
+   */
+  inline Eigen::VectorXd randomConfiguration(const JointModelVariant & jmodel,
+                                             const Eigen::VectorXd & lower_pos_limit,
+                                             const Eigen::VectorXd & upper_pos_limit);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointDifferenceVisitor to compute
+   *             the tangent vector that must be integrated during one unit time to go from q0 to q1
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  q0    Initial configuration
+   * @param[in]  q1    Wished configuration
+   *
+   * @return     The corresponding velocity
+   */
+  inline Eigen::VectorXd difference(const JointModelVariant & jmodel,
+                                    const Eigen::VectorXd & q0,
+                                    const Eigen::VectorXd & q1);
+
+  
+  /**
+   * @brief       Visit a JointModelVariant through JointDifferenceVisitor to compute the distance
+   *              between two configurations
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  q0    Configuration 1
+   * @param[in]  q1    Configuration 2
+   *
+   * @return     The corresponding distance
+   */
+  inline double distance(const JointModelVariant & jmodel,
+                         const Eigen::VectorXd & q0,
+                         const Eigen::VectorXd & q1);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointNvVisitor to get the dimension of 
+   *             the joint tangent space
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   *
+   * @return     The dimension of joint tangent space
+   */
+  inline int nv(const JointModelVariant & jmodel);
+
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointNqVisitor to get the dimension of 
+   *             the joint configuration space
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   *
+   * @return     The dimension of joint configuration space
+   */
+  inline int nq(const JointModelVariant & jmodel);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointIdxQVisitor to get the index in the full model configuration
+   *             space corresponding to the first degree of freedom of the Joint
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   *
+   * @return     The index in the full model configuration space corresponding to the first
+   *             degree of freedom of jmodel
+   */
+  inline int idx_q(const JointModelVariant & jmodel);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointIdxVVisitor to get the index in the full model tangent
+   *             space corresponding to the first joint tangent space degree
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   *
+   * @return     The index in the full model tangent space corresponding to the first
+   *             joint tangent space degree
+   */
+  inline int idx_v(const JointModelVariant & jmodel);
+
+  
+  /**
+   * @brief      Visit a JointModelVariant through JointIdVisitor to get the index of the joint in the kinematic chain
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   *
+   * @return     The index of the joint in the kinematic chain
+   */
+  inline JointIndex id(const JointModelVariant & jmodel);
+
+  /**
+   * @brief      Visit a JointModelVariant through JointSetIndexesVisitor to set
+   *             the indexes of the joint in the kinematic chain
+   *
+   * @param[in]  jmodel  The JointModelVariant
+   * @param[in]  id      The index of joint in the kinematic chain
+   * @param[in]  q       The index in the full model configuration space corresponding to the first degree of freedom
+   * @param[in]  v       The index in the full model tangent space corresponding to the first joint tangent space degree
+   *
+   * @return     The index of the joint in the kinematic chain
+   */
+  inline void setIndexes(JointModelVariant & jmodel, JointIndex id, int q,int v);
+
+  //
+  // Visitors on JointDatas
+  //
+  
+  
+  /**
+   * @brief      Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint 
+   *             as a dense constraint
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The constraint dense corresponding to the joint derived constraint
+   */
+  inline ConstraintXd constraint_xd(const JointDataVariant & jdata);
+
+  /**
+   * @brief      Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform  (transform
+   *             between the entry frame and the exit frame of the joint)
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The joint transform corresponding to the joint derived transform (sXp)
+   */
+  inline SE3 joint_transform(const JointDataVariant & jdata);
+
+  /**
+   * @brief      Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion 
+   *             as a dense motion
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The motion dense corresponding to the joint derived motion
+   */
+  inline Motion motion(const JointDataVariant & jdata);
+
+  /**
+   * @brief      Visit a JointDataVariant through JointBiasVisitor to get the joint bias
+   *             as a dense motion
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The motion dense corresponding to the joint derived bias
+   */
+  inline Motion bias(const JointDataVariant & jdata);
+
+
+  /**
+   * @brief      Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix 
+   *             decomposition
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The U matrix of the inertia matrix decomposition
+   */
+  inline Eigen::Matrix<double,6,Eigen::Dynamic> u_inertia(const JointDataVariant & jdata);
+
+  /**
+   * @brief      Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix 
+   *             decomposition
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The D^{-1} matrix of the inertia matrix decomposition
+   */
+  inline Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> dinv_inertia(const JointDataVariant & jdata);
+
+  /**
+   * @brief      Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix 
+   *             decomposition
+   *
+   * @param[in]  jdata  The jdata
+   *
+   * @return     The U*D^{-1} matrix of the inertia matrix decomposition
+   */
+  inline Eigen::Matrix<double,6,Eigen::Dynamic> udinv_inertia(const JointDataVariant & jdata);
+
+} // namespace se3
+
+
+/* --- Details -------------------------------------------------------------------- */
+#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
+
+
+#endif // ifndef __se3_joint_basic_visitors_hpp__

src/multibody/joint/joint-basic-visitors.hxx

+//
+// 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_basic_visitors_hxx__
+#define __se3_joint_basic_visitors_hxx__
+
+// remove the includes ?
+#include <Eigen/StdVector>
+#include <boost/variant.hpp>
+#include "pinocchio/assert.hpp"
+#include "pinocchio/multibody/joint/joint-variant.hpp"
+
+namespace se3
+{
+  /// @cond DEV
+  
+  /**
+   * @brief      CreateJointData visitor
+   */
+  class CreateJointData: public boost::static_visitor<JointDataVariant>
+  {
+  public:
+    template<typename D>
+    JointDataVariant operator()(const JointModelBase<D> & jmodel) const
+    { return JointDataVariant(jmodel.createData()); }
+    
+    static JointDataVariant run( const JointModelVariant & jmodel)
+    { return boost::apply_visitor( CreateJointData(), jmodel ); }
+  };
+  inline JointDataVariant createData(const JointModelVariant & jmodel)
+  {
+    return CreateJointData::run(jmodel);
+  }
+
+  /**
+   * @brief      JointCalcZeroOrderVisitor
+   */
+  class JointCalcZeroOrderVisitor: public boost::static_visitor<>
+  {
+  public:
+    const Eigen::VectorXd & q;
+    
+    JointCalcZeroOrderVisitor(const Eigen::VectorXd & q) : q(q) {}
+    
+    template<typename D1, typename D2>
+    void operator()(const JointModelBase<D1> & , JointDataBase<D2> & ) const
+    { 
+      SE3_STATIC_ASSERT(false, MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE);
+    }
+
+    template<typename D>
+    void operator()(const JointModelBase<D> & jmodel, JointDataBase<D> & jdata) const
+    { 
+      jmodel.calc(jdata,q);
+    }
+    
+    static void run( const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q)
+    { boost::apply_visitor( JointCalcZeroOrderVisitor(q), jmodel, jdata ); }
+  };
+  inline void calc_zero_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q)
+  {
+    JointCalcZeroOrderVisitor::run( jmodel, jdata, q );
+  }
+
+  /**
+   * @brief      JointCalcFirstOrderVisitor
+   */
+  class JointCalcFirstOrderVisitor: public boost::static_visitor<>
+  {
+  public:
+    const Eigen::VectorXd & q;
+    const Eigen::VectorXd & v;
+    
+    JointCalcFirstOrderVisitor(const Eigen::VectorXd & q, const Eigen::VectorXd & v) : q(q), v(v) {}
+    
+    template<typename D1, typename D2>
+    void operator()(const JointModelBase<D1> & , JointDataBase<D2> & ) const
+    { 
+      SE3_STATIC_ASSERT(false, MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE);
+    }
+
+    template<typename D>
+    void operator()(const JointModelBase<D> & jmodel, JointDataBase<D> & jdata) const
+    { 
+      jmodel.calc(jdata,q,v);
+    }
+    
+    static void run( const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q, const Eigen::VectorXd & v)
+    { boost::apply_visitor( JointCalcFirstOrderVisitor(q, v), jmodel, jdata ); }
+  };