| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2022-2024 INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#ifndef __pinocchio_algorithm_contact_solver_base_hpp__ |
| 6 |
|
|
#define __pinocchio_algorithm_contact_solver_base_hpp__ |
| 7 |
|
|
|
| 8 |
|
|
#include "pinocchio/math/fwd.hpp" |
| 9 |
|
|
#include "pinocchio/math/comparison-operators.hpp" |
| 10 |
|
|
|
| 11 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
| 12 |
|
|
#include <hpp/fcl/timings.h> |
| 13 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
| 14 |
|
|
|
| 15 |
|
|
namespace pinocchio |
| 16 |
|
|
{ |
| 17 |
|
|
|
| 18 |
|
|
template<typename _Scalar> |
| 19 |
|
|
struct ContactSolverBaseTpl |
| 20 |
|
|
{ |
| 21 |
|
|
typedef _Scalar Scalar; |
| 22 |
|
|
|
| 23 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
| 24 |
|
|
typedef hpp::fcl::CPUTimes CPUTimes; |
| 25 |
|
|
typedef hpp::fcl::Timer Timer; |
| 26 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
| 27 |
|
|
|
| 28 |
|
✗ |
explicit ContactSolverBaseTpl(const int problem_size) |
| 29 |
|
✗ |
: problem_size(problem_size) |
| 30 |
|
✗ |
, max_it(1000) |
| 31 |
|
✗ |
, it(0) |
| 32 |
|
✗ |
, absolute_precision(Scalar(1e-6)) |
| 33 |
|
✗ |
, relative_precision(Scalar(1e-6)) |
| 34 |
|
✗ |
, absolute_residual(Scalar(-1)) |
| 35 |
|
✗ |
, relative_residual(Scalar(-1)) |
| 36 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
| 37 |
|
✗ |
, timer(false) |
| 38 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
| 39 |
|
|
{ |
| 40 |
|
|
} |
| 41 |
|
|
|
| 42 |
|
|
/// \brief Returns the size of the problem |
| 43 |
|
✗ |
int getProblemSize() const |
| 44 |
|
|
{ |
| 45 |
|
✗ |
return problem_size; |
| 46 |
|
|
} |
| 47 |
|
|
|
| 48 |
|
|
/// \brief Get the number of iterations achieved by the solver. |
| 49 |
|
✗ |
int getIterationCount() const |
| 50 |
|
|
{ |
| 51 |
|
✗ |
return it; |
| 52 |
|
|
} |
| 53 |
|
|
|
| 54 |
|
|
/// \brief Set the maximum number of iterations. |
| 55 |
|
✗ |
void setMaxIterations(const int max_it) |
| 56 |
|
|
{ |
| 57 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it > 0, "max_it should be greater than 0."); |
| 58 |
|
✗ |
this->max_it = max_it; |
| 59 |
|
|
} |
| 60 |
|
|
/// \brief Get the maximum number of iterations allowed. |
| 61 |
|
✗ |
int getMaxIterations() const |
| 62 |
|
|
{ |
| 63 |
|
✗ |
return max_it; |
| 64 |
|
|
} |
| 65 |
|
|
|
| 66 |
|
|
/// \brief Set the absolute precision for the problem. |
| 67 |
|
✗ |
void setAbsolutePrecision(const Scalar absolute_precision) |
| 68 |
|
|
{ |
| 69 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
| 70 |
|
|
absolute_precision >= Scalar(0), "absolute_precision should be positive."); |
| 71 |
|
✗ |
this->absolute_precision = absolute_precision; |
| 72 |
|
|
} |
| 73 |
|
|
/// \brief Get the absolute precision requested. |
| 74 |
|
✗ |
Scalar getAbsolutePrecision() const |
| 75 |
|
|
{ |
| 76 |
|
✗ |
return absolute_precision; |
| 77 |
|
|
} |
| 78 |
|
|
|
| 79 |
|
|
/// \brief Set the relative precision for the problem. |
| 80 |
|
✗ |
void setRelativePrecision(const Scalar relative_precision) |
| 81 |
|
|
{ |
| 82 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
| 83 |
|
|
relative_precision >= Scalar(0), "relative_precision should be positive."); |
| 84 |
|
✗ |
this->relative_precision = relative_precision; |
| 85 |
|
|
} |
| 86 |
|
|
/// \brief Get the relative precision requested. |
| 87 |
|
✗ |
Scalar getRelativePrecision() const |
| 88 |
|
|
{ |
| 89 |
|
✗ |
return relative_precision; |
| 90 |
|
|
} |
| 91 |
|
|
|
| 92 |
|
|
/// \brief Returns the value of the absolute residual value corresponding to the contact |
| 93 |
|
|
/// complementary conditions. |
| 94 |
|
✗ |
Scalar getAbsoluteConvergenceResidual() const |
| 95 |
|
|
{ |
| 96 |
|
✗ |
return absolute_residual; |
| 97 |
|
|
} |
| 98 |
|
|
/// \brief Returns the value of the relative residual value corresponding to the difference |
| 99 |
|
|
/// between two successive iterates (infinity norms). |
| 100 |
|
✗ |
Scalar getRelativeConvergenceResidual() const |
| 101 |
|
|
{ |
| 102 |
|
✗ |
return relative_residual; |
| 103 |
|
|
} |
| 104 |
|
|
|
| 105 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
| 106 |
|
✗ |
CPUTimes getCPUTimes() const |
| 107 |
|
|
{ |
| 108 |
|
✗ |
return timer.elapsed(); |
| 109 |
|
|
} |
| 110 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
| 111 |
|
|
|
| 112 |
|
|
protected: |
| 113 |
|
|
/// \brief Size of the problem |
| 114 |
|
|
int problem_size; |
| 115 |
|
|
/// \brief Maximum number of iterations. |
| 116 |
|
|
int max_it; |
| 117 |
|
|
/// \brief Number of iterations needed to achieve convergence. |
| 118 |
|
|
int it; |
| 119 |
|
|
/// \brief Desired absolute precision. |
| 120 |
|
|
Scalar absolute_precision; |
| 121 |
|
|
/// \brief Desired relative precision. |
| 122 |
|
|
Scalar relative_precision; |
| 123 |
|
|
/// \brief Absolule convergence residual value. |
| 124 |
|
|
Scalar absolute_residual; |
| 125 |
|
|
/// \brief Relative convergence residual value |
| 126 |
|
|
Scalar relative_residual; |
| 127 |
|
|
|
| 128 |
|
|
#ifdef PINOCCHIO_WITH_HPP_FCL |
| 129 |
|
|
Timer timer; |
| 130 |
|
|
#endif // PINOCCHIO_WITH_HPP_FCL |
| 131 |
|
|
|
| 132 |
|
|
}; // struct ContactSolverBaseTpl |
| 133 |
|
|
|
| 134 |
|
|
} // namespace pinocchio |
| 135 |
|
|
|
| 136 |
|
|
#endif // ifndef __pinocchio_algorithm_contact_solver_base_hpp__ |
| 137 |
|
|
|