Commit d3fbbd18 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Add distance algo and its default version

parent b2458ebb
......@@ -23,6 +23,8 @@
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include <cmath>
namespace se3
{
......@@ -410,6 +412,23 @@ namespace se3
{
return squaredDistance<LieGroupTpl>(model, q0, q1);
}
template<typename LieGroup_t>
inline double
distance(const Model & model,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return std::sqrt(squaredDistance<LieGroup_t>(model, q0, q1).sum());
}
inline double
distance(const Model & model,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1)
{
return std::sqrt(squaredDistance<LieGroupTpl>(model, q0, q1).sum());
}
template<typename LieGroup_t, typename JointModel> struct RandomConfigurationStepAlgo;
......
Markdown is supported
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