Unverified Commit fe7b0e19 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #61 from jmirabel/devel

Add derivative of the difference operation.
parents a3e02b65 0b65bcbf
......@@ -88,6 +88,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
......
......@@ -197,7 +197,7 @@ namespace hpp {
size_type numberDof () const;
/// Returns a LiegroupSpace representing the configuration space.
LiegroupSpaceConstPtr_t configSpace () const { return configSpace_; }
const LiegroupSpacePtr_t& configSpace () const { return configSpace_; }
/// \}
// -----------------------------------------------------------------------
......
......@@ -116,7 +116,8 @@ namespace hpp {
typedef boost::shared_ptr<GeomData> GeomDataPtr_t;
typedef boost::shared_ptr<const GeomData> GeomDataConstPtr_t;
class LiegroupElement;
template <typename vector_type> class LiegroupNonconstElementBase;
typedef LiegroupNonconstElementBase< vector_t> LiegroupElement;
HPP_PREDEF_CLASS (LiegroupSpace);
typedef boost::shared_ptr <LiegroupSpace> LiegroupSpacePtr_t;
typedef boost::shared_ptr <const LiegroupSpace> LiegroupSpaceConstPtr_t;
......
......@@ -25,49 +25,29 @@ namespace hpp {
/// \addtogroup liegroup
/// \{
/// Element of a Lie group
///
/// See class LiegroupSpace.
class LiegroupElement
template <typename vector_type>
class LiegroupElementBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
friend LiegroupElement operator+
(const LiegroupElement& e, const vector_t& v);
friend vector_t operator-
(const LiegroupElement& e1, const LiegroupElement& e2);
/// Constructor
/// \param value vector representation,
/// \param liegroupSpace space the element belongs to.
LiegroupElement (const vector_t& value,
const LiegroupSpacePtr_t& liegroupSpace) :
value_ (value), space_ (liegroupSpace)
template <typename Derived>
LiegroupElementBase (const Eigen::EigenBase<Derived>& value,
const LiegroupSpacePtr_t& liegroupSpace) :
value_ (value.derived()), space_ (liegroupSpace)
{
assert (value_.size () == space_->nq ());
}
/// Constructor
/// \param liegroupSpace space the element belongs to.
LiegroupElement (const LiegroupSpacePtr_t& liegroupSpace) :
value_ (liegroupSpace->nq ()), space_ (liegroupSpace)
{
assert (value_.size () == space_->nq ());
check();
}
/// Constructor
/// \param value vector representation,
///
/// By default the space containing the value is a vector space.
explicit LiegroupElement (const vector_t& value) :
value_ (value), space_ (LiegroupSpace::create (value.size ()))
{
}
/// Constructor of trivial element
LiegroupElement () :
value_ (), space_ (LiegroupSpace::empty ())
{
value_.resize (0);
}
template <typename Derived>
explicit LiegroupElementBase (const Eigen::EigenBase<Derived>& value) :
value_ (value.derived()),
space_ (LiegroupSpace::create (value.derived().size ())) {}
/// get reference to vector of Lie groups
const LiegroupSpacePtr_t& space () const
......@@ -76,13 +56,7 @@ namespace hpp {
}
/// Const vector representation
const vector_t& vector () const
{
return value_;
}
/// Modifiable vector representation
vectorOut_t vector ()
const vector_type& vector () const
{
return value_;
}
......@@ -93,9 +67,6 @@ namespace hpp {
return value_.size ();
}
/// Set element to neutral element
void setNeutral ();
/// Check that size of vector fits size of space
/// \note only in debug mode
void check () const
......@@ -103,13 +74,75 @@ namespace hpp {
assert (value_.size () == space_->nq ());
}
/// Inplace integration of a velocity vector
LiegroupElement& operator+= (const vectorIn_t& v);
private:
vector_t value_;
protected:
vector_type value_;
LiegroupSpacePtr_t space_;
}; // class LiegroupElement
};
typedef LiegroupElementBase<vectorIn_t> LiegroupConstElementRef;
template <typename vector_type>
class LiegroupNonconstElementBase : public LiegroupElementBase<vector_type>
{
public:
typedef LiegroupElementBase<vector_type> Base;
/// Constructor
/// \param value vector representation,
/// \param liegroupSpace space the element belongs to.
LiegroupNonconstElementBase (const vector_type& value,
const LiegroupSpacePtr_t& space)
: Base (value, space) {}
/// Constructor
/// \param value vector representation,
///
/// By default the space containing the value is a vector space.
LiegroupNonconstElementBase (const LiegroupSpacePtr_t& space) :
Base (vector_t (space->nq ()), space) {}
/// Constructor
/// \param value vector representation,
///
/// By default the space containing the value is a vector space.
explicit LiegroupNonconstElementBase (const vector_type& value)
: Base (value) {}
/// Copy constructor
template <typename vector_type2>
LiegroupNonconstElementBase (const LiegroupElementBase<vector_type2>& other)
: Base (other.vector(), other.space()) {}
/// Constructor of trivial element
LiegroupNonconstElementBase () : Base (vector_t(), LiegroupSpace::empty ()) {}
/// Const vector representation
const vector_type& vector () const
{
return Base::vector();
}
/// Modifiable vector representation
vector_type& vector ()
{
return this->value_;
}
/// Set element to neutral element
void setNeutral ()
{
this->value_ = this->space_->neutral ().vector ();
}
/// Inplace integration of a velocity vector
LiegroupNonconstElementBase& operator+= (vectorIn_t v);
};
/// Element of a Lie group
///
/// See class LiegroupSpace.
typedef LiegroupNonconstElementBase< vector_t> LiegroupElement;
typedef LiegroupNonconstElementBase<vectorOut_t> LiegroupElementRef;
/// Integration of a velocity vector from a configuration
///
......@@ -119,7 +152,8 @@ namespace hpp {
///
/// By extension of the vector space case, we represent the integration
/// of a constant velocity during unit time by an addition
LiegroupElement operator+ (const LiegroupElement& e, const vector_t& v);
template <typename vector_type>
LiegroupElement operator+ (const LiegroupElementBase<vector_type>& e, vectorIn_t v);
/// Difference between two configurations
///
......@@ -128,13 +162,16 @@ namespace hpp {
///
/// By extension of the vector space case, we represent the integration
/// of a constant velocity during unit time by an addition
vector_t operator- (const LiegroupElement& e1, const LiegroupElement& e2);
template <typename vector_type1, typename vector_type2>
vector_t operator- (const LiegroupElementBase<vector_type1>& e1, const LiegroupElementBase<vector_type2>& e2);
/// \}
/// Compute the log as a tangent vector of a Lie group element
vector_t log (const LiegroupElement& lge);
template <typename vector_type>
vector_t log (const LiegroupElementBase<vector_type>& lge);
inline std::ostream& operator<< (std::ostream& os, const LiegroupElement& e)
template <typename vector_type>
inline std::ostream& operator<< (std::ostream& os, const LiegroupElementBase<vector_type>& e)
{
os << "Lie group element in " << *(e.space ())
<< " represented by vector (" << e. vector ().transpose () << ")";
......
......@@ -98,15 +98,6 @@ namespace hpp {
static LiegroupSpacePtr_t empty ();
/// \}
/// Create instance of empty space
static LiegroupSpacePtr_t create ()
{
LiegroupSpace* ptr (new LiegroupSpace ());
LiegroupSpacePtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Create instance of vector space of given size
static LiegroupSpacePtr_t create (const size_type& size)
{
......@@ -175,6 +166,18 @@ namespace hpp {
/// \param[out] J the Jacobian of y
void Jintegrate (vectorIn_t v, matrixOut_t J) const;
/// Compute the Jacobian of the difference operation.
/// Given \f$ v = q1 - q0 \f$,
///
/// \param[in] q0,q1
/// \param[in] J0 the Jacobian of q0.
/// \param[in] J1 the Jacobian of q1.
/// \param[out] J0 the Jacobian of v with respect to q0.
/// \param[out] J1 the Jacobian of v with respect to q1.
/// \note to compute only one jacobian, provide for J0 or J1 an empty matrix.
template <bool ApplyOnTheLeft>
void Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
/// Return name of Lie group
std::string name () const;
......@@ -187,14 +190,14 @@ namespace hpp {
protected:
/// Constructor of empty space
LiegroupSpace ();
/// Constructor of vector space of given size
LiegroupSpace (const size_type& size);
LiegroupSpace (const LiegroupSpace& other);
LiegroupSpace (const LiegroupType& type);
private:
/// Constructor of empty space
LiegroupSpace ();
/// Initialize weak pointer to itself
void init (const LiegroupSpaceWkPtr_t weak);
/// Compute size of space
......
......@@ -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
......
......@@ -29,8 +29,8 @@ namespace hpp {
{
enum {
BoundSize = LieGroup1::BoundSize + LieGroup2::BoundSize,
NR = LieGroup1::BoundSize + LieGroup2::BoundSize,
NT = LieGroup1::BoundSize + LieGroup2::BoundSize
NR = LieGroup1::NR + LieGroup2::NR,
NT = LieGroup1::NT + LieGroup2::NT
};
typedef se3::CartesianProductOperation<LieGroup1, LieGroup2> Base;
......
// 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
......@@ -107,6 +107,15 @@ namespace hpp {
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template void integrate<true, DefaultLieGroupMap>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
template void integrate<false, DefaultLieGroupMap>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result);
// TODO remove me. This is kept for backward compatibility
template void integrate<true, se3::LieGroupTpl>
(const DevicePtr_t& robot,
ConfigurationIn_t configuration,
......@@ -120,7 +129,7 @@ namespace hpp {
ConfigurationIn_t configuration,
vectorIn_t velocity, ConfigurationOut_t result)
{
integrate<true, se3::LieGroupTpl> (robot, configuration, velocity, result);
integrate<true, DefaultLieGroupMap> (robot, configuration, velocity, result);
}
template <typename LieGroup>
......@@ -135,6 +144,12 @@ namespace hpp {
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
template void interpolate<DefaultLieGroupMap> (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
const value_type& u,
ConfigurationOut_t result);
// TODO remove me. This is kept for backward compatibility
template void interpolate<se3::LieGroupTpl> (const DevicePtr_t& robot,
ConfigurationIn_t q0,
ConfigurationIn_t q1,
......@@ -159,6 +174,11 @@ namespace hpp {
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
template void difference <DefaultLieGroupMap> (const DevicePtr_t& robot,
ConfigurationIn_t q1,
ConfigurationIn_t q2,
vectorOut_t result);
// TODO remove me. This is kept for backward compatibility
template void difference <se3::LieGroupTpl> (const DevicePtr_t& robot,
ConfigurationIn_t q1,
ConfigurationIn_t q2,
......
......@@ -263,7 +263,7 @@ namespace hpp {
static void algo(const se3::JointModelBase<JointModel> &,
LiegroupSpace& space)
{
typedef typename LieGroupTpl::operation<JointModel>::type LG_t;
typedef typename DefaultLieGroupMap::operation<JointModel>::type LG_t;
space *= LiegroupSpace::create (LG_t());
}
......@@ -293,7 +293,7 @@ namespace hpp {
currentVelocity_.resize(numberDof());
currentAcceleration_.resize(numberDof());
configSpace_ = LiegroupSpace::create();
configSpace_ = LiegroupSpace::empty();
const Model& m (model());
ConfigSpaceVisitor::ArgsType args (*configSpace_);
for (JointIndex i = 1; i < m.joints.size(); ++i)
......
// Copyright (c) 2018, CNRS
// 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_SRC_JDIFFERENCE_VISITOR_HH
# define HPP_PINOCCHIO_SRC_JDIFFERENCE_VISITOR_HH
# include <hpp/pinocchio/liegroup/vector-space.hh>
namespace hpp {
namespace pinocchio {
namespace liegroupType {
template <bool ApplyOnTheLeft>
struct JdifferenceVisitor : public boost::static_visitor <>
{
JdifferenceVisitor (vectorIn_t& q0, vectorIn_t& q1,
matrixOut_t& J0, matrixOut_t& J1)
: q0_ (q0)
, q1_ (q1)
, J0_ (J0)
, J1_ (J1)
, iq_ (0)
, iv_ (0)
{}
template <typename LgT> void operator () (const LgT& lg)
{
typename LgT::JacobianMatrix_t J0int (lg.nv(), lg.nv());
typename LgT::JacobianMatrix_t J1int (lg.nv(), lg.nv());
lg.Jdifference (
q0_.segment<LgT::NQ>(iq_, lg.nq()),
q1_.segment<LgT::NQ>(iq_, lg.nq()),
J0int,
J1int);
if (J0_.size() > 0) {
if (ApplyOnTheLeft)
J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
else
J0_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J0int);
}
if (J1_.size() > 0) {
if (ApplyOnTheLeft)
J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
else
J1_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J1int);
}
iq_ += lg.nq();
iv_ += lg.nv();
}
template <int N, bool rot>
void operator () (const liegroup::VectorSpaceOperation<N,rot>& lg)
{
if (J0_.size() > 0) {
if (ApplyOnTheLeft)
J0_.middleRows<N> (iv_, lg.nv()) *= -1;
else
J0_.middleCols<N> (iv_, lg.nv()) *= -1;
}
iq_ += lg.nq();
iv_ += lg.nv();
}
vectorIn_t & q0_, q1_;
matrixOut_t& J0_, J1_;
size_type iq_, iv_;
}; // struct JdifferenceVisitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_JDIFFERENCE_VISITOR_HH
......@@ -24,22 +24,18 @@ namespace hpp {
namespace pinocchio {
typedef std::vector <LiegroupType> LiegroupTypes;
void LiegroupElement::setNeutral ()
template <typename vector_type>
LiegroupNonconstElementBase<vector_type>& LiegroupNonconstElementBase<vector_type>::operator+= (vectorIn_t v)
{
value_ = space_->neutral ().vector ();