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

Merge pull request #82 from jmirabel/devel

Update to pinocchio v2.
parents 7bb9b9c2 750e72fc
......@@ -74,6 +74,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/pinocchio/device-sync.hh
include/hpp/pinocchio/humanoid-robot.hh
include/hpp/pinocchio/joint.hh
include/hpp/pinocchio/joint-collection.hh
include/hpp/pinocchio/frame.hh
include/hpp/pinocchio/body.hh
include/hpp/pinocchio/gripper.hh
......
......@@ -42,10 +42,6 @@ namespace hpp {
class HPP_PINOCCHIO_DLLAPI Body
{
public:
typedef se3::JointIndex JointIndex;
typedef se3::FrameIndex FrameIndex;
/// \name Construction and copy and destruction
/// @{
/// Constructor
......@@ -102,10 +98,10 @@ namespace hpp {
/// If frameIndex==-1 (after init), search in pinocchio frame list the proper index.
void searchFrameIndex() const;
const se3::Model& model() const ;
se3::Model& model() ;
const se3::Frame & frame() const ;
se3::Frame & frame() ;
const Model& model() const ;
Model& model() ;
const ::pinocchio::Frame & frame() const ;
::pinocchio::Frame & frame() ;
DeviceWkPtr_t devicePtr;
JointIndex jointIndex;
......
......@@ -19,7 +19,7 @@
# include <list>
# include <pinocchio/multibody/data.hpp> // se3::Data
# include <pinocchio/multibody/data.hpp> // ::pinocchio::Data
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device.hh>
......@@ -43,7 +43,7 @@ namespace hpp {
public:
typedef std::vector <JointIndex> JointRootIndexes_t;
/// \cond
// This fixes an alignment issue of se3::Data::hg
// This fixes an alignment issue of ::pinocchio::Data::hg
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \endcond
......
......@@ -28,13 +28,15 @@
# include <hpp/pinocchio/config.hh>
# include <hpp/pinocchio/fwd.hh>
namespace se3
namespace pinocchio
{
struct GeometryObject;
}
namespace hpp {
namespace pinocchio {
typedef ::pinocchio::GeometryObject GeometryObject;
/// Specialization of fcl::CollisionObject to add a name to objects
///
/// Objects moved by a robot joint. They can collide each other and
......@@ -58,8 +60,8 @@ namespace hpp {
const std::string& name () const;
/// Access to pinocchio object
const se3::GeometryObject & pinocchio () const;
se3::GeometryObject & pinocchio ();
const GeometryObject & pinocchio () const;
GeometryObject & pinocchio ();
/// Access to fcl object
FclConstCollisionObjectPtr_t fcl (const GeomData& data) const;
......
......@@ -157,7 +157,7 @@ namespace hpp {
}
/// Write a SE3 taking into account the indentation
std::ostream& display (std::ostream& os, const se3::SE3& m);
std::ostream& display (std::ostream& os, const SE3& m);
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_CONFIGURATION_HH
......@@ -32,7 +32,7 @@ namespace hpp {
JACOBIAN = 0x2,
VELOCITY = 0x4,
ACCELERATION = 0x8,
COM = 0x10,
COM = 0x16,
COMPUTE_ALL = 0Xffff
};
......
......@@ -113,7 +113,7 @@ namespace hpp {
/// Select computation
/// Optimize computation time by selecting only necessary values in
/// method computeForwardKinematics.
void controlComputation (const Computation_t& flag);
virtual void controlComputation (const Computation_t& flag);
/// Get computation flag
Computation_t computationFlag () const { return d().computationFlag_; }
/// Compute forward kinematics
......
......@@ -277,6 +277,8 @@ namespace hpp {
/// - sum all the BB obtained.
fcl::AABB computeAABB() const;
void controlComputation (const Computation_t& flag);
protected:
/// \brief Constructor
Device(const std::string& name);
......
......@@ -66,7 +66,7 @@ namespace hpp {
/// \name Kinematic chain
/// \{
/// Returns true if the frame type is se3::FIXED_JOINT
/// Returns true if the frame type is ::pinocchio::FIXED_JOINT
bool isFixed () const;
/// Returns the joint associated to this frame
......@@ -114,7 +114,7 @@ namespace hpp {
return frameIndex_;
}
const se3::Frame& pinocchio() const;
const ::pinocchio::Frame& pinocchio() const;
/// \}
......@@ -123,7 +123,7 @@ namespace hpp {
FrameIndex frameIndex_;
std::vector<FrameIndex> children_;
se3::Frame& pinocchio();
::pinocchio::Frame& pinocchio();
/// Store list of childrens.
void setChildList();
......
......@@ -33,6 +33,8 @@
namespace hpp {
namespace pinocchio {
typedef double value_type;
HPP_PREDEF_CLASS (Body);
HPP_PREDEF_CLASS (CollisionObject);
HPP_PREDEF_CLASS (Device);
......@@ -50,19 +52,22 @@ namespace hpp {
enum InOutType { INNER, OUTER };
// Pinocchio typedefs
typedef se3::JointIndex JointIndex;
typedef se3::FrameIndex FrameIndex;
typedef se3::GeomIndex GeomIndex;
typedef se3::Model Model;
typedef se3::Data Data;
typedef se3::GeometryModel GeomModel;
typedef se3::GeometryData GeomData;
typedef se3::SE3 Transform3f;
typedef se3::JointModel JointModel;
template <typename _Scalar, int _Options> struct JointCollectionTpl;
typedef JointCollectionTpl<value_type, 0> JointCollection;
typedef ::pinocchio::JointIndex JointIndex;
typedef ::pinocchio::FrameIndex FrameIndex;
typedef ::pinocchio::GeomIndex GeomIndex;
typedef ::pinocchio::ModelTpl<value_type, 0, JointCollectionTpl> Model;
typedef ::pinocchio::DataTpl <value_type, 0, JointCollectionTpl> Data ;
typedef ::pinocchio::GeometryModel GeomModel;
typedef ::pinocchio::GeometryData GeomData;
typedef ::pinocchio::SE3 Transform3f;
typedef ::pinocchio::SE3 SE3;
typedef ::pinocchio::JointModelTpl<value_type, 0, JointCollectionTpl> JointModel;
typedef Eigen::Array <bool, Eigen::Dynamic, 1> ArrayXb;
typedef double value_type;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> vector_t;
typedef vector_t Configuration_t;
typedef Eigen::Ref <const Configuration_t> ConfigurationIn_t;
......
......@@ -66,7 +66,7 @@ namespace hpp {
}
/// Get the frame Id of the gripper in the vector of frame of the Model
const se3::FrameIndex& frameId () const
const FrameIndex& frameId () const
{
return fid_;
}
......@@ -122,7 +122,7 @@ namespace hpp {
DeviceWkPtr_t device_;
/// Joint of the robot that holds handles.
JointPtr_t joint_;
se3::FrameIndex fid_;
FrameIndex fid_;
/// Clearance
value_type clearance_;
/// Weak pointer to itself
......
//
// Copyright (c) 2018 CNRS
// Author: Joseph Mirabel
//
//
// 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_JOINT_COLLECTION_HH
#define HPP_PINOCCHIO_JOINT_COLLECTION_HH
#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
#include "pinocchio/multibody/joint/joint-planar.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
//#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
//#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
namespace hpp {
namespace pinocchio {
template<typename _Scalar, int _Options>
struct JointCollectionTpl
{
typedef _Scalar Scalar;
enum { Options = _Options };
// Joint Revolute
typedef ::pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
typedef ::pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
typedef ::pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;
// Joint Revolute Unaligned
typedef ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelRevoluteUnaligned;
// Joint Revolute UBounded
typedef ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,0> JointModelRUBX;
typedef ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,1> JointModelRUBY;
typedef ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar,Options,2> JointModelRUBZ;
// Joint Prismatic
typedef ::pinocchio::JointModelPrismaticTpl<Scalar,Options,0> JointModelPX;
typedef ::pinocchio::JointModelPrismaticTpl<Scalar,Options,1> JointModelPY;
typedef ::pinocchio::JointModelPrismaticTpl<Scalar,Options,2> JointModelPZ;
// Joint Prismatic Unaligned
typedef ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelPrismaticUnaligned;
// Joint Spherical
typedef ::pinocchio::JointModelSphericalTpl<Scalar,Options> JointModelSpherical;
// Joint Spherical ZYX
typedef ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> JointModelSphericalZYX;
// Joint Translation
typedef ::pinocchio::JointModelTranslationTpl<Scalar,Options> JointModelTranslation;
// Joint FreeFlyer
typedef ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> JointModelFreeFlyer;
// Joint Planar
typedef ::pinocchio::JointModelPlanarTpl<Scalar,Options> JointModelPlanar;
// Joint Composite
typedef ::pinocchio::JointModelCompositeTpl<Scalar,Options,pinocchio::JointCollectionTpl> JointModelComposite;
typedef boost::variant<
// JointModelVoid,
JointModelRX, JointModelRY, JointModelRZ
, JointModelFreeFlyer, JointModelPlanar
, JointModelRevoluteUnaligned
, JointModelPX, JointModelPY, JointModelPZ
, JointModelPrismaticUnaligned
, JointModelTranslation
, JointModelRUBX, JointModelRUBY, JointModelRUBZ
> JointModelVariant;
// Joint Revolute
typedef ::pinocchio::JointDataRevoluteTpl<Scalar,Options,0> JointDataRX;
typedef ::pinocchio::JointDataRevoluteTpl<Scalar,Options,1> JointDataRY;
typedef ::pinocchio::JointDataRevoluteTpl<Scalar,Options,2> JointDataRZ;
// Joint Revolute Unaligned
typedef ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataRevoluteUnaligned;
// Joint Revolute UBounded
typedef ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,0> JointDataRUBX;
typedef ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,1> JointDataRUBY;
typedef ::pinocchio::JointDataRevoluteUnboundedTpl<Scalar,Options,2> JointDataRUBZ;
// Joint Prismatic
typedef ::pinocchio::JointDataPrismaticTpl<Scalar,Options,0> JointDataPX;
typedef ::pinocchio::JointDataPrismaticTpl<Scalar,Options,1> JointDataPY;
typedef ::pinocchio::JointDataPrismaticTpl<Scalar,Options,2> JointDataPZ;
// Joint Prismatic Unaligned
typedef ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> JointDataPrismaticUnaligned;
// Joint Spherical
typedef ::pinocchio::JointDataSphericalTpl<Scalar,Options> JointDataSpherical;
// Joint Spherical ZYX
typedef ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> JointDataSphericalZYX;
// Joint Translation
typedef ::pinocchio::JointDataTranslationTpl<Scalar,Options> JointDataTranslation;
// Joint FreeFlyer
typedef ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> JointDataFreeFlyer;
// Joint Planar
typedef ::pinocchio::JointDataPlanarTpl<Scalar,Options> JointDataPlanar;
// Joint Composite
typedef ::pinocchio::JointDataCompositeTpl<Scalar,Options,pinocchio::JointCollectionTpl> JointDataComposite;
typedef boost::variant<
// JointDataVoid
JointDataRX, JointDataRY, JointDataRZ
, JointDataFreeFlyer, JointDataPlanar
, JointDataRevoluteUnaligned
, JointDataPX, JointDataPY, JointDataPZ
, JointDataPrismaticUnaligned
, JointDataTranslation
, JointDataRUBX, JointDataRUBY, JointDataRUBZ
> JointDataVariant;
};
typedef JointCollection::JointModelVariant JointModelVariant;
typedef JointCollection::JointDataVariant JointDataVariant;
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_JOINT_COLLECTION_HH
......@@ -171,6 +171,25 @@ namespace hpp {
/// Inplace integration of a velocity vector
LiegroupElementBase& operator+= (vectorIn_t v);
/// Assignment from another LiegroupElement
template <typename vector_type2>
LiegroupElementBase& operator= (const LiegroupElementConstBase<vector_type2>& other)
{
this->space_ = other.space();
this->value_ = other.vector();
return *this;
}
/// Assignment from a vector
template <typename Vector>
LiegroupElementBase& operator= (const Eigen::MatrixBase<Vector>& v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector);
assert (this->space_->nq() == v.derived().size());
this->value_.noalias() = v.derived();
return *this;
}
};
/// Integration of a velocity vector from a configuration
......
......@@ -59,8 +59,8 @@ namespace hpp {
liegroup::SpecialOrthogonalOperation<2> >,
liegroup::SpecialOrthogonalOperation <2>,
liegroup::SpecialOrthogonalOperation <3>,
se3::SpecialEuclideanOperation <2>,
se3::SpecialEuclideanOperation <3> >
liegroup::SpecialEuclideanOperation <2>,
liegroup::SpecialEuclideanOperation <3> >
LiegroupType;
#endif
......@@ -109,8 +109,12 @@ namespace hpp {
/// Return \f$SE(3)\f$
static LiegroupSpacePtr_t SE3 ();
/// Return \f$SO(2)\f$
static LiegroupSpacePtr_t R2xSO2 ();
static LiegroupSpacePtr_t SO2 ();
/// Return \f$SO(3)\f$
static LiegroupSpacePtr_t SO3 ();
/// Return \f$\mathbf{R}^2 \times SO(2)\f$
static LiegroupSpacePtr_t R2xSO2 ();
/// Return \f$\mathbf{R}^3 \times SO(3)\f$
static LiegroupSpacePtr_t R3xSO3 ();
/// Return empty Lie group
static LiegroupSpacePtr_t empty ();
......@@ -168,6 +172,15 @@ namespace hpp {
/// Return the neutral element as a vector
LiegroupElement neutral () const;
/// Create a LiegroupElement from a configuration.
LiegroupElement element (vectorIn_t q) const;
/// Create a LiegroupElementRef from a configuration.
LiegroupElementRef elementRef (vectorOut_t q) const;
/// Create a LiegroupElementRef from a configuration.
LiegroupElementConstRef elementConstRef (vectorIn_t q) const;
/// Return exponential of a tangent vector
LiegroupElement exp (vectorIn_t v) const;
......@@ -188,7 +201,7 @@ namespace hpp {
/// \note For each elementary Lie group in q.space (), ranging
/// over indices \f$[iq, iq+nq-1]\f$, the Jacobian
/// \f$J_{Lg} (q [iq:iq+nq])\f$ is computed by method
/// se3::LieGroupBase::dIntegrate_dq.
/// ::pinocchio::LieGroupBase::dIntegrate_dq.
/// lines \f$[iq:iq+nq]\f$ of Jq are then left multiplied by
/// \f$J_{Lg} (q [iq:iq+nq])\f$.
template <DerivativeProduct side>
......@@ -210,7 +223,7 @@ namespace hpp {
/// \note For each elementary Lie group in q.space (), ranging
/// over indices \f$[iv, iv+nv-1]\f$, the Jacobian
/// \f$J_{Lg} (q [iv:iv+nv])\f$ is computed by method
/// se3::LieGroupBase::dIntegrate_dq.
/// ::pinocchio::LieGroupBase::dIntegrate_dq.
/// lines \f$[iv:iv+nv]\f$ of Jv are then left multiplied by
/// \f$J_{Lg} (q [iv:iv+nv])\f$.
template <DerivativeProduct side>
......
......@@ -55,52 +55,52 @@ namespace hpp {
//---------------- RnxSOnLieGroupMap -------------------------------//
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<typename Scalar, int Options, int Axis>
struct RnxSOnLieGroupMap::operation <se3::JointModelRevoluteTpl<Scalar, Options, Axis> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelRevoluteTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<typename Scalar, int Options, int Axis>
struct RnxSOnLieGroupMap::operation <se3::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
// JointModelPrismaticTpl, JointModelPrismaticUnaligned, JointModelTranslation
template<typename Scalar, int Options, int Axis>
struct RnxSOnLieGroupMap::operation <se3::JointModelPrismatic<Scalar, Options, Axis> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelPrismaticTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelTranslationTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelTranslationTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelSphericalTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelSphericalTpl<Scalar, Options> > {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelSphericalZYXTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelSphericalZYXTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelFreeFlyerTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelFreeFlyerTpl<Scalar, Options> > {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<3, false>,
liegroup::SpecialOrthogonalOperation<3>
> type;
};
template<typename Scalar, int Options>
struct RnxSOnLieGroupMap::operation <se3::JointModelPlanarTpl<Scalar, Options> > {
struct RnxSOnLieGroupMap::operation < ::pinocchio::JointModelPlanarTpl<Scalar, Options> > {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<2, false>,
liegroup::SpecialOrthogonalOperation<2>
......@@ -111,49 +111,49 @@ namespace hpp {
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteTpl<Scalar, Options, Axis> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelRevoluteTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
// JointModelPrismaticTpl, JointModelPrismaticUnaligned, JointModelTranslation
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelPrismatic<Scalar, Options, Axis> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelPrismaticTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelTranslationTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelTranslationTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelSphericalTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelSphericalTpl<Scalar, Options> > {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelSphericalZYXTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelSphericalZYXTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelFreeFlyerTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelFreeFlyerTpl<Scalar, Options> > {
typedef liegroup::SpecialEuclideanOperation<3> type;
};
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelPlanarTpl<Scalar, Options> > {
struct DefaultLieGroupMap::operation < ::pinocchio::JointModelPlanarTpl<Scalar, Options> > {
typedef liegroup::SpecialEuclideanOperation<2> type;
};
/// \endcond
......
......@@ -25,7 +25,7 @@ namespace hpp {
namespace pinocchio {
namespace liegroup {
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation : public se3::CartesianProductOperation<LieGroup1, LieGroup2>
struct CartesianProductOperation : public ::pinocchio::CartesianProductOperation<LieGroup1, LieGroup2>
{
enum {
BoundSize = LieGroup1::BoundSize + LieGroup2::BoundSize,
......@@ -33,7 +33,7 @@ namespace hpp {
NT = LieGroup1::NT + LieGroup2::NT
};
typedef se3::CartesianProductOperation<LieGroup1, LieGroup2> Base;
typedef ::pinocchio::CartesianProductOperation<LieGroup1, LieGroup2> Base;
template <class ConfigL_t, class ConfigR_t>