constraint-model-base.hpp

//
// Copyright (c) 2023 INRIA
//

#ifndef __pinocchio_algorithm_constraint_model_base_hpp__
#define __pinocchio_algorithm_constraint_model_base_hpp__

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/fwd.hpp"

namespace pinocchio
{

 template<class Derived>
 struct ConstraintModelBase : NumericalBase<Derived>
 {
 typedef typename traits<Derived>::Scalar Scalar;
 enum
 {
 Options = traits<Derived>::Options
 };
 typedef typename traits<Derived>::ConstraintData ConstraintData;

 typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector;
 // typedef Eigen::Matrix<Eigen::DenseIndex,Eigen::Dynamic,1,Options> IndexVector;
 typedef std::vector<Eigen::DenseIndex> IndexVector;

 Derived & derived()
 {
 return static_cast<Derived &>(*this);
 }
 const Derived & derived() const
 {
 return static_cast<const Derived &>(*this);
 }

 template<typename NewScalar>
 typename CastType<NewScalar, Derived>::type cast() const
 {
 return derived().template cast<NewScalar>();
 }

 template<int Options, template<typename, int> class JointCollectionTpl>
 void calc(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
 ConstraintData & cdata) const
 {
 derived().calc(model, data, cdata);
 }

 template<typename JacobianMatrix, int Options, template<typename, int> class JointCollectionTpl>
 void jacobian(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
 ConstraintData & cdata,
 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix) const
 {
 derived().jacobian(model, data, cdata, jacobian_matrix.const_cast_derived());
 }

 // Attributes common to all constraints

 std::string name;

 BooleanVector colwise_sparsity;

 IndexVector colwise_span_indexes;

 template<typename OtherDerived>
 bool operator==(const ConstraintModelBase<OtherDerived> & other) const
 {
 return name == other.name && colwise_sparsity == other.colwise_sparsity
 && colwise_span_indexes == other.colwise_span_indexes;
 }

 template<typename OtherDerived>
 ConstraintModelBase & operator=(const ConstraintModelBase<OtherDerived> & other)
 {
 name = other.name;
 colwise_sparsity = other.colwise_sparsity;
 colwise_span_indexes = other.colwise_span_indexes;

 return *this;
 }

 ConstraintData createData() const
 {
 return derived().createData();
 }

 protected:
 template<int Options, template<typename, int> class JointCollectionTpl>
 ConstraintModelBase(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
 : colwise_sparsity(model.nv)
 {
 static const bool default_sparsity_value = false;
 colwise_sparsity.fill(default_sparsity_value);
 }

 ConstraintModelBase()
 {
 }

 ConstraintModelBase & base()
 {
 return *this;
 }
 const ConstraintModelBase & base() const
 {
 return *this;
 }
 };

} // namespace pinocchio

#endif // ifndef __pinocchio_algorithm_constraint_model_base_hpp__