constrained-dynamics.hpp

Go to the documentation of this file.

//
// Copyright (c) 2019-2021 INRIA
//
 
#ifndef __pinocchio_algorithm_constrained_dynamics_hpp__
#define __pinocchio_algorithm_constrained_dynamics_hpp__
 
#include "pinocchio/algorithm/contact-info.hpp"
 
#include "pinocchio/algorithm/proximal.hpp"
 
namespace pinocchio
{
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    class Allocator>
  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
  inline void initConstraintDynamics(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models);
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType1,
    typename TangentVectorType2,
    class ConstraintModelAllocator,
    class ConstraintDataAllocator>
  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
  inline const
    typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<ConfigVectorType> & q,
      const Eigen::MatrixBase<TangentVectorType1> & v,
      const Eigen::MatrixBase<TangentVectorType2> & tau,
      const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
        contact_models,
      std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
      ProximalSettingsTpl<Scalar> & settings);
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType1,
    typename TangentVectorType2,
    class ConstraintModelAllocator,
    class ConstraintDataAllocator>
  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
  inline const
    typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<ConfigVectorType> & q,
      const Eigen::MatrixBase<TangentVectorType1> & v,
      const Eigen::MatrixBase<TangentVectorType2> & tau,
      const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
        contact_models,
      std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas)
  {
    ProximalSettingsTpl<Scalar> settings;
    return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings);
  }
 
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType1,
    typename TangentVectorType2,
    class ModelAllocator,
    class DataAllocator>
  PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
  inline const
    typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      DataTpl<Scalar, Options, JointCollectionTpl> & data,
      const Eigen::MatrixBase<ConfigVectorType> & q,
      const Eigen::MatrixBase<TangentVectorType1> & v,
      const Eigen::MatrixBase<TangentVectorType2> & tau,
      const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
      std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data)
  {
    ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>();
    return contactABA(
      model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings);
  }
 
} // namespace pinocchio
 
#include "pinocchio/algorithm/constrained-dynamics.hxx"
 
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
  #include "pinocchio/algorithm/constrained-dynamics.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
 
#endif // ifndef __pinocchio_algorithm_constrained_dynamics_hpp__