Commit da4c6c41 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Fix isValidConfiguration

parent a3e50e17
......@@ -92,6 +92,14 @@ namespace hpp {
LieGroup2::getRotationSubJacobian(Jin.template rightCols<LieGroup2::NV>(),
J .template rightCols<LieGroup2::NR>());
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
return LieGroup1::isValidConfig(q.template head<LieGroup1::NQ>(), eps)
&& LieGroup2::isValidConfig(q.template tail<LieGroup2::NQ>(), eps);
}
}; // struct CartesianProductOperation
} // namespace liegroup
} // namespace pinocchio
......
......@@ -69,6 +69,13 @@ namespace hpp {
{
const_cast<Eigen::MatrixBase<JacobianOut_t>&> (Jout) = Jin;
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > & q, const value_type& eps)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
return (std::abs(q.norm() - 1) < eps );
}
};
} // namespace liegroup
} // namespace pinocchio
......
......@@ -88,6 +88,12 @@ namespace hpp {
{
details::assign_if<rot>::run(Jin, Jout);
}
template <class ConfigIn_t>
static bool isValidConfig(const Eigen::MatrixBase<ConfigIn_t > &, const value_type&)
{
return true;
}
};
} // namespace liegroup
} // namespace pinocchio
......
......@@ -133,37 +133,30 @@ namespace hpp {
JOINT_MODEL_VISITOR_INIT(IsValidStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> &,
ConfigurationIn_t,
const value_type &,
bool & ret)
{
ret = true;
}
template<int I>
static void algo(const se3::JointModelRevoluteUnbounded<I> & jmodel,
static void algo(const se3::JointModelBase<JointModel> & jmodel,
ConfigurationIn_t q,
const value_type & eps,
bool & ret)
{
ret = (std::abs(jmodel.jointConfigSelector(q).norm() - 1) < eps );
}
static void algo(const se3::JointModelFreeFlyer & jmodel,
ConfigurationIn_t q,
const value_type & eps,
bool & ret)
{
ret = (std::abs(jmodel.jointConfigSelector(q).norm() - 1) < eps );
typedef typename LieGroupTpl::operation<JointModel>::type LG_t;
ret = ret && LG_t::isValidConfig(jmodel.jointConfigSelector(q), eps);
}
};
template<>
void IsValidStep::algo<se3::JointModelComposite>(const se3::JointModelBase<se3::JointModelComposite> & jmodel,
ConfigurationIn_t q,
const value_type & eps,
bool & ret)
{
se3::details::Dispatch<IsValidStep>::run(jmodel, IsValidStep::ArgsType(q, eps, ret));
}
bool isValidConfiguration (const DevicePtr_t& robot, ConfigurationIn_t q, const value_type& eps)
{
bool ret;
bool ret = true;
const se3::Model& model = robot->model();
for (std::size_t i = 1; model.njoints; ++i) {
for (std::size_t i = 1; i < model.njoints; ++i) {
IsValidStep::run(model.joints[i],
IsValidStep::ArgsType(q, eps, ret));
if (!ret) return false;
......
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