Merge pull request #399 from florent-lamiraux/devel

Lie group operation struct are aware of the dimension of the element they manipulate

src/algorithm/joint-configuration.hpp

@@ -433,8 +433,8 @@ namespace se3
                     const Eigen::VectorXd & lowerLimits,
                     const Eigen::VectorXd & upperLimits)
     {
-      LieGroup_t::template operation<JointModel>::type
-        ::randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
+      typename LieGroup_t::template operation<JointModel>::type a;
+        a.randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
                               jmodel.jointConfigSelector(upperLimits),
                               jmodel.jointConfigSelector(q));
     }

src/multibody/liegroup/cartesian-product.hpp

@@ -41,6 +41,20 @@ namespace se3
     typedef CartesianProductOperation<LieGroup1, LieGroup2>  LieGroupDerived;
     SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
 
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const
+    {
+      return NQ;
+    }
+    /// Get dimension of Lie Group tangent space
+    Index nv () const
+    {
+      return NV;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -70,21 +84,26 @@ namespace se3
     }
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
-      LieGroup1::random(out.template head<LieGroup1::NQ>());
-      LieGroup2::random(out.template tail<LieGroup2::NQ>());
+      LieGroup1 ().random(out.template head<LieGroup1::NQ>());
+      LieGroup2 ().random(out.template tail<LieGroup2::NQ>());
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
-                                         const Eigen::MatrixBase<ConfigR_t> & upper,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
+                                  const Eigen::MatrixBase<ConfigR_t> & upper,
+                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const
     {
       ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
-      LieGroup1::randomConfiguration(lower.template head<LieGroup1::NQ>(), upper.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NQ>());
-      LieGroup2::randomConfiguration(lower.template tail<LieGroup2::NQ>(), upper.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NQ>());
+      LieGroup1 ().randomConfiguration(lower.template head<LieGroup1::NQ>(),
+                                       upper.template head<LieGroup1::NQ>(),
+                                       out.template head<LieGroup1::NQ>());
+      LieGroup2 ().randomConfiguration(lower.template tail<LieGroup2::NQ>(),
+                                       upper.template tail<LieGroup2::NQ>(),
+                                       out.template tail<LieGroup2::NQ>());
     }
 
     template <class ConfigL_t, class ConfigR_t>

src/multibody/liegroup/operation-base.hpp

@@ -127,7 +127,7 @@ namespace se3
      * @return     The joint configuration
      */
     template <class Config_t>
-    static void random (const Eigen::MatrixBase<Config_t>& qout);
+    void random (const Eigen::MatrixBase<Config_t>& qout) const;
 
     /**
      * @brief      Generate a configuration vector uniformly sampled among
@@ -139,9 +139,10 @@ namespace se3
      * @return     The joint configuration
      */
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
-                                    const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
-                                    const Eigen::MatrixBase<ConfigOut_t> & qout);
+    void randomConfiguration
+    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
+     const Eigen::MatrixBase<ConfigOut_t> & qout) const;
 
     /**
      * @brief      the tangent vector that must be integrated during one unit time to go from q0 to q1
@@ -204,11 +205,12 @@ namespace se3
                                       const Eigen::MatrixBase<ConfigR_t> & q1,
                                       const Scalar& u);
 
-    static ConfigVector_t random();
+    ConfigVector_t random() const;
 
     template <class ConfigL_t, class ConfigR_t>
-    static ConfigVector_t randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
-                                              const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit);
+    ConfigVector_t randomConfiguration
+    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
 
     template <class ConfigL_t, class ConfigR_t>
     static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
@@ -233,7 +235,23 @@ namespace se3
     static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                          const Eigen::MatrixBase<ConfigR_t> & q1,
                                          const Scalar & prec);
-
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const;
+    /// Get dimension of Lie Group tangent space
+    Index nv () const;
+
+    Derived& derived ()
+    {
+      return static_cast <Derived&> (*this);
+    }
+
+    const Derived& derived () const
+    {
+      return static_cast <const Derived&> (*this);
+    }
     /// \}
 
   protected:

src/multibody/liegroup/operation-base.hxx

@@ -71,8 +71,8 @@ namespace se3 {
    */
   template <class Derived>
   template <class Config_t>
