Commit 6e7bec22 authored by Wolfgang Merkt's avatar Wolfgang Merkt
Browse files

Revert assert changes in src/spatial

parent 54503f81
......@@ -20,10 +20,9 @@
#define PINOCCHIO_STRING_LITERAL(string) #string
#include <exception>
#include <cassert>
#undef assert
#define PINOCCHIO_ASSERT_THROW_AT_RUNTIME(cond) if (!(cond)) { throw std::runtime_error("Wrong argument size"); }
#define PINOCCHIO_ASSERT_THROW_AT_RUNTIME(cond, message) if (!(cond)) { throw std::runtime_error(message); }
// #define PINOCCHIO_ASSERT_THROW_AT_RUNTIME(cond) void(0);
// For more details, visit https://stackoverflow.com/questions/171435/portability-of-warning-preprocessor-directive
......
......@@ -54,7 +54,7 @@ namespace pinocchio
fout -= m.act(fin);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -108,7 +108,7 @@ namespace pinocchio
fout -= v.cross(fin);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -187,7 +187,7 @@ namespace pinocchio
fout -= m.actInv(fin);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -309,7 +309,7 @@ namespace pinocchio
mout -= m.act(min);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -385,7 +385,7 @@ namespace pinocchio
mout -= v.cross(min);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -434,7 +434,7 @@ namespace pinocchio
mout -= m.actInv(min);
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......@@ -509,7 +509,7 @@ namespace pinocchio
fout -= I*min;
break;
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "Wrong Op requesed value");
assert(false && "Wrong Op requesed value");
break;
}
}
......
......@@ -27,7 +27,7 @@ namespace pinocchio
Eigen::QuaternionBase<QuaternionLike> & quat_out)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 3);
assert(v.size() == 3);
typedef typename Vector3Like::Scalar Scalar;
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options };
......@@ -147,7 +147,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix43Like> & Jexp)
{
// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
typedef typename Vector3Like::Scalar Scalar;
......
......@@ -91,7 +91,7 @@ namespace pinocchio
if(tr > Scalar(3)) theta = 0; // acos((3-1)/2)
else if(tr < Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
else theta = math::acos((tr - Scalar(1))/Scalar(2));
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(theta == theta && "theta contains some NaN"); // theta != NaN
assert(theta == theta && "theta contains some NaN"); // theta != NaN
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
if (theta < PI_value - 1e-2)
......
......@@ -63,7 +63,7 @@ namespace pinocchio
template<typename V6>
Derived & operator=(const Eigen::MatrixBase<V6> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 6);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
linear() = v.template segment<3>(LINEAR);
angular() = v.template segment<3>(ANGULAR);
return derived();
......
......@@ -64,7 +64,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(f_like.size() == 6);
assert(f_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return m_ref; }
......@@ -112,7 +112,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(f_like.size() == 6);
assert(f_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return m_ref; }
......
......@@ -51,8 +51,8 @@ namespace pinocchio
template<typename V1,typename V2>
ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(w.size() == 3);
assert(v.size() == 3);
assert(w.size() == 3);
linear() = v; angular() = w;
}
......@@ -61,7 +61,7 @@ namespace pinocchio
: m_data(v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 6);
assert(v.size() == 6);
}
template<int O2>
......
......@@ -169,7 +169,7 @@ namespace pinocchio
InertiaTpl(const Matrix6 & I6)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME((I6 - I6.transpose()).isMuchSmallerThan(I6));
assert((I6 - I6.transpose()).isMuchSmallerThan(I6));
mass() = I6(LINEAR, LINEAR);
const Matrix3 & mc_cross = I6.template block <3,3>(ANGULAR,LINEAR);
lever() = unSkew(mc_cross);
......
......@@ -85,7 +85,7 @@ namespace pinocchio
template<typename V6>
Derived & operator=(const Eigen::MatrixBase<V6> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 6);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
linear() = v.template segment<3>(LINEAR);
angular() = v.template segment<3>(ANGULAR);
return derived();
......
......@@ -87,7 +87,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v_like.size() == 6);
assert(v_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return m_ref; }
......@@ -183,7 +183,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v_like.size() == 6);
assert(v_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return m_ref; }
......
......@@ -59,8 +59,8 @@ namespace pinocchio
template<typename V1,typename V2>
MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(w.size() == 3);
assert(v.size() == 3);
assert(w.size() == 3);
linear() = v; angular() = w;
}
......@@ -69,7 +69,7 @@ namespace pinocchio
: m_data(v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 6);
assert(v.size() == 6);
}
template<int O2>
......
......@@ -84,7 +84,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME((M + M.transpose()).isMuchSmallerThan(M));
assert((M + M.transpose()).isMuchSmallerThan(M));
Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3,v);
typedef typename Vector3::RealScalar Scalar;
......
......@@ -32,7 +32,7 @@ namespace pinocchio
explicit Symmetric3Tpl(const Eigen::Matrix<Sc,N,N,Opt> & I)
{
EIGEN_STATIC_ASSERT(N==3,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (I-I.transpose()).isMuchSmallerThan(I) );
assert( (I-I.transpose()).isMuchSmallerThan(I) );
m_data(0) = I(0,0);
m_data(1) = I(1,0); m_data(2) = I(1,1);
m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2);
......@@ -379,7 +379,7 @@ namespace pinocchio
Symmetric3Tpl operator-(const Matrix3 &S) const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (S-S.transpose()).isMuchSmallerThan(S) );
assert( (S-S.transpose()).isMuchSmallerThan(S) );
return Symmetric3Tpl( m_data(0)-S(0,0),
m_data(1)-S(1,0), m_data(2)-S(1,1),
m_data(3)-S(2,0), m_data(4)-S(2,1), m_data(5)-S(2,2) );
......@@ -387,7 +387,7 @@ namespace pinocchio
Symmetric3Tpl operator+(const Matrix3 &S) const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (S-S.transpose()).isMuchSmallerThan(S) );
assert( (S-S.transpose()).isMuchSmallerThan(S) );
return Symmetric3Tpl( m_data(0)+S(0,0),
m_data(1)+S(1,0), m_data(2)+S(1,1),
m_data(3)+S(2,0), m_data(4)+S(2,1), m_data(5)+S(2,2) );
......@@ -413,7 +413,7 @@ namespace pinocchio
Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");
assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");
Symmetric3Tpl Sres;
......
Markdown is supported
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