diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0f67f6b5ddf374ac1116c777d40e7cce4a01966..05134485475e4e92fcd831738c3736f488384b69 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,7 @@ SET(${PROJECT_NAME}_HEADERS
multibody/model.hpp
multibody/visitor.hpp
tools/timer.hpp
+ algorithm/rnea.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -53,6 +54,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
FOREACH(header ${${PROJECT_NAME}_HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4288093ce28c7f66d60f69aa8db7a7a0154700fb
--- /dev/null
+++ b/src/algorithm/rnea.hpp
@@ -0,0 +1,113 @@
+#ifndef __se3_rne_hpp__
+#define __se3_rne_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3
+{
+ inline const Eigen::VectorXd&
+ rnea(const Model & model, Data& data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v,
+ const Eigen::VectorXd & a);
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+ 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;
+
+ JOINT_VISITOR_INIT(RneaForwardStep);
+
+ template<typename JointModel>
+ 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;
+
+ jmodel.calc(jdata.derived(),q,v);
+
+ const Model::Index & parent = model.parents[i];
+ data.liMi[i] = model.jointPlacements[i]*jdata.M();
+
+ if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
+ else data.oMi[i] = data.liMi[i];
+
+ data.v[i] = jdata.v();
+ if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
+
+ jmodel.jointMotion(a);
+ data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
+ 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 fusion::JointVisitor<RneaBackwardStep>
+ {
+ 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]);
+ }
+ };
+
+ inline const Eigen::VectorXd&
+ rnea(const Model & model, Data& data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v,
+ const Eigen::VectorXd & a)
+ {
+ data.v[0] = Motion::Zero();
+ data.a[0] = -model.gravity;
+
+ 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));
+ }
+
+ return data.tau;
+ }
+} // namespace se3
+
+#endif // ifndef __se3_rne_hpp__
+
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 94eaccc59d00856ab86cf287ce6daa50ff223b80..522f49c9de530899adb88ce212c012cfd45cd298 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -3,85 +3,12 @@
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
#include <iostream>
#include "pinocchio/tools/timer.hpp"
-
-
-namespace se3
-{
-
- 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;
-
- JOINT_VISITOR_INIT(RneaForwardStep);
-
- template<typename JointModel>
- 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;
-
- jmodel.calc(jdata.derived(),q,v);
-
- const Model::Index & parent = model.parents[i];
- data.liMi[i] = model.jointPlacements[i]*jdata.M();
-
- if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
- else data.oMi[i] = data.liMi[i];
-
- data.v[i] = jdata.v();
- if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
-
- jmodel.jointMotion(a);
- data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
- 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 fusion::JointVisitor<RneaBackwardStep>
- {
- 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
//#define __SSE3__
#include <fenv.h>
@@ -141,28 +68,17 @@ int main()
model.addBody(model.getBodyId("larm6"),JointModelRX(),SE3::Random(),Inertia::Random(),"lgrip");
se3::Data data(model);
+ data.v[0] = Motion::Zero();
+ data.a[0] = -model.gravity;
VectorXd q = VectorXd::Random(model.nq);
VectorXd v = VectorXd::Random(model.nv);
VectorXd a = VectorXd::Random(model.nv);
-
- data.v[0] = Motion::Zero();
- data.a[0] = -model.gravity;
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::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));
- }
+ rnea(model,data,q,v,a);
}
timer.toc(std::cout,1000);