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

Replace assert with exceptions - addresses #28

include/curves/bernstein.h

@@ -41,7 +41,9 @@ struct Bern {
   ~Bern() {}
 
   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);
   }
 

include/curves/bezier_curve.h

@@ -66,10 +66,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
         size_(std::distance(PointsBegin, PointsEnd)),
         degree_(size_ - 1),
         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);
     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) {
       control_points_.push_back(*it);

include/curves/cubic_hermite_spline.h

@@ -233,7 +233,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
     //
     const Time dt = (t1 - t0);
     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;
     evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative);
     // std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<"

include/curves/helpers/effector_spline.h

@@ -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.
 Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset) {
   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));
 }
 
@@ -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,
                          const Time time_offset) {
   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 d = offset / (time_offset * time_offset * time_offset) * -n;
   Point c = -3 * d * time_offset;

include/curves/optimization/OptimizeSpline.h

@@ -38,7 +38,9 @@ struct SplineOptimizer {
   ///\brief Initializes optimizer environment.
   SplineOptimizer() {
     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.

include/curves/optimization/details.h

@@ -174,8 +174,12 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
   // add remaining variables (only if no end_pos constraints)
   for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
 
-  assert(numControlPoints > numConstants);
-  assert(numControlPoints == variables_.size());
+  if (numControlPoints <= numConstants) {
+    throw std::runtime_error("numControlPoints < numConstants");
+  }
+  if (numControlPoints != variables_.size()) {
+    throw std::runtime_error("numControlPoints != variables_.size()");
+  }
 
   problemData.numControlPoints = numControlPoints;
   problemData.numVariables = numControlPoints - numConstants;
@@ -246,8 +250,12 @@ void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, proble
   // compute sub-bezier curves
   T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
 
-  assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size());
-  assert(pDef.inequalityMatrices_.size() == beziers.size());
+  if (pDef.inequalityMatrices_.size() != pDef.inequalityVectors_.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;
   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
       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>
@@ -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;
   unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
                nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
-  assert(nPoints1 > 0);
-  assert(nPoints2 > 0);
+  if (nPoints1 <= 0 || 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 newDeg = (deg1 + deg2);
   // the integral of the primitive will simply be the last control points of the primitive,

include/curves/quadratic_variable.h

@@ -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) {
-    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)
@@ -45,7 +47,9 @@ struct quadratic_variable {
 
   // linear evaluation
   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();
   }
 
@@ -98,15 +102,21 @@ struct quadratic_variable {
   }
 
   const matrix_x_t& A() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return A_;
   }
   const point_t& b() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return b_;
   }
   const Numeric c() const {
-    assert(!isZero());
+    if (isZero()) {
+      throw std::runtime_error("Not initialized! (isZero)");
+    }
     return c_;
   }
   bool isZero() const { return zero; }

include/curves/serialization/archive.hpp

@@ -59,7 +59,9 @@ struct Serializable {
   /// \brief Loads a Derived object from an XML file.
   template <class Derived>
   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());
     if (ifs) {
       boost::archive::xml_iarchive ia(ifs);
@@ -74,7 +76,9 @@ struct Serializable {
   /// \brief Saved a Derived object as an XML file.
   template <class Derived>
   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());
     if (ofs) {
       boost::archive::xml_oarchive oa(ofs);

include/curves/so3_linear.h

@@ -256,7 +256,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
       theta = PI_value;  // acos((-1-1)/2)
     else
       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.
     if (theta < PI_value - 1e-2) {

python/curves/python_variables.cpp

@@ -5,7 +5,9 @@
 
 namespace curves {
 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;
   for (int i = 0; i < vectors.cols(); ++i) {
     res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i)));