-  void LieGroupOperationBase<Derived>::random(
-      const Eigen::MatrixBase<Config_t>& qout)
+  void LieGroupOperationBase<Derived>::random
+  (const Eigen::MatrixBase<Config_t>& qout) const
   {
     EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
     return Derived::random_impl (qout);
@@ -83,12 +83,12 @@ namespace se3 {
   void LieGroupOperationBase<Derived>::randomConfiguration(
       const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
       const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
-      const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const Eigen::MatrixBase<ConfigOut_t> & qout) const
   {
     EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t  , ConfigVector_t);
     EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t  , ConfigVector_t);
     EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
-    Derived::randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
+    derived ().randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
   }
 
   template <class Derived>
@@ -163,7 +163,8 @@ namespace se3 {
   }
 
   template <class Derived>
-  typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::random()
+  typename LieGroupOperationBase<Derived>::ConfigVector_t
+  LieGroupOperationBase<Derived>::random() const
   {
     ConfigVector_t qout;
     random(qout);
@@ -172,9 +173,10 @@ namespace se3 {
 
   template <class Derived>
   template <class ConfigL_t, class ConfigR_t>
-  typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::randomConfiguration(
-      const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
-      const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
+  typename LieGroupOperationBase<Derived>::ConfigVector_t
+  LieGroupOperationBase<Derived>::randomConfiguration
+  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+   const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const
   {
     ConfigVector_t qout;
     randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
@@ -226,6 +228,20 @@ namespace se3 {
   {
     return q0.isApprox(q1, prec);
   }
+
+  template <class Derived>
+  typename LieGroupOperationBase <Derived>::Index
+  LieGroupOperationBase <Derived>::nq () const
+  {
+    return Derived::nq ();
+  }
+
+  template <class Derived>
+  typename LieGroupOperationBase <Derived>::Index
+  LieGroupOperationBase <Derived>::nv () const
+  {
+    return Derived::nv ();
+  }
 } // namespace se3
 
 #endif // __se3_lie_group_operation_base_hxx__

src/multibody/liegroup/special-euclidean.hpp

@@ -61,6 +61,20 @@ namespace se3
     typedef SpecialEuclideanOperation LieGroupDerived;
     SE3_LIE_GROUP_TYPEDEF;
 
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const
+    {
+      return NQ;
+    }
+    /// Get dimension of Lie Group tangent space
+    Index nv () const
+    {
+      return NV;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -146,17 +160,18 @@ namespace se3
                                        // const Eigen::MatrixBase<ConfigR_t> & q1)
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       R2crossSO2_t::random(qout);
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
-                                         const Eigen::MatrixBase<ConfigR_t> & upper,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
+                                  const Eigen::MatrixBase<ConfigR_t> & upper,
+                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const
     {
-      R2crossSO2_t::randomConfiguration(lower, upper, qout);
+      R2crossSO2_t ().randomConfiguration(lower, upper, qout);
     }
 
     template <class ConfigL_t, class ConfigR_t>
@@ -193,6 +208,20 @@ namespace se3
     typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
     typedef SE3 Transformation_t;
 
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const
+    {
+      return NQ;
+    }
+    /// Get dimension of Lie Group tangent space
+    Index nv () const
+    {
+      return NV;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -243,17 +272,18 @@ namespace se3
     }
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       R3crossSO3_t::random(qout);
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
-                                         const Eigen::MatrixBase<ConfigR_t> & upper,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
+                                  const Eigen::MatrixBase<ConfigR_t> & upper,
+                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const
     {
-      R3crossSO3_t::randomConfiguration(lower, upper, qout);
+      R3crossSO3_t ().randomConfiguration(lower, upper, qout);
     }
 
     template <class ConfigL_t, class ConfigR_t>

src/multibody/liegroup/special-orthogonal.hpp

@@ -55,6 +55,20 @@ namespace se3
     typedef SpecialOrthogonalOperation LieGroupDerived;
     SE3_LIE_GROUP_TYPEDEF;
 
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const
+    {
+      return NQ;
+    }
+    /// Get dimension of Lie Group tangent space
+    Index nv () const
+    {
+      return NV;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -121,7 +135,7 @@ namespace se3
                                        // const Eigen::MatrixBase<ConfigR_t> & q1)
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       Config_t& out = (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived();
       const Scalar angle = -PI + 2*PI * ((Scalar)rand())/RAND_MAX;
@@ -129,9 +143,10 @@ namespace se3
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
-                                         const Eigen::MatrixBase<ConfigR_t> &,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
+                                  const Eigen::MatrixBase<ConfigR_t> &,
+                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const
     {
       random_impl(qout);
     }
@@ -147,6 +162,20 @@ namespace se3
     typedef Eigen::Map<      Quaternion_t> QuaternionMap_t;
     typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
 
+    /// Get dimension of Lie Group vector representation
+    ///
+    /// For instance, for SO(3), the dimension of the vector representation is
+    /// 4 (quaternion) while the dimension of the tangent space is 3.
+    Index nq () const
+    {
+      return NQ;
+    }
+    /// Get dimension of Lie Group tangent space
+    Index nv () const
+    {
+      return NV;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -198,7 +227,7 @@ namespace se3
     }
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       QuaternionMap_t out (
           (const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived().data()
@@ -207,9 +236,10 @@ namespace se3
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
-                                         const Eigen::MatrixBase<ConfigR_t> &,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
+                                  const Eigen::MatrixBase<ConfigR_t> &,
+                                  const Eigen::MatrixBase<ConfigOut_t> & qout)
+      const
     {
       random_impl(qout);
     }

src/multibody/liegroup/vector-space.hpp

@@ -39,8 +39,27 @@ namespace se3
   struct VectorSpaceOperation : public LieGroupOperationBase <VectorSpaceOperation<Size> >
   {
     typedef VectorSpaceOperation<Size>  LieGroupDerived;
+
     SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
 
+    /// Constructor
+    /// \param size size of the vector space: should be the equal to template
+    ///        argument for static sized vector-spaces.
+    VectorSpaceOperation (int size = Size) : size_ (size)
+    {
+      assert (size_ >= 0);
+      assert (Size == Eigen::Dynamic || size_ == Size);
+    }
+
+    Index nq () const
+    {
+      return size_;
+    }
+    Index nv () const
+    {
+      return size_;
+    }
+
     template <class ConfigL_t, class ConfigR_t, class Tangent_t>
     static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                 const Eigen::MatrixBase<ConfigR_t> & q1,
@@ -62,18 +81,19 @@ namespace se3
                                        // const Eigen::MatrixBase<ConfigR_t> & q1)
 
     template <class Config_t>
-    static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
+    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
     {
       qout.setRandom();
     }
 
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
-    static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
-                                         const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
-                                         const Eigen::MatrixBase<ConfigOut_t> & qout)
+    void randomConfiguration_impl
+    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
+     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
+     const Eigen::MatrixBase<ConfigOut_t> & qout) const
     {
       ConfigOut_t& res = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
-      for (int i = 0; i < NQ; ++i)
+      for (int i = 0; i < nq (); ++i)
       {
         if(lower_pos_limit[i] == -std::numeric_limits<Scalar>::infinity() ||
            upper_pos_limit[i] ==  std::numeric_limits<Scalar>::infinity() )
@@ -86,6 +106,8 @@ namespace se3
         res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
       }
     }
+  private:
+    Index size_;
   }; // struct VectorSpaceOperation
 
 } // namespace se3

unittest/liegroups.cpp

@@ -54,9 +54,10 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
 
     BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(LieGroupType::integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
     BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(LieGroupType::interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
-    BOOST_CHECK_MESSAGE(jmodel.randomConfiguration( -1 * Ones, Ones).size()
-                        == LieGroupType::randomConfiguration(-1 * Ones, Ones).size(),
-                        std::string(error_prefix + " - RandomConfiguration dimensions "));
+    BOOST_CHECK_MESSAGE
+      (jmodel.randomConfiguration( -1 * Ones, Ones).size() ==
+       LieGroupType().randomConfiguration(-1 * Ones, Ones).size(),
+       std::string(error_prefix + " - RandomConfiguration dimensions "));
     BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(LieGroupType::difference(q1,q2)) ,std::string(error_prefix + " - difference "));
     BOOST_CHECK_MESSAGE(fabs(jmodel.distance(q1,q2) - LieGroupType::distance(q1,q2)) < 1e-12 ,std::string(error_prefix + " - distance "));
 }
@@ -122,7 +123,7 @@ BOOST_AUTO_TEST_CASE ( test_vector_space )
 
   bool error = false;
   try {
-    VSO_t::randomConfiguration(lo, up, q);
+    VSO_t ().randomConfiguration(lo, up, q);
   } catch (const std::runtime_error&) {
     error = true;
   }