Program Listing for File cartesian-product-variant.hpp
↰ Return to documentation for file (include/pinocchio/multibody/liegroup/cartesian-product-variant.hpp
)
//
// Copyright (c) 2018 CNRS
//
#ifndef __pinocchio_cartesian_product_variant_hpp__
#define __pinocchio_cartesian_product_variant_hpp__
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
#include "pinocchio/container/aligned-vector.hpp"
namespace pinocchio
{
template<typename Scalar, int Options = 0,
template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
struct CartesianProductOperationVariantTpl;
typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl> CartesianProductOperationVariant;
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
{
typedef _Scalar Scalar;
enum {
Options = _Options,
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
};
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
struct CartesianProductOperationVariantTpl
: public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
CartesianProductOperationVariantTpl()
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
{};
explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
{
append(lg);
};
CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1,
const LieGroupGeneric & lg2)
: m_nq(0), m_nv(0)
, lg_nqs(0), lg_nvs(0)
, m_neutral(0)
{
append(lg1); append(lg2);
};
void append(const LieGroupGeneric & lg);
template<typename LieGroupDerived>
void append (const LieGroupBase<LieGroupDerived>& lg)
{
LieGroupGeneric lgGeneric (lg);
append(lgGeneric);
}
CartesianProductOperationVariantTpl operator* (const CartesianProductOperationVariantTpl& other) const;
CartesianProductOperationVariantTpl& operator*= (const CartesianProductOperationVariantTpl& other);
inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
{
append(lg);
return *this;
}
template<typename LieGroupDerived>
inline CartesianProductOperationVariantTpl& operator*= (const LieGroupBase<LieGroupDerived>& lg)
{
append<LieGroupDerived>(lg);
return *this;
}
int nq() const { return m_nq; }
int nv() const { return m_nv; }
std::string name() const { return m_name; }
ConfigVector_t neutral() const { return m_neutral; }
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const;
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
template <class Config_t, class Jacobian_t>
void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Jacobian_t> & J) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op=SETTO) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op=SETTO) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrate_product_impl(const Config_t & q,
const Tangent_t & v,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dIntegrateOnTheLeft,
const ArgumentPosition arg,
const AssignmentOperatorType op) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <class ConfigL_t, class ConfigR_t>
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
template <class Config_t>
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
template <class Config_t>
bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qout,
const Scalar& prec) const;
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout) const;
template <class ConfigL_t, class ConfigR_t>
bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec) const;
bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
template <typename LieGroup1, typename LieGroup2>
bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
protected:
PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
Index m_nq, m_nv;
std::vector<Index> lg_nqs, lg_nvs;
std::string m_name;
ConfigVector_t m_neutral;
};
} // namespace pinocchio
#include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
#endif // ifndef __pinocchio_cartesian_product_variant_hpp__