Unverified Commit 80268435 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #159 from florent-lamiraux/pr

[Liegroup] Remove overload of method isNormalized in SpecialEuclidean
parents 43f619b6 4b11c644
Pipeline #18588 passed with stage
in 6 minutes and 5 seconds
......@@ -31,6 +31,7 @@
#include <hpp/pinocchio/deprecated.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <pinocchio/math/quaternion.hpp>
namespace hpp {
namespace pinocchio {
......@@ -203,13 +204,24 @@ LiegroupElement operator+(const LiegroupElementConstBase<vector_type>& e,
/// Difference between two configurations
///
/// \param e1, e2 elements of the Lie group,
/// \return the velocity that integrated from e2 yiels e1
/// \return the velocity that integrated from e2 yields e1
///
/// By extension of the vector space case, we represent the integration
/// of a constant velocity during unit time by an addition
template <typename vector_type1, typename vector_type2>
vector_t operator-(const LiegroupElementConstBase<vector_type1>& e1,
const LiegroupElementConstBase<vector_type2>& e2);
/// Check if a configuration is normalized
///
/// \param e1 configuration to be checked
/// \param eps the error threshold
/// \return whether the configuration is normalized
///
template <typename vector_type>
bool checkNormalized(
const LiegroupElementConstBase<vector_type>& e1,
const value_type& eps = PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE);
/// \}
/// Compute the log as a tangent vector of a Lie group element
......
......@@ -82,13 +82,6 @@ struct SpecialEuclideanOperation
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
......
// Copyright (c) 2022, CNRS
// Authors: Florent Lamiraux
// Le Quang Anh
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
#ifndef HPP_PINOCCHIO_SRC_IS_NORMALIZED_VISITOR_HH
#define HPP_PINOCCHIO_SRC_IS_NORMALIZED_VISITOR_HH
namespace hpp {
namespace pinocchio {
namespace liegroupType {
/// Is Normalized visitor
template <typename vector_type>
struct IsNormalizedVisitor : public boost::static_visitor<> {
IsNormalizedVisitor(const vector_type& e1, const value_type& eps, bool& res)
: e1_(e1), iq_(0), eps_(eps), result(res) {}
template <typename LiegroupType>
void operator()(LiegroupType& op) {
result &= op.isNormalized(
e1_.template segment<LiegroupType::NQ>(iq_, op.nq()), eps_);
iq_ += op.nq();
}
const vector_type& e1_;
size_type iq_;
const value_type& eps_;
bool& result;
}; // struct IsNormalizedVisitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_IS_NORMALIZED_VISITOR_HH
......@@ -33,6 +33,7 @@
#include <hpp/util/serialization.hh>
#include "../src/addition-visitor.hh"
#include "../src/is-normalized-visitor.hh"
#include "../src/log-visitor.hh"
#include "../src/size-visitor.hh"
#include "../src/substraction-visitor.hh"
......@@ -112,6 +113,27 @@ template vector_t operator-(const LiegroupElementConstBase<vectorOut_t>& e1,
template vector_t operator-(const LiegroupElementConstBase<vectorOut_t>& e1,
const LiegroupElementConstBase<vectorOut_t>& e2);
template <typename vector_type>
bool checkNormalized(const LiegroupElementConstBase<vector_type>& e1,
const value_type& eps) {
bool result = true;
liegroupType::IsNormalizedVisitor<vector_type> isNormalizedvisitor(
e1.vector(), eps, result);
for (LiegroupTypes::const_iterator it = e1.space()->liegroupTypes().begin();
it != e1.space()->liegroupTypes().end(); ++it) {
boost::apply_visitor(isNormalizedvisitor, *it);
}
return result;
}
template bool checkNormalized(const LiegroupElementConstBase<vector_t>& e1,
const value_type& eps);
template bool checkNormalized(const LiegroupElementConstBase<vectorIn_t>& e1,
const value_type& eps);
template bool checkNormalized(const LiegroupElementConstBase<vectorOut_t>& e1,
const value_type& eps);
template <typename vector_type>
vector_t log(const LiegroupElementConstBase<vector_type>& lge) {
using liegroupType::LogVisitor;
......
......@@ -52,7 +52,7 @@ struct Interpolate : public boost::static_visitor<> {
value_type u_;
vectorOut_t& er_;
size_type iq_, iv_;
}; // struct SubstractionVisitor
}; // struct Interpolate Visitor
} // namespace visitor
} // namespace liegroupType
} // namespace pinocchio
......
......@@ -43,6 +43,7 @@
#include <hpp/pinocchio/humanoid-robot.hh>
#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/pinocchio/simple-device.hh>
#include <pinocchio/algorithm/joint-configuration.hpp>
......@@ -130,25 +131,35 @@ BOOST_AUTO_TEST_CASE(is_valid_configuration) {
Configuration_t q = robot->neutralConfiguration();
BOOST_CHECK(isNormalized(robot, q, eps));
LiegroupElementConstRef P1(q, robot->configSpace());
BOOST_CHECK(checkNormalized(P1, eps));
/// Set a quaternion of norm != 1
q[3] = 1;
q[4] = 1;
BOOST_CHECK(!isNormalized(robot, q, eps));
LiegroupElementConstRef P2(q, robot->configSpace());
BOOST_CHECK(!checkNormalized(P2, eps));
robot = unittest::makeDevice(unittest::CarLike);
q = robot->neutralConfiguration();
BOOST_CHECK(isNormalized(robot, q, eps));
LiegroupElementConstRef P3(q, robot->configSpace());
BOOST_CHECK(checkNormalized(P3, eps));
/// Set a complex of norm != 1
q.segment<2>(2) << 1, 1;
BOOST_CHECK(!isNormalized(robot, q, eps));
LiegroupElementConstRef P4(q, robot->configSpace());
BOOST_CHECK(!checkNormalized(P4, eps));
robot = unittest::makeDevice(unittest::CarLike);
q = robot->neutralConfiguration();
BOOST_CHECK(isNormalized(robot, q, eps));
LiegroupElementConstRef P5(q, robot->configSpace());
BOOST_CHECK(checkNormalized(P5, eps));
}
void test_difference_and_distance(DevicePtr_t robot) {
......
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