Unverified Commit 5f14887a authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #29 from wxmerkt/wxm-replace-assert-issue-28

Replace assert with exceptions - addresses #28
parents e360fb3a ce872cf6
...@@ -41,7 +41,9 @@ struct Bern { ...@@ -41,7 +41,9 @@ struct Bern {
~Bern() {} ~Bern() {}
Numeric operator()(const Numeric u) const { Numeric operator()(const Numeric u) const {
assert(u >= 0. && u <= 1.); if (!(u >= 0. && u <= 1.)) {
throw std::invalid_argument("u needs to be betwen 0 and 1.");
}
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i); return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
} }
......
...@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
size_(std::distance(PointsBegin, PointsEnd)), size_(std::distance(PointsBegin, PointsEnd)),
degree_(size_ - 1), degree_(size_ - 1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) { bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_)) {
assert(bernstein_.size() == size_); if (bernstein_.size() != size_) {
throw std::invalid_argument("Invalid size of polynomial");
}
In it(PointsBegin); In it(PointsBegin);
if (Safe && (size_ < 1 || T_max_ <= T_min_)) { if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO throw std::invalid_argument("can't create bezier min bound is higher than max bound");
} }
for (; it != PointsEnd; ++it) { for (; it != PointsEnd; ++it) {
control_points_.push_back(*it); control_points_.push_back(*it);
......
...@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
// //
const Time dt = (t1 - t0); const Time dt = (t1 - t0);
const Time alpha = (t - t0) / dt; const Time alpha = (t - t0) / dt;
assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]"); if (!(0. <= alpha && alpha <= 1.)) {
throw std::runtime_error("alpha must be in [0,1]");
}
Numeric h00, h10, h01, h11; Numeric h00, h10, h01, h11;
evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative); evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative);
// std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" // std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"
......
...@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t; ...@@ -37,7 +37,9 @@ typedef exact_cubic_t::spline_t spline_t;
/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line. /// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) { Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
Numeric norm = normal.norm(); Numeric norm = normal.norm();
assert(norm > 0.); if (norm < 0.) {
throw std::runtime_error("Norm of normal is less than 0!");
}
return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset)); return std::make_pair(source.first + time_offset, (source.second + normal / norm * offset));
} }
...@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer ...@@ -46,7 +48,9 @@ Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numer
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time, spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset, const Time init_time,
const Time time_offset) { const Time time_offset) {
Numeric norm = normal.norm(); Numeric norm = normal.norm();
assert(norm > 0.); if (norm < 0.) {
throw std::runtime_error("Norm of normal is less than 0!");
}
Point n = normal / norm; Point n = normal / norm;
Point d = offset / (time_offset * time_offset * time_offset) * -n; Point d = offset / (time_offset * time_offset * time_offset) * -n;
Point c = -3 * d * time_offset; Point c = -3 * d * time_offset;
......
...@@ -38,7 +38,9 @@ struct SplineOptimizer { ...@@ -38,7 +38,9 @@ struct SplineOptimizer {
///\brief Initializes optimizer environment. ///\brief Initializes optimizer environment.
SplineOptimizer() { SplineOptimizer() {
MSKrescodee r_ = MSK_makeenv(&env_, NULL); MSKrescodee r_ = MSK_makeenv(&env_, NULL);
assert(r_ == MSK_RES_OK); if (r_ != MSK_RES_OK) {
throw std::runtime_error("Issue initializing MSK_makeenv");
}
} }
///\brief Destructor. ///\brief Destructor.
......
...@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition ...@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
// add remaining variables (only if no end_pos constraints) // add remaining variables (only if no end_pos constraints)
for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_)); for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
assert(numControlPoints > numConstants); if (numControlPoints <= numConstants) {
assert(numControlPoints == variables_.size()); throw std::runtime_error("numControlPoints < numConstants");
}
if (numControlPoints != variables_.size()) {
throw std::runtime_error("numControlPoints != variables_.size()");
}
problemData.numControlPoints = numControlPoints; problemData.numControlPoints = numControlPoints;
problemData.numVariables = numControlPoints - numConstants; problemData.numVariables = numControlPoints - numConstants;
...@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble ...@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
// compute sub-bezier curves // compute sub-bezier curves
T_bezier_t beziers = split<Point, Numeric>(pDef, pData); T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size()); if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.size()) {
assert(pDef.inequalityMatrices_.size() == beziers.size()); throw std::invalid_argument("The sizes of the inequality matrices and vectors do not match.");
}
if (pDef.inequalityMatrices_.size() != beziers.size()) {
throw std::invalid_argument("The sizes of the inequality matrices and the bezier degree do not match.");
}
long currentRowIdx = 0; long currentRowIdx = 0;
typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin(); typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin();
...@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble ...@@ -264,7 +272,7 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
currentRowIdx += cmit->rows(); currentRowIdx += cmit->rows();
} }
} }
assert(rows == currentRowIdx); // we filled all the constraints assert(rows == currentRowIdx); // we filled all the constraints - NB: leave assert for Debug tests
} }
template <typename Point, typename Numeric, typename In> template <typename Point, typename Numeric, typename In>
...@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po ...@@ -273,8 +281,9 @@ quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In Po
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t; typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)), unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2)); nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
assert(nPoints1 > 0); if (nPoints1 <= 0 || nPoints2 <= 0) {
assert(nPoints2 > 0); throw std::runtime_error("This should never happen because an unsigned int cannot go negative without underflowing.");
}
unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1; unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
unsigned int newDeg = (deg1 + deg2); unsigned int newDeg = (deg1 + deg2);
// the integral of the primitive will simply be the last control points of the primitive, // the integral of the primitive will simply be the last control points of the primitive,
......
...@@ -35,7 +35,9 @@ struct quadratic_variable { ...@@ -35,7 +35,9 @@ struct quadratic_variable {
} }
quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) { quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) {
assert(A.cols() == b.rows() && A.cols() == A.rows()); if (A.cols() != b.rows() || A.cols() != A.rows()) {
throw std::invalid_argument("The dimensions of A and b are incorrect.");
}
} }
quadratic_variable(const point_t& b, const Numeric c = 0) quadratic_variable(const point_t& b, const Numeric c = 0)
...@@ -45,7 +47,9 @@ struct quadratic_variable { ...@@ -45,7 +47,9 @@ struct quadratic_variable {
// linear evaluation // linear evaluation
Numeric operator()(const Eigen::Ref<const point_t>& val) const { Numeric operator()(const Eigen::Ref<const point_t>& val) const {
assert(!isZero()); if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return val.transpose() * A() * val + b().transpose() * val + c(); return val.transpose() * A() * val + b().transpose() * val + c();
} }
...@@ -98,15 +102,21 @@ struct quadratic_variable { ...@@ -98,15 +102,21 @@ struct quadratic_variable {
} }
const matrix_x_t& A() const { const matrix_x_t& A() const {
assert(!isZero()); if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return A_; return A_;
} }
const point_t& b() const { const point_t& b() const {
assert(!isZero()); if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return b_; return b_;
} }
const Numeric c() const { const Numeric c() const {
assert(!isZero()); if (isZero()) {
throw std::runtime_error("Not initialized! (isZero)");
}
return c_; return c_;
} }
bool isZero() const { return zero; } bool isZero() const { return zero; }
......
...@@ -59,7 +59,9 @@ struct Serializable { ...@@ -59,7 +59,9 @@ struct Serializable {
/// \brief Loads a Derived object from an XML file. /// \brief Loads a Derived object from an XML file.
template <class Derived> template <class Derived>
void loadFromXML(const std::string& filename, const std::string& tag_name) { void loadFromXML(const std::string& filename, const std::string& tag_name) {
assert(!tag_name.empty()); if (tag_name.empty()) {
throw std::invalid_argument("tag_name cannot be empty.");
}
std::ifstream ifs(filename.c_str()); std::ifstream ifs(filename.c_str());
if (ifs) { if (ifs) {
boost::archive::xml_iarchive ia(ifs); boost::archive::xml_iarchive ia(ifs);
...@@ -74,7 +76,9 @@ struct Serializable { ...@@ -74,7 +76,9 @@ struct Serializable {
/// \brief Saved a Derived object as an XML file. /// \brief Saved a Derived object as an XML file.
template <class Derived> template <class Derived>
void saveAsXML(const std::string& filename, const std::string& tag_name) const { void saveAsXML(const std::string& filename, const std::string& tag_name) const {
assert(!tag_name.empty()); if (tag_name.empty()) {
throw std::invalid_argument("tag_name cannot be empty.");
}
std::ofstream ofs(filename.c_str()); std::ofstream ofs(filename.c_str());
if (ofs) { if (ofs) {
boost::archive::xml_oarchive oa(ofs); boost::archive::xml_oarchive oa(ofs);
......
...@@ -256,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > { ...@@ -256,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
theta = PI_value; // acos((-1-1)/2) theta = PI_value; // acos((-1-1)/2)
else else
theta = acos((tr - Scalar(1)) / Scalar(2)); theta = acos((tr - Scalar(1)) / Scalar(2));
assert(theta == theta && "theta contains some NaN"); // theta != NaN if (!std::isfinite(theta)) {
throw std::runtime_error("theta contains some NaN");
}
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small. // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if (theta < PI_value - 1e-2) { if (theta < PI_value - 1e-2) {
......
...@@ -5,7 +5,9 @@ ...@@ -5,7 +5,9 @@
namespace curves { namespace curves {
std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) { std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) {
assert(vectors.cols() * 3 == matrices.cols()); if (vectors.cols() * 3 != matrices.cols()) {
throw std::invalid_argument("vectors.cols() * 3 != matrices.cols()");
}
std::vector<linear_variable_t> res; std::vector<linear_variable_t> res;
for (int i = 0; i < vectors.cols(); ++i) { for (int i = 0; i < vectors.cols(); ++i) {
res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i))); res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i)));
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment