Commit 6a2e9e66 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Clean code.

parent 9bd88d25
......@@ -21,76 +21,6 @@
namespace se3
{
// Compute the vector of joint index of the subtree.
// inline void
// subtreeIndexes(const Model & model, SubtreeModel & stModel)
// {
// JointIndex root = stModel.root;
// const size_t shift = root;
// std::vector<bool> shouldInclude (model.nbody-shift, true);
// for( Model::JointIndex i= (Model::JointIndex) (model.nbody-1);i>root;--i )
// {
// if (!shouldInclude[i - shift]) continue;
// Model::JointIndex j = i;
// while (j > root) j = model.parents[j];
// if (j == root) continue;
// j = i;
// while (j > root) {
// shouldInclude[j - shift] = false;
// j = model.parents[j];
// }
// }
// stModel.joints.clear();
// for(int i=(int)shouldInclude.size()-1; i>=0;--i)
// if (shouldInclude[i]) stModel.joints.push_back(i+shift);
// }
// struct SubtreeJacobianCenterOfMassBackwardStep
// : public fusion::JointVisitor<SubtreeJacobianCenterOfMassBackwardStep>
// {
// typedef boost::fusion::vector<const se3::Model &,
// se3::Data &,
// JointIndex
// > ArgsType;
// JOINT_VISITOR_INIT(SubtreeJacobianCenterOfMassBackwardStep);
// template<typename JointModel>
// static void algo(const se3::JointModelBase<JointModel> & jmodel,
// se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
// const se3::Model& model,
// se3::Data& data,
// const JointIndex& r )
// {
// const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
// const Model::JointIndex & parent = model.parents[i];
// if (i != r) {
// data.com[parent] += data.com[i];
// data.mass[parent] += data.mass[i];
// }
// typedef Data::Matrix6x Matrix6x;
// typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
// ColBlock Jcols = jmodel.jointCols(data.J);
// Jcols = data.oMi[i].act(jdata.S());
// if( JointModel::NV==1 )
// data.Jcom.col(jmodel.idx_v())
// = data.mass[i] * Jcols.template topLeftCorner<3,1>()
// - data.com[i].cross(Jcols.template bottomLeftCorner<3,1>());
// else
// jmodel.jointCols(data.Jcom)
// = data.mass[i] * Jcols.template topRows<3>()
// - skew(data.com[i]) * Jcols.template bottomRows<3>();
// data.com[i] /= data.mass[i];
// }
// } ;
inline void
subtreeCenterOfMass(const Model & model, Data & data,
const std::vector <JointIndex>& roots)
......
Supports Markdown
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