Skip to content
Snippets Groups Projects
Commit 939c994c authored by jcarpent's avatar jcarpent
Browse files

[C++] Uniformize naming of the algorithm for forward kinematics

parent 4213fa3a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
benchmark/timings.cpp
+ 5
− 5
View file @ 939c994c
......@@ -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;
......
src/algorithm/center-of-mass.hxx
+ 1
− 1
View file @ 939c994c
......@@ -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();
......
src/algorithm/energy.hpp
+ 2
− 2
View file @ 939c994c
......@@ -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);
......
src/algorithm/kinematics.hpp
+ 35
− 38
View file @ 939c994c
......@@ -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
......
src/python/algorithms.hpp
+ 18
− 18
View file @ 939c994c
......@@ -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)",
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment