diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index f886e31fcb42e448c4a74ef07cece3c3d5dcbaaf..d5f2a83a410114d933c13dffb516465cc6d28005 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/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
diff --git a/src/multibody/visitor.hpp b/src/multibody/visitor.hpp
index fb768442d7f2c3c932541fbd2465a8d34f2a0a9a..d0cce5bb57d86d4d30e86f65602b7f0cb5245ad6 100644
--- a/src/multibody/visitor.hpp
+++ b/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>
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index 1143aa8759b44968ed74006a9328f43438b7ead2..329a780b5f26c9a0074a2558fbd37f697b9f746c 100644
--- a/unittest/CMakeLists.txt
+++ b/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)
diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd4dfea3739ce54662cbf8cb48273a5d7d474b5d
--- /dev/null
+++ b/unittest/visitor.cpp
@@ -0,0 +1,95 @@
+//
+// 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 ()