proximal.hpp

//
// Copyright (c) 2019-2022 INRIA
//
 
#ifndef __pinocchio_algorithm_proximal_hpp__
#define __pinocchio_algorithm_proximal_hpp__
 
#include <Eigen/Core>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
 
namespace pinocchio
{
 
  template<typename _Scalar>
  struct ProximalSettingsTpl
  {
    typedef _Scalar Scalar;
 
    ProximalSettingsTpl()
    : absolute_accuracy(Eigen::NumTraits<Scalar>::dummy_precision())
    , relative_accuracy(Eigen::NumTraits<Scalar>::dummy_precision())
    , mu(0)
    , max_iter(1)
    , absolute_residual(-1.)
    , relative_residual(-1.)
    , iter(0)
    {
    }
 
    ProximalSettingsTpl(const Scalar accuracy, const Scalar mu, const int max_iter)
    : absolute_accuracy(accuracy)
    , relative_accuracy(accuracy)
    , mu(mu)
    , max_iter(max_iter)
    , absolute_residual(-1.)
    , relative_residual(-1.)
    , iter(0)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        check_expression_if_real<Scalar>(accuracy >= 0.) && "Accuracy must be positive.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        check_expression_if_real<Scalar>(mu >= 0.) && "mu must be positive");
      assert(max_iter >= 1 && "max_iter must be greater or equal to 1");
    }
 
    ProximalSettingsTpl(
      const Scalar absolute_accuracy,
      const Scalar relative_accuracy,
      const Scalar mu,
      const int max_iter)
    : absolute_accuracy(absolute_accuracy)
    , relative_accuracy(relative_accuracy)
    , mu(mu)
    , max_iter(max_iter)
    , absolute_residual(-1.)
    , relative_residual(-1.)
    , iter(0)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        check_expression_if_real<Scalar>(absolute_accuracy >= 0.)
        && "Absolute accuracy must be positive.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        check_expression_if_real<Scalar>(relative_accuracy >= 0.)
        && "Relative accuracy must be positive.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        check_expression_if_real<Scalar>(mu >= 0.) && "mu must be positive");
      assert(max_iter >= 1 && "max_iter must be greater or equal to 1");
    }
 
    // data
 
    Scalar absolute_accuracy;
 
    Scalar relative_accuracy;
 
    Scalar mu;
 
    int max_iter;
 
    // data that can be modified by the algorithm
 
    Scalar absolute_residual;
 
    Scalar relative_residual;
 
    int iter;
  };
 
} // namespace pinocchio
 
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
  #include "pinocchio/algorithm/proximal.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
 
#endif // ifndef __pinocchio_algorithm_proximal_hpp__