Commit 3834a6d8 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add neutral algo to compute the neutral configuration of a model

parent 13b2a852
//
// Copyright (c) 2016-2017 CNRS
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -133,7 +133,7 @@ namespace se3
* @brief Return true if the given configurations are equivalents
* \warning Two configurations can be equivalent but not equally coefficient wise (e.g for quaternions)
*
* @param[in] model Model
* @param[in] model Model
* @param[in] q1 The first configuraiton to compare
* @param[in] q2 The Second configuraiton to compare
* @param[in] prec precision of the comparison
......@@ -145,6 +145,29 @@ namespace se3
const Eigen::VectorXd & q1,
const Eigen::VectorXd & q2,
const double & prec = Eigen::NumTraits<double>::dummy_precision());
/**
* @brief Return the neutral configuration element related to the model configuration space.
*
* @param[in] model Model
*
* @return The neutral configuration element.
*/
template<typename LieGroup_t>
inline Eigen::VectorXd neutral(const Model & model);
/**
* @brief Return the neutral configuration element related to the joint model
*
* @param[in] jmodel Model of the joint
*
* @return The neutral configuration element.
*/
template<typename LieGroup_t, typename JointModelDerived>
inline Eigen::VectorXd neutral(const JointModelBase<JointModelDerived> & jmodel);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
......
......@@ -492,6 +492,49 @@ namespace se3
{
return isSameConfiguration<LieGroupTpl>(model, q1, q2, prec);
}
template<typename LieGroup_t, typename JointModel> struct NeutralStepAlgo;
template<typename LieGroup_t>
struct NeutralStep : public fusion::JointModelVisitor< NeutralStep<LieGroup_t> >
{
typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
JOINT_MODEL_VISITOR_INIT(NeutralStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_1(NeutralStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct NeutralStepAlgo
{
static void run(const se3::JointModelBase<JointModel> & jmodel,
Eigen::VectorXd & neutral_elt)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
jmodel.jointConfigSelector(neutral_elt) = lgo.neutral();
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStep, NeutralStepAlgo);
template<typename LieGroup_t>
inline Eigen::VectorXd neutral(const Model & model)
{
Eigen::VectorXd neutral_elt(model.nq);
typename NeutralStep<LieGroup_t>::ArgsType args(neutral_elt);
for( Model::JointIndex i=1; i<(Model::JointIndex) model.njoints; ++i )
{
NeutralStep<LieGroup_t>::run(model.joints[i], args);
}
return neutral_elt;
}
inline Eigen::VectorXd neutral(const Model & model)
{
return neutral<LieGroupTpl>(model);
}
} // namespace se3
......
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