Commit 13aaab12 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add DefaultLieGroupMap

parent db4f0ec8
......@@ -87,6 +87,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/liegroup-space.hh
include/hpp/pinocchio/liegroup/vector-space.hh
include/hpp/pinocchio/liegroup/cartesian-product.hh
include/hpp/pinocchio/liegroup/special-euclidean.hh
include/hpp/pinocchio/liegroup/special-orthogonal.hh
include/hpp/pinocchio/urdf/util.hh
......
......@@ -23,6 +23,7 @@
#include <hpp/pinocchio/liegroup/vector-space.hh>
#include <hpp/pinocchio/liegroup/cartesian-product.hh>
#include <hpp/pinocchio/liegroup/special-orthogonal.hh>
#include <hpp/pinocchio/liegroup/special-euclidean.hh>
namespace hpp {
namespace pinocchio {
......@@ -73,6 +74,48 @@ namespace hpp {
liegroup::SpecialOrthogonalOperation<2>
> type;
};
// Default implementation is empty
struct DefaultLieGroupMap {
template<typename JointModel> struct operation {};
};
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelRevolute<Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnbounded<Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnaligned > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelPrismatic<Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelPrismaticUnaligned > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelTranslation > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<> struct DefaultLieGroupMap::operation <se3::JointModelSpherical> {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelSphericalZYX> {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<> struct DefaultLieGroupMap::operation <se3::JointModelFreeFlyer> {
typedef liegroup::SpecialEuclideanOperation<3> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelPlanar> {
typedef liegroup::SpecialEuclideanOperation<2> type;
};
} // namespace pinocchio
} // namespace hpp
......
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-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.
//
// hpp-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
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH
#define HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
namespace hpp {
namespace pinocchio {
namespace liegroup {
template<int N>
struct SpecialEuclideanOperation : public se3::SpecialEuclideanOperation<N>
{
typedef se3::SpecialEuclideanOperation<N> Base;
enum {
NT = N,
NR = Base::NV - N,
BoundSize = NT
};
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return Base::squaredDistance(q0, q1);
}
// Intentionally not implemented as it does not make sense.
/*
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const typename ConfigL_t::Scalar& w)
{
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<NT, false>,
liegroup::SpecialOrthogonalOperation<N>
> type;
return type::squaredDistance (q0, q1, w);
}
*/
template <class ConfigIn_t, class ConfigOut_t>
static void setBound(
const Eigen::MatrixBase<ConfigIn_t > & bound,
const Eigen::MatrixBase<ConfigOut_t> & out)
{
if (bound.size() == Base::NQ || bound.size() == BoundSize) {
const_cast<Eigen::MatrixBase<ConfigOut_t>&>(out).head(bound.size()) = bound;
} else {
HPP_THROW(std::invalid_argument, "Expected vector of size "
<< BoundSize << " or " << Base::NQ
<< ", got size " << bound.size());
}
}
template <class JacobianIn_t, class JacobianOut_t>
static void getRotationSubJacobian(
const Eigen::MatrixBase<JacobianIn_t > & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout)
{
const_cast<Eigen::MatrixBase<JacobianOut_t>&> (Jout) = Jin.template bottomLeftCorner<3,3>();
}
template <class ConfigIn_t>
static bool isNormalized(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.template tail<4>().norm() - 1) < eps );
}
};
} // namespace liegroup
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_LIEGROUP_SPECIAL_EUCLIDEAN_OPERATION_HH
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