Skip to content

Commit

Permalink
[core] Fix error estimation too optimistic for adaptive steppers. (#716)
Browse files Browse the repository at this point in the history 
* [core] Error and warning reporting more consistent with Python.
* [core] Fix exception handling for visco-elastic coupling and internal flex forces.
* [core] Fix error estimation too optimistic for adaptive steppers.
  • Loading branch information
@duburcqa
duburcqa committed Feb 13, 2024
1 parent a9ed750 commit 4fadce5
Show file tree
Hide file tree
Showing 5 changed files with 60 additions and 33 deletions.
9 changes: 5 additions & 4 deletions core/include/jiminy/core/macros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,18 +65,19 @@ namespace jiminy::internal
*/
#define THROW_ERROR(exception, ...) \
throw exception( \
toString("In " FILE_LINE ": In ", \
toString("In ", \
jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION), \
":\n\x1b[1;31merror:\x1b[0m ", \
" (" FILE_LINE "):\n", \
__VA_ARGS__))

#ifdef NDEBUG
# define PRINT_WARNING(...)
#else
# define PRINT_WARNING(...) \
std::cout << "In " FILE_LINE ": In " \
std::cout << "\x1b[1;93mWARNING\x1b[0m: In " \
<< jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
<< ":\n\x1b[1;93mwarning:\x1b[0m " << toString(__VA_ARGS__) << std::endl
<< " (" FILE_LINE "):\n" \
<< toString(__VA_ARGS__) << std::endl
#endif

#endif // JIMINY_MACRO_H
16 changes: 16 additions & 0 deletions core/include/jiminy/core/stepper/lie_group.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,13 @@ namespace Eigen
const DataType & a() const { return derived().a(); }
DataType & a() { return derived().a(); }

StateDerivativeBase & absInPlace()
{
v().array() = v().array().abs();
a().array() = a().array().abs();
return *this;
}

template<int p>
RealScalar lpNorm() const;
RealScalar norm() const { return lpNorm<2>(); };
Expand Down Expand Up @@ -1246,6 +1253,15 @@ namespace Eigen
vector_[i] += scale * vectorIn[i]; \
} \
return *this; \
} \
\
StateDerivativeVector & absInPlace() \
{ \
for (std::size_t i = 0; i < vector_.size(); ++i) \
{ \
vector_[i].absInPlace(); \
} \
return *this; \
}

