Merge pull request #310 from nmansard/topic/bugvisitor

Bug correction in joint visitor

src/multibody/joint/joint-base.hpp

@@ -197,6 +197,22 @@ namespace se3
 
     JointDataDense<NQ, NV> toDense() const  { return static_cast<const JointDataDerived*>(this)->toDense_impl();   }
 
+  protected:
+    /// Default constructor: protected.
+    /// 
+    /// Prevent the construction of stand-alone JointDataBase.
+    inline JointDataBase() {} // TODO: default value should be set to -1
+    /// Copy constructor: protected.
+    ///
+    /// Copy of stand-alone JointDataBase are prevented, but can be used from inhereting
+    /// objects. Copy is done by calling copy operator.
+    inline JointDataBase( const JointDataBase& clone) { *this = clone; }
+    /// Copy operator: protected.
+    ///
+    /// Copy of stand-alone JointDataBase are prevented, but can be used from inhereting
+    /// objects. 
+    inline JointDataBase& operator= (const JointDataBase&) { return *this; }
+
   }; // struct JointDataBase
 
   template<int NV>
@@ -465,6 +481,29 @@ namespace se3
 
     JointModelDense<NQ, NV> toDense() const  { return derived().toDense_impl();   }
     
+  protected:
+
+    /// Default constructor: protected.
+    /// 
+    /// Prevent the construction of stand-alone JointModelBase.
+    inline JointModelBase() {} // TODO: default value should be set to -1
+    /// Copy constructor: protected.
+    ///
+    /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
+    /// objects. Copy is done by calling copy operator.
+    inline JointModelBase( const JointModelBase& clone) { *this = clone; }
+    /// Copy operator: protected.
+    ///
+    /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
+    /// objects. 
+    inline JointModelBase& operator= (const JointModelBase& clone) 
+    {
+      i_id = clone.i_id;
+      i_q = clone.i_q;
+      i_v = clone.i_v;
+      return *this; 
+    }
+
   }; // struct JointModelBase
 
 } // namespace se3

src/multibody/visitor.hpp

@@ -27,10 +27,13 @@
 
 namespace boost {
   namespace fusion {
+
+    // Append the element T at the front of boost fusion vector V.
     template<typename T,typename V>
     typename result_of::push_front<V const, T>::type
     append(T const& t,V const& v) { return push_front(v,t); }
 
+    // Append the elements T1 and T2 at the front of boost fusion vector V.
     template<typename T1,typename T2,typename V>
     typename result_of::push_front<typename result_of::push_front<V const, T2>::type const, T1>::type
     append2(T1 const& t1,T2 const& t2,V const& v) { return push_front(push_front(v,t2),t1); }
@@ -53,7 +56,7 @@ namespace se3
 	JointDataVariant& jdataSpec = static_cast<const Visitor*>(this)->jdata;
 
 	bf::invoke(&Visitor::template algo<D>,
-		   bf::append2(jmodel,
+		   bf::append2(boost::ref(jmodel),
 			       boost::ref(boost::get<typename D::JointDataDerived>(jdataSpec)),
 			       static_cast<const Visitor*>(this)->args));
       }
@@ -74,10 +77,9 @@ namespace se3
       template<typename D>
       void operator() (const JointModelBase<D> & jmodel) const
       {
-
-  bf::invoke(&Visitor::template algo<D>,
-       bf::append(jmodel,
-             static_cast<const Visitor*>(this)->args));
+        bf::invoke(&Visitor::template algo<D>,
+                   bf::append(boost::ref(jmodel),
+                              static_cast<const Visitor*>(this)->args));
       }
 
       template<typename ArgsTmp>

unittest/CMakeLists.txt

@@ -113,3 +113,4 @@ ADD_UNIT_TEST(joint-configurations eigen3)
 ADD_UNIT_TEST(joint eigen3)
 ADD_UNIT_TEST(explog eigen3)
 ADD_UNIT_TEST(finite-differences eigen3)
+ADD_UNIT_TEST(visitor eigen3)

unittest/visitor.cpp

+//
+// Copyright (c) 2015-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/>.
+
+#include <iostream>
+
+#include <pinocchio/multibody/joint/joint.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include "pinocchio/multibody/visitor.hpp"
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE   ModuleTestVisitor
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+namespace se3
+{
+
+  struct SimpleVisitor : public se3::fusion::JointVisitor<SimpleVisitor>
+  {
+    typedef boost::fusion::vector<const se3::Model &,
+                                    se3::Data &,
+                                    int
+                                    > ArgsType;
+
+    JOINT_VISITOR_INIT(SimpleVisitor);
+
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                     se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                     const se3::Model & model,
+                     se3::Data & data,
+                     int jointId);
+  };
+
+  template<typename JointModel>
+  void SimpleVisitor::algo(const se3::JointModelBase<JointModel> & /*jmodel*/,
+                           se3::JointDataBase<typename JointModel::JointDataDerived> & /*jdata*/,
+                           const se3::Model & /*model*/,
+                           se3::Data & /*data*/,
+                           int /*dummy*/)
+  { /* --- do nothing --- */ }
+
+  template<>
+  void SimpleVisitor::algo(const se3::JointModelBase<JointModelRevoluteUnaligned> & jmodel,
+                           se3::JointDataBase<JointDataRevoluteUnaligned> & /*jdata*/,
+                           const se3::Model & /*model*/,
+                           se3::Data & /*data*/,
+                           int /*dummy*/)
+  {
+    BOOST_CHECK( jmodel.shortname() == JointModelRevoluteUnaligned::classname() );
+    Eigen::Vector3d axis = jmodel.derived().axis;
+    Eigen::Vector3d axis_z; axis_z << 0,0,1;
+
+    BOOST_CHECK ( axis == axis_z );
+  }
+
+} // namespace se3
+
+/* Validates the access to memory stored in joint models, by using the class
+ * joint model revolute unaligned. 
+ */
+
+BOOST_AUTO_TEST_SUITE ( TestVisitor )
+
+BOOST_AUTO_TEST_CASE ( test_runal )
+{
+  using namespace se3;
+
+  se3::Model model;
+  model.addJoint(0,se3::JointModelRevoluteUnaligned(0,0,1),se3::SE3::Random());
+  Data data(model);
+
+  for( Model::JointIndex i=1;i<(Model::JointIndex)model.njoint;++i )
+    {
+      SimpleVisitor::run(model.joints[i],data.joints[i],
+                         SimpleVisitor::ArgsType(model,data,i));
+    }
+}
+
+BOOST_AUTO_TEST_SUITE_END ()