From 8ac35aa825d7152c09896e28107f30a777e52e8d Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Mon, 28 Dec 2015 14:58:41 +0100
Subject: [PATCH] =?UTF-8?q?[C++][Bench]=C2=A0Add=20empty=20forward=20loop?=
=?UTF-8?q?=20in=20the=20model=20tree?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
benchmark/timings.cpp | 7 +++++++
src/algorithm/kinematics.hpp | 40 ++++++++++++++++++++++++++++++++++++
2 files changed, 47 insertions(+)
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index 2ef6d5e1f..07bdef8e9 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -169,6 +169,13 @@ int main(int argc, const char ** argv)
forwardKinematics(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
}
std::cout << "Second Order Kinematics = \t"; timer.toc(std::cout,NBT);
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ emptyForwardPass(model,data);
+ }
+ std::cout << "Empty Forward Pass = \t"; timer.toc(std::cout,NBT);
std::cout << "--" << std::endl;
return 0;
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index e43ee8426..5d3f7d0a6 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -23,6 +23,15 @@
namespace se3
{
+ ///
+ /// \brief Browse through the model tree structure with an empty step
+ ///
+ /// \param[in] model The model structure of the rigid body system.
+ /// \param[in] data The data structure of the rigid body system.
+ ///
+ inline void emptyForwardPass(const Model & model,
+ Data & data);
+
inline void forwardKinematics(const Model & model,
Data & data,
const Eigen::VectorXd & q);
@@ -43,6 +52,37 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
+
+ struct emptyForwardStep : public fusion::JointVisitor<emptyForwardStep>
+ {
+ typedef boost::fusion::vector<const se3::Model &,
+ se3::Data &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT (emptyForwardStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> &,
+ se3::JointDataBase<typename JointModel::JointData> &,
+ const se3::Model &,
+ se3::Data &)
+ { // do nothing
+ }
+
+ };
+
+ inline void emptyForwardPass(const Model & model,
+ Data & data)
+ {
+ for (Model::Index i=1; i < (Model::Index) model.nbody; ++i)
+ {
+ emptyForwardStep::run(model.joints[i],
+ data.joints[i],
+ emptyForwardStep::ArgsType (model,data)
+ );
+ }
+ }
+
struct ForwardKinematicZeroStep : public fusion::JointVisitor<ForwardKinematicZeroStep>
{
typedef boost::fusion::vector<const se3::Model &,
--
GitLab