Skip to content
Snippets Groups Projects
Commit 19be33a1 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Moved the RNEA into src/algorithm.

parent 423a7eef
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 2
− 0
View file @ 19be33a1
......@@ -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)
......
src/algorithm/rnea.hpp 0 → 100644
+ 113
− 0
View file @ 19be33a1
#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__
unittest/rnea.cpp
+ 4
− 88
View file @ 19be33a1
......@@ -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);
......
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