#define State_SHARED_ADDON \
Expand Down
10 changes: 8 additions & 2 deletions core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,14 @@ namespace jiminy
/// \brief Stepper order, used to scale the error.
inline constexpr double STEPPER_ORDER = 5.0;
/// \brief Safety factor when updating the error, should be less than 1.
inline constexpr double SAFETY = 0.8;
/// \brief Miminum allowed relative step decrease.
///
/// \details The larger the more conservative. More precisely, a small safety factor means
/// that the step size will be increased less aggressively when the error is small
/// and decreased more aggressively when the error is large.
inline constexpr double SAFETY = 0.9;
/// \brief Maximum acceptable error threshold below which step size is increased.
inline constexpr double ERROR_THRESHOLD = 0.5;
/// \brief Mininum allowed relative step decrease.
inline constexpr double MIN_FACTOR = 0.2;
/// \brief Maximum allowed relative step increase.
inline constexpr double MAX_FACTOR = 5.0;
Expand Down
9 changes: 6 additions & 3 deletions core/src/engine/engine_multi_robot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -352,7 +352,7 @@ namespace jiminy
// Compute intermediary quantities
rot12.noalias() = oMf1.rotation().transpose() * oMf2.rotation();
rotLog12 = pinocchio::log3(rot12, angle);
if (angle < 0.95 * M_PI)
if (angle > 0.95 * M_PI)
{
THROW_ERROR(std::runtime_error,
"Relative angle between reference frames of viscoelastic "
Expand Down Expand Up @@ -3409,8 +3409,11 @@ namespace jiminy

const Eigen::Map<const Eigen::Quaterniond> quat(q.segment<4>(positionIndex).data());
const Eigen::Vector3d angleAxis = pinocchio::quaternion::log3(quat, angle);
assert((angle < 0.95 * M_PI) &&
"Flexible joint angle must be smaller than 0.95 * pi.");
if (angle > 0.95 * M_PI) // Angle is always positive
{
THROW_ERROR(std::runtime_error,
"Flexible joint angle must be smaller than 0.95 * pi.");
}
pinocchio::Jlog3(angle, angleAxis, rotJlog3);
uInternal.segment<3>(velocityIndex) -=
rotJlog3 * (stiffness.array() * angleAxis.array()).matrix();
Expand Down
49 changes: 25 additions & 24 deletions core/src/stepper/runge_kutta_dopri_stepper.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,13 @@ namespace jiminy
double RungeKuttaDOPRIStepper::computeError(
const State & initialState, const State & solution, double dt)
{
// Compute error scale given absolute and relative tolerance
otherSolution_.setZero();
initialState.difference(otherSolution_, scale_);
scale_.absInPlace();
scale_ *= tolRel_;
scale_ += tolAbs_;

// Compute alternative solution
stateIncrement_.setZero();
for (std::size_t i = 0; i < ki_.size(); ++i)
Expand All @@ -36,44 +43,38 @@ namespace jiminy
// Evaluate error between both states to adjust step
solution.difference(otherSolution_, error_);

// Compute absolute and relative element-wise maximum error
double errorAbsNorm = INF;
double errorRelNorm = INF;
if (tolAbs_ > EPS)
{
errorAbsNorm = error_.normInf() / tolAbs_;
}
if (tolRel_ > EPS)
{
otherSolution_.setZero();
solution.difference(otherSolution_, scale_);
error_ /= scale_;
errorRelNorm = error_.normInf() / tolRel_;
}

// Return the smallest error between absolute and relative
return std::min(errorAbsNorm, errorRelNorm);
// Return element-wise maximum rescaled error
error_ /= scale_;
return error_.normInf();
}

bool RungeKuttaDOPRIStepper::adjustStepImpl(double error, double & dt)
{
// Make sure the error is defined, otherwise rely on a simple heuristic
// Make sure the error is defined, otherwise rely on simple heuristic
if (std::isnan(error))
{
dt *= 0.1;
return false;
}

// Adjustment algorithm from boost implementation
/* Adjustment algorithm from boost implementation.
For technical reference, see original boost::odeint implementation:
https://beta.boost.org/doc/libs/1_82_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.controlled_steppers
*/
if (error < 1.0)
{
// Only increase if error is sufficiently small
if (error < std::pow(DOPRI::SAFETY, DOPRI::STEPPER_ORDER))
/* Increase step size only if the error is sufficiently small.
The threshold must be chosen in a way to guarantee that it actually decreases. */
if (error <
std::min(DOPRI::ERROR_THRESHOLD, std::pow(DOPRI::SAFETY, DOPRI::STEPPER_ORDER)))
{
// Prevent numeric rounding error when close to zero
const double newError = std::max(
/* Prevent numeric rounding error when close to zero.
Multiply step size by 'DOPRI::SAFETY / (error ** (1 / DOPRI::STEPPER_ORDER))',
up to 'DOPRI::MAX_FACTOR'.
*/
const double clippedError = std::max(
error, std::pow(DOPRI::MAX_FACTOR / DOPRI::SAFETY, -DOPRI::STEPPER_ORDER));
dt *= DOPRI::SAFETY * std::pow(newError, -1.0 / DOPRI::STEPPER_ORDER);
dt *= DOPRI::SAFETY * std::pow(clippedError, -1.0 / DOPRI::STEPPER_ORDER);
}
return true;
}
Expand Down

0 comments on commit 4fadce5

Please sign in to comment.