constraint_violation.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <Eigen/Core>
 
namespace slp {
 
template <typename Scalar>
Scalar constraint_violation(const Eigen::VectorX<Scalar>& c_e) {
  return c_e.template lpNorm<1>();
}
 
template <typename Scalar>
Scalar constraint_violation(const Eigen::VectorX<Scalar>& c_e,
                            const Eigen::VectorX<Scalar>& c_i,
                            const Eigen::VectorX<Scalar>& s) {
  return c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>();
}
 
}  // namespace slp