Commit 459e15f6 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update to new API of pinocchio:

* Use R3 x SO3 instead of SE3
* Use R2 x SO2 instead of SE2
* Introduce weights
parent fd453ade
......@@ -80,6 +80,11 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/center-of-mass-computation.hh
include/hpp/pinocchio/simple-device.hh
include/hpp/pinocchio/liegroup.hh
include/hpp/pinocchio/liegroup/vector-space.hh
include/hpp/pinocchio/liegroup/cartesian-product.hh
include/hpp/pinocchio/liegroup/special-orthogonal.hh
include/hpp/pinocchio/urdf/util.hh
)
......
// Copyright (c) 2017, 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_HH
#define HPP_PINOCCHIO_LIEGROUP_HH
#include <pinocchio/multibody/joint/fwd.hpp>
// #include <pinocchio/multibody/liegroup/liegroup.hpp>
#include <hpp/pinocchio/liegroup/vector-space.hh>
#include <hpp/pinocchio/liegroup/cartesian-product.hh>
#include <hpp/pinocchio/liegroup/special-orthogonal.hh>
namespace hpp {
namespace pinocchio {
// Default implementation is empty
struct LieGroupTpl {
template<typename JointModel> struct operation {};
};
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<int Axis> struct LieGroupTpl::operation <se3::JointModelRevolute<Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<int Axis> struct LieGroupTpl::operation <se3::JointModelRevoluteUnbounded<Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelRevoluteUnaligned > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
template<int Axis> struct LieGroupTpl::operation <se3::JointModelPrismatic<Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelPrismaticUnaligned > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelTranslation > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<> struct LieGroupTpl::operation <se3::JointModelSpherical> {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelSphericalZYX> {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<> struct LieGroupTpl::operation <se3::JointModelFreeFlyer> {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<3, false>,
liegroup::SpecialOrthogonalOperation<3>
> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelPlanar> {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<2, false>,
liegroup::SpecialOrthogonalOperation<2>
> type;
};
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_LIEGROUP_HH
// Copyright (c) 2017, 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_CARTESIAN_PRODUCT_OPERATION_HH
#define HPP_PINOCCHIO_LIEGROUP_CARTESIAN_PRODUCT_OPERATION_HH
#include <pinocchio/multibody/liegroup/cartesian-product.hpp>
namespace hpp {
namespace pinocchio {
namespace liegroup {
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation : public se3::CartesianProductOperation<LieGroup1, LieGroup2>
{
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::CartesianProductOperation<LieGroup1,LieGroup2>::squaredDistance(q0, q1);
}
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)
{
return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), w)
+ LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), w);
}
}; // struct CartesianProductOperation
} // namespace liegroup
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_LIEGROUP_CARTESIAN_PRODUCT_OPERATION_HH
// Copyright (c) 2017, 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_ORTHOGONAL_OPERATION_HH
#define HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH
#include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
namespace hpp {
namespace pinocchio {
namespace liegroup {
template<int N>
struct SpecialOrthogonalOperation : public se3::SpecialOrthogonalOperation<N>
{
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::SpecialOrthogonalOperation<N>::squaredDistance(q0, q1);
}
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)
{
return w * squaredDistance_impl(q0, q1);
}
};
} // namespace liegroup
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_LIEGROUP_SPECIAL_ORTHOGONAL_OPERATION_HH
// Copyright (c) 2017, 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_VECTOR_SPACE_OPERATION_HH
#define HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
#include <pinocchio/multibody/liegroup/vector-space.hpp>
namespace hpp {
namespace pinocchio {
namespace liegroup {
template<int Size, bool rot>
struct VectorSpaceOperation : public se3::VectorSpaceOperation<Size>
{
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::VectorSpaceOperation<Size>::squaredDistance(q0, q1);
}
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)
{
if (rot) return w * squaredDistance_impl(q0, q1);
else return squaredDistance_impl(q0, q1);
}
};
} // namespace liegroup
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_LIEGROUP_VECTOR_SPACE_OPERATION_HH
......@@ -18,6 +18,7 @@
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup.hh>
namespace hpp {
namespace pinocchio {
......@@ -40,7 +41,7 @@ namespace hpp {
vectorIn_t velocity, ConfigurationOut_t result)
{
const se3::Model& model = robot->model();
result.head(model.nq) = se3::integrate(model, configuration, velocity);
result.head(model.nq) = se3::integrate<LieGroupTpl>(model, configuration, velocity);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = configuration.tail (dim) + velocity.tail (dim);
if (saturateConfig) saturate(robot, result);
......@@ -59,7 +60,7 @@ namespace hpp {
const value_type& u,
ConfigurationOut_t result)
{
result = se3::interpolate(robot->model(), q0, q1, u);
result = se3::interpolate<LieGroupTpl>(robot->model(), q0, q1, u);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
......@@ -67,7 +68,7 @@ namespace hpp {
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = se3::differentiate(robot->model(), q2, q1);
result = se3::differentiate<LieGroupTpl>(robot->model(), q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
......@@ -75,8 +76,7 @@ namespace hpp {
bool isApprox (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, value_type eps)
{
// TODO add precision argument in se3::isSameConfiguration
if (!se3::isSameConfiguration(robot->model(), q1, q2)) return false;
if (!se3::isSameConfiguration<LieGroupTpl>(robot->model(), q1, q2, eps)) return false;
const size_type& dim = robot->extraConfigSpace().dimension();
return q2.tail (dim).isApprox (q1.tail (dim), eps);
}
......@@ -84,10 +84,10 @@ namespace hpp {
value_type distance (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2)
{
vector_t dist = se3::distance(robot->model(), q1, q2);
vector_t dist = se3::squaredDistance<LieGroupTpl>(robot->model(), q1, q2);
const size_type& dim = robot->extraConfigSpace().dimension();
if (dim == 0) return dist.norm();
else return sqrt (dist.squaredNorm() + (q2.tail (dim) - q1.tail (dim)).squaredNorm ());
if (dim == 0) return sqrt(dist.sum());
else return sqrt (dist.sum() + (q2.tail (dim) - q1.tail (dim)).squaredNorm ());
}
void normalize (const DevicePtr_t& robot, Configuration_t& q)
......
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