delassus.hpp

//
// Copyright (c) 2020-2024 INRIA
// Copyright (c) 2023 KU Leuven
//

#pragma once

// IWYU pragma: begin_keep
#include <cassert>
#include <algorithm>
#include <vector>
#include <cstddef>
#include <limits>

#include <Eigen/Core>
#include <boost/fusion/container/vector.hpp>

#include "pinocchio/macros.hpp"
#include "pinocchio/unsupported.hpp"

#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"

#include "pinocchio/math.hpp"

#include "pinocchio/utils/check.hpp"

#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/constraints.hpp"
#include "pinocchio/algorithm/model.hpp"
// IWYU pragma: end_keep

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 initPvDelassus(
 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,
 class ModelAllocator,
 class DataAllocator,
 typename MatrixType>
 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
 void computeDelassusMatrix(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const Eigen::MatrixBase<ConfigVectorType> & q,
 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
 std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data,
 const Eigen::MatrixBase<MatrixType> & delassus,
 const Scalar mu = 0);

 template<
 typename Scalar,
 int Options,
 template<typename, int> class JointCollectionTpl,
 typename ConfigVectorType,
 class ModelAllocator,
 class DataAllocator,
 typename MatrixType>
 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
 void computeDampedDelassusMatrixInverse(
 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
 DataTpl<Scalar, Options, JointCollectionTpl> & data,
 const Eigen::MatrixBase<ConfigVectorType> & q,
 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
 std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data,
 const Eigen::MatrixBase<MatrixType> & damped_delassus_inverse,
 const Scalar mu,
 const bool scaled = false,
 const bool Pv = true);

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/delassus.hxx"
// IWYU pragma: end_exports