Merge pull request #587 from jcarpent/devel

{Lower,Upper} should appear as standard vector, not as ConfigVector

src/algorithm/joint-configuration.hpp

@@ -29,6 +29,7 @@ namespace se3
    * @param[in]  model   Model that must be integrated
    * @param[in]  q       Initial configuration (size model.nq)
    * @param[in]  v       Velocity (size model.nv)
+   *
    * @return     The integrated configuration (size model.nq)
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
@@ -43,6 +44,7 @@ namespace se3
    * @param[in]  q0      Initial configuration vector (size model.nq)
    * @param[in]  q1      Final configuration vector (size model.nq)
    * @param[in]  u       u in [0;1] position along the interpolation.
+   *
    * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
    */
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
@@ -58,6 +60,7 @@ namespace se3
    * @param[in]  model   Model to be differenced
    * @param[in]  q0      Initial configuration (size model.nq)
    * @param[in]  q1      Wished configuration (size model.nq)
+   *
    * @return     The corresponding velocity (size model.nv)
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
@@ -73,6 +76,7 @@ namespace se3
    * @param[in]  model      Model we want to compute the distance
    * @param[in]  q0         Configuration 0 (size model.nq)
    * @param[in]  q1         Configuration 1 (size model.nq)
+   *
    * @return     The corresponding squared distances for each joint (size model.njoints-1 = number of joints)
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
@@ -86,6 +90,7 @@ namespace se3
    * @param[in]  model      Model we want to compute the distance
    * @param[in]  q0         Configuration 0 (size model.nq)
    * @param[in]  q1         Configuration 1 (size model.nq)
+   *
    * @return     The distance between the two configurations
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
@@ -96,9 +101,11 @@ namespace se3
   /**
    * @brief      Generate a configuration vector uniformly sampled among provided limits.
    *
-   *\warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
+   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
    *
-   * @param[in]  model        Model we want to generate a configuration vector of
+   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of uniformlySample
+   *
+   * @param[in]  model        Model for which we want to generate a configuration vector.
    * @param[in]  lowerLimits  Joints lower limits
    * @param[in]  upperLimits  Joints upper limits
    *
@@ -113,10 +120,13 @@ namespace se3
   /**
    * @brief      Generate a configuration vector uniformly sampled among the joint limits of the specified Model.
    *
-   *\warning     If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit},
+   * @remarks    Limits are not taken into account for rotational transformations (typically SO(2),SO(3)), because they are by definition unbounded.
+   *
+   * @warning    If limits are infinite (no one specified when adding a body or no modification directly in my_model.{lowerPositionLimit,upperPositionLimit},
    *             exceptions may be thrown in the joint implementation of uniformlySample
    *
-   * @param[in]  model   Model we want to generate a configuration vector of
+   * @param[in]  model   Model for which we want to generate a configuration vector.
+   *
    * @return     The resulted configuration vector (size model.nq)
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
@@ -128,6 +138,7 @@ namespace se3
    *
    * @param[in]     model      Model
    * @param[in,out] q          Configuration to normalize
+   *
    */
   template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
   inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
@@ -135,7 +146,7 @@ namespace se3
   
   /**
    * @brief         Return true if the given configurations are equivalents
-   * \warning       Two configurations can be equivalent but not equally coefficient wise (e.g for quaternions)
+   * @warning       Two configurations can be equivalent but not equally coefficient wise (e.g for quaternions)
    *
    * @param[in]     model     Model
    * @param[in]     q1        The first configuraiton to compare

src/multibody/model.hpp

@@ -123,10 +123,10 @@ namespace se3
     TangentVectorType velocityLimit;
 
     /// \brief Lower joint configuration limit
-    ConfigVectorType lowerPositionLimit;
+    VectorXs lowerPositionLimit;
     
     /// \brief Upper joint configuration limit
-    ConfigVectorType upperPositionLimit;
+    VectorXs upperPositionLimit;
 
     /// \brief Vector of operational frames registered on the model.
     FrameVector frames;

src/parsers/sample-models.cpp

@@ -22,7 +22,7 @@ namespace se3
 {
   namespace buildModels
   {
-    const SE3 Id = SE3::Identity();
+    static const SE3 Id = SE3::Identity();
 
     template<typename JointModel>
     static void addJointAndBody(Model & model,
@@ -93,7 +93,11 @@ namespace se3
         addJointAndBody(model, jff, "universe", "root", Id);
       }
       else
+      {
         addJointAndBody(model, JointModelFreeFlyer(), "universe", "root", Id);
+        model.lowerPositionLimit.segment<4>(3).fill(-1.);
+        model.upperPositionLimit.segment<4>(3).fill( 1.);
+      }
 
       // lleg
       addJointAndBody(model,JointModelRX(),"root_joint","lleg1");
@@ -266,13 +270,17 @@ namespace se3
 
       /* --- Free flyer --- */
       if(usingFF)
-          ffidx = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"freeflyer_joint");
+      {
+        ffidx = model.addJoint(0,JointModelFreeFlyer(),SE3::Identity(),"freeflyer_joint");
+        model.lowerPositionLimit.segment<4>(3).fill(-1.);
+        model.upperPositionLimit.segment<4>(3).fill( 1.);
+      }
       else
-        {
-          JointModelComposite jff((JointModelTranslation()));
-          jff.addJoint(JointModelSphericalZYX());
-          ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
-        }
+      {
+        JointModelComposite jff((JointModelTranslation()));
+        jff.addJoint(JointModelSphericalZYX());
+        ffidx = model.addJoint(0,jff,SE3::Identity(),"freeflyer_joint");
+      }
       model.addJointFrame(ffidx);
 
       /* --- Lower limbs --- */

src/parsers/sample-models.hpp

@@ -89,7 +89,8 @@ namespace se3
      * \deprecated use se3::humanoid or se3::humanoidRandom instead. 
      */
     PINOCCHIO_DEPRECATED
-    inline void humanoidSimple(Model & model, bool usingFF = true) { humanoidRandom(model,usingFF); }
+    inline void humanoidSimple(Model & model, bool usingFF = true)
+    { humanoidRandom(model,usingFF); }
    
   } // namespace buildModels
 } // namespace se3