Skip to content
GitLab
Commit 84f5ab1d authored by jcarpent's avatar jcarpent
Browse files

[LieGroup] Move LieGroup algo code to a dedicated file

parent 6795c5ce
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 84f5ab1d
......@@ -203,6 +203,8 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/liegroup.hpp
multibody/liegroup/liegroup-algo.hpp
multibody/liegroup/liegroup-algo.hxx
multibody/liegroup/liegroup-variant.hpp
multibody/liegroup/liegroup-variant-visitors.hpp
multibody/liegroup/liegroup-variant-visitors.hxx
......
src/algorithm/joint-configuration.hxx
View file @ 84f5ab1d
......@@ -22,106 +22,13 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
#include <cmath>
/* --- Details -------------------------------------------------------------------- */
namespace se3
{
namespace details
{
template<typename Algo>
struct Dispatch
{
static void run (const JointModelComposite& jmodel,
typename Algo::ArgsType args)
{
for (size_t i = 0; i < jmodel.joints.size(); ++i)
Algo::run(jmodel.joints[i], args);
}
};
#define SE3_DETAILS_WRITE_ARGS_0(JM) const JointModelBase<JM> & jmodel
#define SE3_DETAILS_WRITE_ARGS_1(JM) SE3_DETAILS_WRITE_ARGS_0(JM), typename boost::fusion::result_of::at_c<ArgsType, 0>::type a0
#define SE3_DETAILS_WRITE_ARGS_2(JM) SE3_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1
#define SE3_DETAILS_WRITE_ARGS_3(JM) SE3_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2
#define SE3_DETAILS_WRITE_ARGS_4(JM) SE3_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_1(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_2(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_2(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_3(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1, a2)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_4(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1, a2, a3)); } \
}
#define SE3_DETAILS_VISITOR_METHOD_ALGO_1(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_1(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_2(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_2(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_3(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_3(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1, a2); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_4(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_4(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1, a2, a3); }
} // namespace details
template<typename LieGroup_t, typename JointModel> struct IntegrateStepAlgo;
template<typename LieGroup_t>
struct IntegrateStep : public fusion::JointModelVisitor<IntegrateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(IntegrateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(IntegrateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct IntegrateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.integrate(jmodel.jointConfigSelector (q),
jmodel.jointVelocitySelector(v),
jmodel.jointConfigSelector (result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStep, IntegrateStepAlgo);
inline Eigen::VectorXd
integrate(const Model & model,
const Eigen::VectorXd & q,
......@@ -145,40 +52,6 @@ namespace se3
return integ;
}
template<typename LieGroup_t, typename JointModel> struct InterpolateStepAlgo;
template<typename LieGroup_t>
struct InterpolateStep : public fusion::JointModelVisitor<InterpolateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
const double,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(InterpolateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_4(InterpolateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct InterpolateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
const double u,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.interpolate(jmodel.jointConfigSelector(q0),
jmodel.jointConfigSelector(q1),
u,
jmodel.jointConfigSelector(result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStep, InterpolateStepAlgo);
inline Eigen::VectorXd
interpolate(const Model & model,
const Eigen::VectorXd & q0,
......@@ -204,37 +77,6 @@ namespace se3
return interp;
}
template<typename LieGroup_t, typename JointModel> struct DifferentiateStepAlgo;
template<typename LieGroup_t>
struct DifferentiateStep : public fusion::JointModelVisitor<DifferentiateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(DifferentiateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(DifferentiateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct DifferentiateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.difference(jmodel.jointConfigSelector(q0),
jmodel.jointConfigSelector(q1),
jmodel.jointVelocitySelector(result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferentiateStep, DifferentiateStepAlgo);
template<typename LieGroup_t>
inline Eigen::VectorXd
differentiate(const Model & model,
......@@ -258,38 +100,6 @@ namespace se3
return differentiate<LieGroupTpl>(model, q0, q1);
}
template<typename LieGroup_t, typename JointModel> struct SquaredDistanceStepAlgo;
template<typename LieGroup_t>
struct SquaredDistanceStep : public fusion::JointModelVisitor<SquaredDistanceStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Model::JointIndex,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(SquaredDistanceStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_4(SquaredDistanceStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct SquaredDistanceStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Model::JointIndex i,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
Eigen::VectorXd & distances)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
distances[(long)i] += lgo.squaredDistance(jmodel.jointConfigSelector(q0),
jmodel.jointConfigSelector(q1));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(SquaredDistanceStep, SquaredDistanceStepAlgo);
template<typename LieGroup_t>
inline Eigen::VectorXd
squaredDistance(const Model & model,
......@@ -330,37 +140,6 @@ namespace se3
return std::sqrt(squaredDistance<LieGroupTpl>(model, q0, q1).sum());
}
template<typename LieGroup_t, typename JointModel> struct RandomConfigurationStepAlgo;
template<typename LieGroup_t>
struct RandomConfigurationStep : public fusion::JointModelVisitor<RandomConfigurationStep<LieGroup_t> >
{
typedef boost::fusion::vector<Eigen::VectorXd &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(RandomConfigurationStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(RandomConfigurationStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct RandomConfigurationStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
Eigen::VectorXd & q,
const Eigen::VectorXd & lowerLimits,
const Eigen::VectorXd & upperLimits)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.randomConfiguration(jmodel.jointConfigSelector(lowerLimits),
jmodel.jointConfigSelector(upperLimits),
jmodel.jointConfigSelector(q));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(RandomConfigurationStep, RandomConfigurationStepAlgo);
template<typename LieGroup_t>
inline Eigen::VectorXd
randomConfiguration(const Model & model, const Eigen::VectorXd & lowerLimits, const Eigen::VectorXd & upperLimits)
......@@ -392,31 +171,6 @@ namespace se3
{
return randomConfiguration<LieGroupTpl>(model);
}
template<typename LieGroup_t, typename JointModel> struct NormalizeStepAlgo;
template<typename LieGroup_t>
struct NormalizeStep : public fusion::JointModelVisitor< NormalizeStep<LieGroup_t> >
{
typedef boost::fusion::vector<Eigen::VectorXd &> ArgsType;
JOINT_MODEL_VISITOR_INIT(NormalizeStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct NormalizeStepAlgo
{
static void run(const se3::JointModelBase<JointModel> & jmodel,
Eigen::VectorXd & qout)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.normalize(jmodel.jointConfigSelector(qout));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStep, NormalizeStepAlgo);
template<typename LieGroup_t>
inline void normalize(const Model & model, Eigen::VectorXd & qout)
......@@ -433,38 +187,6 @@ namespace se3
return normalize<LieGroupTpl>(model,qout);
}
template<typename LieGroup_t, typename JointModel> struct IsSameConfigurationStepAlgo;
template<typename LieGroup_t>
struct IsSameConfigurationStep : public fusion::JointModelVisitor<IsSameConfigurationStep<LieGroup_t> >
{
typedef boost::fusion::vector<bool &,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
const double&> ArgsType;
JOINT_MODEL_VISITOR_INIT(IsSameConfigurationStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_4(IsSameConfigurationStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct IsSameConfigurationStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
bool & isSame,
const Eigen::VectorXd & q1,
const Eigen::VectorXd & q2,
const double prec)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
isSame &= lgo.isSameConfiguration(jmodel.jointConfigSelector(q1),
jmodel.jointConfigSelector(q2),
prec);
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(IsSameConfigurationStep, IsSameConfigurationStepAlgo);
template<typename LieGroup_t>
inline bool
isSameConfiguration(const Model & model,
......@@ -492,30 +214,7 @@ 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)
......
src/multibody/liegroup/liegroup-algo.hpp 0 → 100644
View file @ 84f5ab1d
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_lie_group_algo_hpp__
#define __se3_lie_group_algo_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
/// Details
#include "pinocchio/multibody/liegroup/liegroup-algo.hxx"
#endif // ifndef __se3_lie_group_algo_hpp__
src/multibody/liegroup/liegroup-algo.hxx 0 → 100644
View file @ 84f5ab1d
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_lie_group_algo_hxx__
#define __se3_lie_group_algo_hxx__
#include "pinocchio/multibody/visitor.hpp"
namespace se3
{
namespace details
{
template<typename Algo>
struct Dispatch
{
static void run (const JointModelComposite& jmodel,
typename Algo::ArgsType args)
{
for (size_t i = 0; i < jmodel.joints.size(); ++i)
Algo::run(jmodel.joints[i], args);
}
};
#define SE3_DETAILS_WRITE_ARGS_0(JM) const JointModelBase<JM> & jmodel
#define SE3_DETAILS_WRITE_ARGS_1(JM) SE3_DETAILS_WRITE_ARGS_0(JM), typename boost::fusion::result_of::at_c<ArgsType, 0>::type a0
#define SE3_DETAILS_WRITE_ARGS_2(JM) SE3_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1
#define SE3_DETAILS_WRITE_ARGS_3(JM) SE3_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2
#define SE3_DETAILS_WRITE_ARGS_4(JM) SE3_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_1(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_2(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_2(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_3(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1, a2)); } \
}
#define SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(Visitor, Algo) \
template <typename LieGroup_t> struct Algo <LieGroup_t, JointModelComposite> { \
typedef typename Visitor<LieGroup_t>::ArgsType ArgsType; \
static void run (SE3_DETAILS_WRITE_ARGS_4(JointModelComposite)) \
{ ::se3::details::Dispatch< Visitor<LieGroup_t> >::run(jmodel.derived(), ArgsType(a0, a1, a2, a3)); } \
}
#define SE3_DETAILS_VISITOR_METHOD_ALGO_1(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_1(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_2(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_2(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_3(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_3(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1, a2); }
#define SE3_DETAILS_VISITOR_METHOD_ALGO_4(Algo, TplParam) \
template<typename JointModel> \
static void algo(SE3_DETAILS_WRITE_ARGS_4(JointModel)) \
{ Algo<TplParam, JointModel>::run(jmodel, a0, a1, a2, a3); }
} // namespace details
template<typename LieGroup_t, typename JointModel> struct IntegrateStepAlgo;
template<typename LieGroup_t>
struct IntegrateStep : public fusion::JointModelVisitor<IntegrateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(IntegrateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(IntegrateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct IntegrateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.integrate(jmodel.jointConfigSelector (q),
jmodel.jointVelocitySelector(v),
jmodel.jointConfigSelector (result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStep, IntegrateStepAlgo);
template<typename LieGroup_t, typename JointModel> struct InterpolateStepAlgo;
template<typename LieGroup_t>
struct InterpolateStep : public fusion::JointModelVisitor<InterpolateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
const double,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(InterpolateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_4(InterpolateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct InterpolateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
const double u,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.interpolate(jmodel.jointConfigSelector(q0),
jmodel.jointConfigSelector(q1),
u,
jmodel.jointConfigSelector(result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStep, InterpolateStepAlgo);
template<typename LieGroup_t, typename JointModel> struct DifferentiateStepAlgo;
template<typename LieGroup_t>
struct DifferentiateStep : public fusion::JointModelVisitor<DifferentiateStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(DifferentiateStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_3(DifferentiateStepAlgo, LieGroup_t)
};
template<typename LieGroup_t, typename JointModel>
struct DifferentiateStepAlgo {
static void run(const se3::JointModelBase<JointModel> & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
Eigen::VectorXd & result)
{
typename LieGroup_t::template operation<JointModel>::type lgo;
lgo.difference(jmodel.jointConfigSelector(q0),
jmodel.jointConfigSelector(q1),
jmodel.jointVelocitySelector(result));
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferentiateStep, DifferentiateStepAlgo);
template<typename LieGroup_t, typename JointModel> struct SquaredDistanceStepAlgo;
template<typename LieGroup_t>
struct SquaredDistanceStep : public fusion::JointModelVisitor<SquaredDistanceStep<LieGroup_t> >
{
typedef boost::fusion::vector<const Model::JointIndex,
const Eigen::VectorXd &,
const Eigen::VectorXd &,
Eigen::VectorXd &
> ArgsType;
JOINT_MODEL_VISITOR_INIT(SquaredDistanceStep);
SE3_DETAILS_VISITOR_METHOD_ALGO_4(SquaredDistanceStepAlgo, LieGroup_t)
};