diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e97b6c7ac8a8e15bc33a7b3257cb57b68589cd1..80ad03e7d096455a750c88cf33354447f1eaac0e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,7 @@ SET(${PROJECT_NAME}_HEADERS
multibody/constraint.hpp
multibody/joint.hpp
multibody/model.hpp
+ multibody/visitor.hpp
tools/timer.hpp
)
diff --git a/src/multibody/visitor.hpp b/src/multibody/visitor.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e5812b14e8c0113a9ca3f1b2dc628d8212358d41
--- /dev/null
+++ b/src/multibody/visitor.hpp
@@ -0,0 +1,60 @@
+#ifndef __se3_visitor_hpp__
+#define __se3_visitor_hpp__
+
+#define BOOST_FUSION_INVOKE_MAX_ARITY 10
+#include <boost/fusion/include/invoke.hpp>
+#include <boost/fusion/include/algorithm.hpp>
+
+
+namespace boost {
+ namespace fusion {
+ 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); }
+
+ 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); }
+ }
+}
+
+
+namespace se3
+{
+ namespace fusion
+ {
+ namespace bf = boost::fusion;
+
+ template<typename Visitor>
+ struct JointVisitor : public boost::static_visitor<>
+ {
+ template<typename D>
+ void operator() (const JointModelBase<D> & jmodel) const
+ {
+ JointDataVariant& jdataSpec = static_cast<const Visitor*>(this)->jdata;
+
+ bf::invoke(&Visitor::template algo<D>,
+ bf::append2(jmodel,
+ boost::ref(boost::get<typename D::JointData&>(jdataSpec)),
+ static_cast<const Visitor*>(this)->args));
+ }
+
+ template<typename ArgsTmp>
+ static void run(const JointModelVariant & jmodel,
+ JointDataVariant & jdata,
+ ArgsTmp args)
+ {
+ return boost::apply_visitor( Visitor(jdata,args),jmodel );
+ }
+ };
+
+ } // namespace fusion
+} // namespace se3
+
+#define JOINT_VISITOR_INIT(VISITOR) \
+ VISITOR( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {} \
+ using JointVisitor<VISITOR>::run; \
+ JointDataVariant & jdata; \
+ ArgsType args
+
+#endif // ifndef __se3_visitor_hpp__
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 4c5c0781f9212d79993d29d1cb7993a4956408e7..94eaccc59d00856ab86cf287ce6daa50ff223b80 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -1,80 +1,39 @@
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
#include <iostream>
#include "pinocchio/tools/timer.hpp"
-#define BOOST_FUSION_INVOKE_MAX_ARITY 10
-#include <boost/fusion/include/sequence.hpp>
-#include <boost/fusion/include/make_vector.hpp>
-#include <boost/fusion/include/next.hpp>
-#include <boost/fusion/include/invoke.hpp>
-#include <boost/fusion/view/joint_view.hpp>
-#include <boost/fusion/include/joint_view.hpp>
-#include <boost/fusion/algorithm.hpp>
-#include <boost/fusion/container.hpp>
-
-
-namespace boost {
- namespace fusion {
- 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); }
-
- 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); }
- }
-}
namespace se3
{
- namespace bf = boost::fusion;
-
-
- struct RneaForwardStep : public boost::static_visitor<>
+ struct RneaForwardStep : public fusion::JointVisitor<RneaForwardStep>
{
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const int&,
+ const Eigen::VectorXd &,
+ const Eigen::VectorXd &,
+ const Eigen::VectorXd &
+ > ArgsType;
- typedef bf::vector< const se3::Model& ,
- se3::Data& ,
- int,
- const Eigen::VectorXd &,
- const Eigen::VectorXd &,
- const Eigen::VectorXd &
- > Args;
- Args args;
- JointDataVariant & jdata;
-
- RneaForwardStep( JointDataVariant & jdata,Args args ) : args(args),jdata(jdata) {}
-
- template<typename D>
- void operator() (const JointModelBase<D> & jmodel) const
- {
- bf::invoke(&RneaForwardStep::algo<D>,
- bf::append2(jmodel,boost::ref(boost::get<typename D::JointData&>(jdata)),args));
- }
+ JOINT_VISITOR_INIT(RneaForwardStep);
- static void run(const JointModelVariant & jmodel,
- JointDataVariant & jdata,
- Args args)
- {
- return boost::apply_visitor( RneaForwardStep(jdata,args),jmodel );
- }
-
template<typename JointModel>
- static void algo(const se3::JointModelBase<JointModel> & jmodel,
- se3::JointDataBase<typename JointModel::JointData> & jdata,
- const se3::Model& model,
- se3::Data& data,
- const int &i,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a)
+ static int algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const int &i,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v,
+ const Eigen::VectorXd & a)
{
using namespace Eigen;
using namespace se3;
@@ -95,48 +54,32 @@ namespace se3
if(parent>0) data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
+ return 0;
}
};
-
-
-struct RneaBackwardStep : public boost::static_visitor<>
-{
- typedef bf::vector<const Model& ,
- Data& ,
- const int &> Args;
- JointDataVariant& jdata;
- Args args;
-
- RneaBackwardStep( JointDataVariant& jdata, Args args)
- : jdata(jdata),args(args) {}
-
- template <typename D>
- void operator()(const JointModelBase<D> & jmodel) const
- {
- bf::invoke(&RneaBackwardStep::algo<D>,
- bf::append2(jmodel,boost::ref(boost::get<typename D::JointData&>(jdata)),args));
- }
-
- static void run( const JointModelVariant & jmodel,
- JointDataVariant & jdata,
- Args args)
- { boost::apply_visitor( RneaBackwardStep(jdata,args),jmodel ); }
-
- template<typename JointModel>
- static void algo(const JointModelBase<JointModel> & jmodel,
- JointDataBase<typename JointModel::JointData> & jdata,
- const Model& model,
- Data& data,
- int i)
+ struct RneaBackwardStep : public fusion::JointVisitor<RneaBackwardStep>
{
- const Model::Index & parent = model.parents[i];
- jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[i];
- if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
- }
-};
+ typedef boost::fusion::vector<const Model&,
+ Data&,
+ const int &> ArgsType;
+
+ JOINT_VISITOR_INIT(RneaBackwardStep);
+
+ template<typename JointModel>
+ static void algo(const JointModelBase<JointModel> & jmodel,
+ JointDataBase<typename JointModel::JointData> & jdata,
+ const Model& model,
+ Data& data,
+ int i)
+ {
+ const Model::Index & parent = model.parents[i];
+ jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[i];
+ if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
+ }
+ };
} // namespace se3
@@ -149,7 +92,7 @@ struct RneaBackwardStep : public boost::static_visitor<>
int main()
{
#ifdef __SSE3__
-_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
+ _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
#endif
@@ -209,17 +152,17 @@ _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(1000)
{
- for( int i=1;i<model.nbody;++i )
- {
- RneaForwardStep::run(model.joints[i],data.joints[i],
- RneaForwardStep::Args(model,data,i,q,v,a));
- }
-
- for( int i=model.nbody-1;i>0;--i )
- {
- RneaBackwardStep::run(model.joints[i],data.joints[i],
- RneaBackwardStep::Args(model,data,i));
- }
+ for( int i=1;i<model.nbody;++i )
+ {
+ RneaForwardStep::run(model.joints[i],data.joints[i],
+ RneaForwardStep::ArgsType(model,data,i,q,v,a));
+ }
+
+ for( int i=model.nbody-1;i>0;--i )
+ {
+ RneaBackwardStep::run(model.joints[i],data.joints[i],
+ RneaBackwardStep::ArgsType(model,data,i));
+ }
}
timer.toc(std::cout,1000);