From 939c994cd5bf64f71a3230bac117aa13a05d7c30 Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Fri, 18 Dec 2015 18:53:55 +0100
Subject: [PATCH] [C++] Uniformize naming of the algorithm for forward
kinematics
---
benchmark/timings.cpp | 10 ++---
src/algorithm/center-of-mass.hxx | 2 +-
src/algorithm/energy.hpp | 4 +-
src/algorithm/kinematics.hpp | 73 +++++++++++++++-----------------
src/python/algorithms.hpp | 36 ++++++++--------
5 files changed, 61 insertions(+), 64 deletions(-)
diff --git a/benchmark/timings.cpp b/benchmark/timings.cpp
index e6cf7557b..2ef6d5e1f 100644
--- a/benchmark/timings.cpp
+++ b/benchmark/timings.cpp
@@ -151,7 +151,7 @@ int main(int argc, const char ** argv)
timer.tic();
SMOOTH(NBT)
{
- geometry(model,data,qs[_smooth]);
+ forwardKinematics(model,data,qs[_smooth]);
}
std::cout << "Geometry = \t"; timer.toc(std::cout,NBT);
@@ -159,16 +159,16 @@ int main(int argc, const char ** argv)
timer.tic();
SMOOTH(NBT)
{
- kinematics(model,data,qs[_smooth],qdots[_smooth]);
+ forwardKinematics(model,data,qs[_smooth],qdots[_smooth]);
}
- std::cout << "Kinematics = \t"; timer.toc(std::cout,NBT);
+ std::cout << "First Order Kinematics = \t"; timer.toc(std::cout,NBT);
timer.tic();
SMOOTH(NBT)
{
- dynamics(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
+ forwardKinematics(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
}
- std::cout << "Dynamics = \t"; timer.toc(std::cout,NBT);
+ std::cout << "Second Order Kinematics = \t"; timer.toc(std::cout,NBT);
std::cout << "--" << std::endl;
return 0;
diff --git a/src/algorithm/center-of-mass.hxx b/src/algorithm/center-of-mass.hxx
index 8621dbead..449710cf0 100644
--- a/src/algorithm/center-of-mass.hxx
+++ b/src/algorithm/center-of-mass.hxx
@@ -104,7 +104,7 @@ namespace se3
data.acom[0].setZero ();
// Forward Step
- dynamics(model, data, q, v, a);
+ forwardKinematics(model, data, q, v, a);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
{
const double mass = model.inertias[i].mass();
diff --git a/src/algorithm/energy.hpp b/src/algorithm/energy.hpp
index 0fb14ee37..ad5538259 100644
--- a/src/algorithm/energy.hpp
+++ b/src/algorithm/energy.hpp
@@ -74,7 +74,7 @@ namespace se3
data.kinetic_energy = 0.;
if (update_kinematics)
- kinematics(model,data,q,v);
+ forwardKinematics(model,data,q,v);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
data.kinetic_energy += model.inertias[i].vtiv(data.v[i]);
@@ -93,7 +93,7 @@ namespace se3
const Motion::ConstLinear_t & g = model.gravity.linear();
if (update_kinematics)
- geometry(model,data,q);
+ forwardKinematics(model,data,q);
for(Model::Index i=1;i<(Model::Index)(model.nbody);++i)
data.potential_energy += model.inertias[i].mass() * data.oMi[i].translation().dot(g);
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 3181e00fb..9d700a7a4 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -23,29 +23,27 @@
namespace se3
{
- inline void geometry(const Model & model,
- Data & data,
- const Eigen::VectorXd & q);
-
-
-
- inline void kinematics(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v);
+ inline void forwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q);
+
+ inline void forwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v);
- inline void dynamics(const Model & model,
- Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a);
+ inline void forwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v,
+ const Eigen::VectorXd & a);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
- struct GeometryStep : public fusion::JointVisitor<GeometryStep>
+ struct ForwardKinematicZeroStep : public fusion::JointVisitor<ForwardKinematicZeroStep>
{
typedef boost::fusion::vector<const se3::Model &,
se3::Data &,
@@ -53,7 +51,7 @@ namespace se3
const Eigen::VectorXd &
> ArgsType;
- JOINT_VISITOR_INIT (GeometryStep);
+ JOINT_VISITOR_INIT (ForwardKinematicZeroStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
@@ -79,21 +77,20 @@ namespace se3
};
inline void
- geometry(const Model & model,
- Data & data,
- const Eigen::VectorXd & q)
+ forwardKinematics(const Model & model,
+ Data & data,
+ const Eigen::VectorXd & q)
{
for (Model::Index i=1; i < (Model::Index) model.nbody; ++i)
{
- GeometryStep::run(model.joints[i],
+ ForwardKinematicZeroStep::run(model.joints[i],
data.joints[i],
- GeometryStep::ArgsType (model,data,i,q)
+ ForwardKinematicZeroStep::ArgsType (model,data,i,q)
);
}
}
-
- struct KinematicsStep : public fusion::JointVisitor<KinematicsStep>
+ struct ForwardKinematicFirstStep : public fusion::JointVisitor<ForwardKinematicFirstStep>
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
@@ -102,7 +99,7 @@ namespace se3
const Eigen::VectorXd &
> ArgsType;
- JOINT_VISITOR_INIT(KinematicsStep);
+ JOINT_VISITOR_INIT(ForwardKinematicFirstStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
@@ -134,20 +131,20 @@ namespace se3
};
inline void
- kinematics(const Model & model, Data& data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v)
+ forwardKinematics(const Model & model, Data & data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v)
{
data.v[0].setZero();
for( Model::Index i=1; i<(Model::Index) model.nbody; ++i )
{
- KinematicsStep::run(model.joints[i],data.joints[i],
- KinematicsStep::ArgsType(model,data,i,q,v));
+ ForwardKinematicFirstStep::run(model.joints[i],data.joints[i],
+ ForwardKinematicFirstStep::ArgsType(model,data,i,q,v));
}
}
- struct DynamicsStep : public fusion::JointVisitor<DynamicsStep>
+ struct ForwardKinematicSecondStep : public fusion::JointVisitor<ForwardKinematicSecondStep>
{
typedef boost::fusion::vector< const se3::Model&,
se3::Data&,
@@ -157,7 +154,7 @@ namespace se3
const Eigen::VectorXd &
> ArgsType;
- JOINT_VISITOR_INIT(DynamicsStep);
+ JOINT_VISITOR_INIT(ForwardKinematicSecondStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
@@ -189,18 +186,18 @@ namespace se3
};
inline void
- dynamics(const Model & model, Data & data,
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & v,
- const Eigen::VectorXd & a)
+ forwardKinematics(const Model & model, Data & data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v,
+ const Eigen::VectorXd & a)
{
data.v[0].setZero();
data.a[0].setZero();
for( Model::Index i=1; i < (Model::Index) model.nbody; ++i )
{
- DynamicsStep::run(model.joints[i],data.joints[i],
- DynamicsStep::ArgsType(model,data,i,q,v,a));
+ ForwardKinematicSecondStep::run(model.joints[i],data.joints[i],
+ ForwardKinematicSecondStep::ArgsType(model,data,i,q,v,a));
}
}
} // namespace se3
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index b31ae65d2..fdfb0fd9f 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -113,28 +113,28 @@ namespace se3
computeJacobians( *model,*data,q );
}
- static void geometry_proxy(const ModelHandler & model,
- DataHandler & data,
- const VectorXd_fx & q)
+ static void fk_0_proxy(const ModelHandler & model,
+ DataHandler & data,
+ const VectorXd_fx & q)
{
- geometry(*model,*data,q);
+ forwardKinematics(*model,*data,q);
}
- static void kinematics_proxy( const ModelHandler& model,
- DataHandler & data,
- const VectorXd_fx & q,
- const VectorXd_fx & qdot )
+ static void fk_1_proxy( const ModelHandler& model,
+ DataHandler & data,
+ const VectorXd_fx & q,
+ const VectorXd_fx & qdot )
{
- kinematics( *model,*data,q,qdot );
+ forwardKinematics(*model,*data,q,qdot);
}
- static void dynamics_proxy(const ModelHandler& model,
- DataHandler & data,
- const VectorXd_fx & q,
- const VectorXd_fx & v,
- const VectorXd_fx & a)
+ static void fk_2_proxy(const ModelHandler& model,
+ DataHandler & data,
+ const VectorXd_fx & q,
+ const VectorXd_fx & v,
+ const VectorXd_fx & a)
{
- dynamics(*model,*data,q,v,a);
+ forwardKinematics(*model,*data,q,v,a);
}
static void computeAllTerms_proxy(const ModelHandler & model,
@@ -245,20 +245,20 @@ namespace se3
"Configuration q (size Model::nq)"),
"Compute the jacobian of the center of mass, put the result in Data and return it.");
- bp::def("kinematics",kinematics_proxy,
+ bp::def("kinematics",fk_1_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"),
"Compute the placements and spatial velocities of all the frames of the kinematic "
"tree and put the results in data.");
- bp::def("geometry",geometry_proxy,
+ bp::def("geometry",fk_0_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the placements of all the frames of the kinematic "
"tree and put the results in data.");
- bp::def("dynamics",dynamics_proxy,
+ bp::def("dynamics",fk_2_proxy,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)",
--
GitLab