Commit bfbfc03d authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update to API changes in Pinocchio

parent f259c15b
......@@ -19,7 +19,7 @@
# include <list>
# include <pinocchio/multibody/model.hpp> // se3::Data
# include <pinocchio/multibody/data.hpp> // se3::Data
# include <hpp/pinocchio/fwd.hh>
# include <hpp/pinocchio/device.hh>
......
......@@ -31,13 +31,6 @@
# include <pinocchio/multibody/fwd.hpp>
# include <pinocchio/multibody/joint/fwd.hpp>
/*# define HPP_PREDEF_CLASS_AND_POINTERS(NAME) \
HPP_PREDEF_CLASS(NAME); \
typedef boost::shared_ptr<NAME> NAME##Ptr_t; \
typedef boost::shared_ptr<const NAME> NAME##ConstPtr_t; \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
*/
namespace hpp {
namespace pinocchio {
HPP_PREDEF_CLASS (Body);
......
......@@ -32,43 +32,53 @@ namespace hpp {
template<typename JointModel> struct operation {};
};
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<int Axis> struct LieGroupTpl::operation <se3::JointModelRevolute<Axis> > {
template<typename Scalar, int Options, int Axis>
struct LieGroupTpl::operation <se3::JointModelRevoluteTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<int Axis> struct LieGroupTpl::operation <se3::JointModelRevoluteUnbounded<Axis> > {
template<typename Scalar, int Options, int Axis>
struct LieGroupTpl::operation <se3::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelRevoluteUnaligned > {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
template<int Axis> struct LieGroupTpl::operation <se3::JointModelPrismatic<Axis> > {
template<typename Scalar, int Options, int Axis>
struct LieGroupTpl::operation <se3::JointModelPrismatic<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelPrismaticUnaligned > {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelTranslation > {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelTranslationTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<> struct LieGroupTpl::operation <se3::JointModelSpherical> {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelSphericalTpl<Scalar, Options> > {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelSphericalZYX> {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelSphericalZYXTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<> struct LieGroupTpl::operation <se3::JointModelFreeFlyer> {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelFreeFlyerTpl<Scalar, Options> > {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<3, false>,
liegroup::SpecialOrthogonalOperation<3>
> type;
};
template<> struct LieGroupTpl::operation <se3::JointModelPlanar> {
template<typename Scalar, int Options>
struct LieGroupTpl::operation <se3::JointModelPlanarTpl<Scalar, Options> > {
typedef liegroup::CartesianProductOperation<
liegroup::VectorSpaceOperation<2, false>,
liegroup::SpecialOrthogonalOperation<2>
......@@ -80,40 +90,50 @@ namespace hpp {
template<typename JointModel> struct operation {};
};
// JointModelRevolute, JointModelRevoluteUnbounded, JointModelRevoluteUnaligned
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelRevolute<Axis> > {
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteTpl<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnbounded<Axis> > {
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> > {
typedef liegroup::SpecialOrthogonalOperation<2> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnaligned > {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelRevoluteUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, true> type;
};
// JointModelPrismatic, JointModelPrismaticUnaligned, JointModelTranslation
template<int Axis> struct DefaultLieGroupMap::operation <se3::JointModelPrismatic<Axis> > {
template<typename Scalar, int Options, int Axis>
struct DefaultLieGroupMap::operation <se3::JointModelPrismatic<Scalar, Options, Axis> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelPrismaticUnaligned > {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelPrismaticUnalignedTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<1, false> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelTranslation > {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelTranslationTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, false> type;
};
// JointModelSpherical, JointModelSphericalZYX,
template<> struct DefaultLieGroupMap::operation <se3::JointModelSpherical> {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelSphericalTpl<Scalar, Options> > {
typedef liegroup::SpecialOrthogonalOperation<3> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelSphericalZYX> {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelSphericalZYXTpl<Scalar, Options> > {
typedef liegroup::VectorSpaceOperation<3, true> type;
};
// JointModelFreeFlyer, JointModelPlanar
template<> struct DefaultLieGroupMap::operation <se3::JointModelFreeFlyer> {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelFreeFlyerTpl<Scalar, Options> > {
typedef liegroup::SpecialEuclideanOperation<3> type;
};
template<> struct DefaultLieGroupMap::operation <se3::JointModelPlanar> {
template<typename Scalar, int Options>
struct DefaultLieGroupMap::operation <se3::JointModelPlanarTpl<Scalar, Options> > {
typedef liegroup::SpecialEuclideanOperation<2> type;
};
} // namespace pinocchio
......
......@@ -36,7 +36,7 @@ namespace hpp {
typedef se3::CartesianProductOperation<LieGroup1, LieGroup2> Base;
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
......@@ -44,13 +44,13 @@ namespace hpp {
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
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);
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);
}
template <class ConfigIn_t, class ConfigOut_t>
......
......@@ -33,7 +33,7 @@ namespace hpp {
};
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
......
......@@ -33,7 +33,7 @@ namespace hpp {
};
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
......@@ -41,7 +41,7 @@ namespace hpp {
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const typename ConfigL_t::Scalar& w)
......
......@@ -61,7 +61,7 @@ namespace hpp {
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
......@@ -69,7 +69,7 @@ namespace hpp {
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const typename ConfigL_t::Scalar& w)
......
......@@ -240,35 +240,37 @@ namespace hpp {
jointPlacement);
}
template<int AXIS>
template<typename Scalar, int Options, int Axis>
value_type computeMaximalDistanceToParent
( const se3::Model & /*model*/, const se3::JointModelRevolute<AXIS> & , const se3::SE3 & jointPlacement )
( const se3::Model & /*model*/, const se3::JointModelRevoluteTpl<Scalar, Options, Axis> & , const se3::SE3 & jointPlacement )
{ return jointPlacement.translation().norm(); }
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & /*model*/, const se3::JointModelRevoluteUnaligned &, const se3::SE3 & jointPlacement )
( const se3::Model & /*model*/, const se3::JointModelRevoluteUnalignedTpl<Scalar, Options> &, const se3::SE3 & jointPlacement )
{ return jointPlacement.translation().norm(); }
template<int AXIS>
template<typename Scalar, int Options, int Axis>
value_type computeMaximalDistanceToParent
( const se3::Model & /*model*/, const se3::JointModelRevoluteUnbounded<AXIS> & ,
( const se3::Model & /*model*/, const se3::JointModelRevoluteUnboundedTpl<Scalar, Options, Axis> & ,
const se3::SE3 & jointPlacement )
{ return jointPlacement.translation().norm(); }
template<int AXIS>
template<typename Scalar, int Options, int Axis>
value_type computeMaximalDistanceToParent
( const se3::Model & model, const se3::JointModelPrismatic<AXIS> & jmodel,
( const se3::Model & model, const se3::JointModelPrismatic<Scalar, Options, Axis> & jmodel,
const se3::SE3 & jointPlacement )
{
return computeMaximalDistanceToParentForAlignedTranslation
<AXIS == 0, AXIS == 1, AXIS == 2>(
<Axis == 0, Axis == 1, Axis == 2>(
model.lowerPositionLimit.segment<1>(jmodel.idx_q()),
model.upperPositionLimit.segment<1>(jmodel.idx_q()),
jointPlacement);
}
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & model, const se3::JointModelPrismaticUnaligned& jmodel,
( const se3::Model & model, const se3::JointModelPrismaticUnalignedTpl<Scalar, Options>& jmodel,
const se3::SE3 & jointPlacement )
{
if( std::isinf (model.lowerPositionLimit[jmodel.idx_q()])
......@@ -282,16 +284,19 @@ namespace hpp {
jointPlacement.act(pmax).norm() );
}
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & /*model*/, const se3::JointModelSpherical& , const se3::SE3 & jointPlacement )
( const se3::Model & /*model*/, const se3::JointModelSphericalTpl<Scalar, Options>& , const se3::SE3 & jointPlacement )
{ return jointPlacement.translation().norm(); }
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & /*model*/, const se3::JointModelSphericalZYX& , const se3::SE3 & jointPlacement )
( const se3::Model & /*model*/, const se3::JointModelSphericalZYXTpl<Scalar, Options>& , const se3::SE3 & jointPlacement )
{ return jointPlacement.translation().norm(); }
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & model, const se3::JointModelTranslation& jmodel, const se3::SE3 & jointPlacement )
( const se3::Model & model, const se3::JointModelTranslationTpl<Scalar, Options>& jmodel, const se3::SE3 & jointPlacement )
{
const size_type& i = jmodel.idx_q();
return computeMaximalDistanceToParentForAlignedTranslation<true, true, true>(
......@@ -301,8 +306,9 @@ namespace hpp {
}
// TODO (really?): handle the case where the translation is bounded.
template<typename Scalar, int Options>
value_type computeMaximalDistanceToParent
( const se3::Model & model, const se3::JointModelPlanar& jmodel, const se3::SE3 & jointPlacement )
( const se3::Model & model, const se3::JointModelPlanarTpl<Scalar, Options>& jmodel, const se3::SE3 & jointPlacement )
{
const size_type& i = jmodel.idx_q();
return computeMaximalDistanceToParentForAlignedTranslation <true, true, false> (
......@@ -344,19 +350,26 @@ namespace hpp {
&& "The function <upperBoundLinearVel> as not been implemented for this class of joint");
return 0.0;
}
value_type upperBoundLinearVelocity( const se3::JointModelFreeFlyer & ) { return 1.0; }
template<int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelRevolute<AXIS> & ) { return 0.0; }
value_type upperBoundLinearVelocity( const se3::JointModelRevoluteUnaligned & ) { return 0.0; }
template<int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelRevoluteUnbounded<AXIS> & ) { return 0.0; }
template<int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelPrismatic<AXIS> & ) { return 1.0; }
value_type upperBoundLinearVelocity( const se3::JointModelPrismaticUnaligned & ) { return 1.0; }
value_type upperBoundLinearVelocity( const se3::JointModelSpherical & ) { return 0.0; }
value_type upperBoundLinearVelocity( const se3::JointModelSphericalZYX & ) { return 0.0; }
value_type upperBoundLinearVelocity( const se3::JointModelTranslation & ) { return 1.0; }
value_type upperBoundLinearVelocity( const se3::JointModelPlanar & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelFreeFlyerTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelRevoluteTpl<Scalar, Options, AXIS> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelRevoluteUnalignedTpl<Scalar, Options> & ) { return 0.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelRevoluteUnboundedTpl<Scalar, Options, AXIS> & ) { return 0.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundLinearVelocity( const se3::JointModelPrismatic<Scalar, Options, AXIS> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelPrismaticUnalignedTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelSphericalTpl<Scalar, Options> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelSphericalZYXTpl<Scalar, Options> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelTranslationTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundLinearVelocity( const se3::JointModelPlanarTpl<Scalar, Options> & ) { return 1.0; }
struct VisitUpperBoundLinearVelocity : public boost::static_visitor<value_type>
......@@ -383,19 +396,26 @@ namespace hpp {
&& "The function <upperBoundAngularVel> as not been implemented for this class of joint");
return 0.0;
}
value_type upperBoundAngularVelocity( const se3::JointModelFreeFlyer & ) { return 1.0; }
template<int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelRevolute<AXIS> & ) { return 1.0; }
value_type upperBoundAngularVelocity( const se3::JointModelRevoluteUnaligned & ) { return 1.0; }
template<int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelRevoluteUnbounded<AXIS> & ) { return 1.0; }
template<int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelPrismatic<AXIS> & ) { return 0.0; }
value_type upperBoundAngularVelocity( const se3::JointModelPrismaticUnaligned & ) { return 0.0; }
value_type upperBoundAngularVelocity( const se3::JointModelSpherical & ) { return 1.0; }
value_type upperBoundAngularVelocity( const se3::JointModelSphericalZYX & ) { return 1.0; }
value_type upperBoundAngularVelocity( const se3::JointModelTranslation & ) { return 0.0; }
value_type upperBoundAngularVelocity( const se3::JointModelPlanar & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelFreeFlyerTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelRevoluteTpl<Scalar, Options, AXIS> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelRevoluteUnalignedTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelRevoluteUnboundedTpl<Scalar, Options, AXIS> & ) { return 1.0; }
template<typename Scalar, int Options, int AXIS>
value_type upperBoundAngularVelocity( const se3::JointModelPrismatic<Scalar, Options, AXIS> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelPrismaticUnalignedTpl<Scalar, Options> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelSphericalTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelSphericalZYXTpl<Scalar, Options> & ) { return 1.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelTranslationTpl<Scalar, Options> & ) { return 0.0; }
template<typename Scalar, int Options>
value_type upperBoundAngularVelocity( const se3::JointModelPlanarTpl<Scalar, Options> & ) { return 1.0; }
struct VisitUpperBoundAngularVelocity : public boost::static_visitor<value_type>
{
......
......@@ -107,8 +107,8 @@ namespace hpp {
{
if (type == "freeflyer") return se3::JointModelFreeFlyer();
else if (type == "planar") return se3::JointModelPlanar();
else if (type == "prismatic_x") return se3::JointModelPrismatic<0>();
else if (type == "prismatic_y") return se3::JointModelPrismatic<1>();
else if (type == "prismatic_x") return se3::JointModelPrismatic<value_type, 0, 0>();
else if (type == "prismatic_y") return se3::JointModelPrismatic<value_type, 0, 1>();
else throw std::invalid_argument
("Root joint type \"" + type + "\" is currently not available.");
}
......
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