Revert assert changes to multibody and codegen

src/codegen/code-generator-algo.hpp

@@ -256,7 +256,7 @@ namespace pinocchio
         }
       }
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(it_Y == Base::getOutputDimension());
+      assert(it_Y == Base::getOutputDimension());
       
       ad_fun.Dependent(ad_X,ad_Y);
       ad_fun.optimize("no_compare_op");
@@ -281,7 +281,7 @@ namespace pinocchio
         }
       }
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(it_Y == Base::getOutputDimension());
+      assert(it_Y == Base::getOutputDimension());
     }
     
   protected:
@@ -343,7 +343,7 @@ namespace pinocchio
         }
       }
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(it_Y == Base::getOutputDimension());
+      assert(it_Y == Base::getOutputDimension());
       
       ad_fun.Dependent(ad_X,ad_Y);
       ad_fun.optimize("no_compare_op");
@@ -438,7 +438,7 @@ namespace pinocchio
                                   ad_q,ad_v,ad_a,
                                   ad_dtau_dq,ad_dtau_dv,ad_dtau_da);
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(ad_Y.size() == Base::getOutputDimension());
+      assert(ad_Y.size() == Base::getOutputDimension());
       
       Eigen::DenseIndex it_Y = 0;
       Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dq;
@@ -547,7 +547,7 @@ namespace pinocchio
                                  ad_q,ad_v,ad_tau,
                                  ad_dddq_dq,ad_dddq_dv,ad_dddq_dtau);
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(ad_Y.size() == Base::getOutputDimension());
+      assert(ad_Y.size() == Base::getOutputDimension());
       
       Eigen::DenseIndex it_Y = 0;
       Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dq;

src/codegen/code-generator-base.hpp

@@ -122,7 +122,7 @@ namespace pinocchio
     template<typename Vector>
     void evalFunction(const Eigen::MatrixBase<Vector> & x)
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(build_forward);
+      assert(build_forward);
       
       generatedFun_ptr->ForwardZero(PINOCCHIO_EIGEN_CONST_CAST(Vector,x),y);
     }
@@ -130,7 +130,7 @@ namespace pinocchio
     template<typename Vector>
     void evalJacobian(const Eigen::MatrixBase<Vector> & x)
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(build_jacobian);
+      assert(build_jacobian);
       
       CppAD::cg::ArrayView<const Scalar> x_(PINOCCHIO_EIGEN_CONST_CAST(Vector,x).data(),(size_t)x.size());
       CppAD::cg::ArrayView<Scalar> jac_(jac.data(),(size_t)jac.size());

src/multibody/data.hxx

@@ -166,7 +166,7 @@ namespace pinocchio
       const int nvj    = nv   (model.joints[joint]);
       const int idx_vj = idx_v(model.joints[joint]);
       
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(idx_vj >= 0 && idx_vj < model.nv);
+      assert(idx_vj >= 0 && idx_vj < model.nv);
       
       if(parent>0) parents_fromRow[(Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
       else         parents_fromRow[(Index)idx_vj] = -1;

src/multibody/fcl.hxx

@@ -20,7 +20,7 @@ namespace pinocchio
   inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2) 
     : Base(co1,co2)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(co1 != co2, "The index of collision objects must not be equal.");
+    assert(co1 != co2 && "The index of collision objects must not be equal.");
   }
 
   inline bool CollisionPair::operator== (const CollisionPair& rhs) const

src/multibody/force-set.hpp

@@ -31,7 +31,7 @@ namespace pinocchio
     { m_f.fill(NAN); m_n.fill(NAN); }
     ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
       : size((int)linear.cols()),m_f(linear), m_n(angular)
-    {  PINOCCHIO_ASSERT_THROW_AT_RUNTIME( linear.cols() == angular.cols() ); }
+    {  assert( linear.cols() == angular.cols() ); }
 
     Matrix6x matrix() const
     {
@@ -92,7 +92,7 @@ namespace pinocchio
 
       Block& operator= (const ForceSetTpl & copy)
       {
-	PINOCCHIO_ASSERT_THROW_AT_RUNTIME(copy.size == len);
+	assert(copy.size == len);
 	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
 	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
 	return *this;
@@ -100,7 +100,7 @@ namespace pinocchio
 
       Block& operator= (const ForceSetTpl::Block & copy)
       {
-	PINOCCHIO_ASSERT_THROW_AT_RUNTIME(copy.len == len);
+	assert(copy.len == len);
 	linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
 	angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
 	return *this;
@@ -110,7 +110,7 @@ namespace pinocchio
       Block& operator= (const Eigen::MatrixBase<D> & m)
       {
         eigen_assert(D::RowsAtCompileTime == 6);
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(m.cols() == len);
+        assert(m.cols() == len);
         linear() = m.template topRows<3>();
         angular() = m.template bottomRows<3>();
         return *this;

src/multibody/geometry.hxx

@@ -42,9 +42,9 @@ namespace pinocchio
   GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object,
                                              const ModelTpl<S2,O2,JointCollectionTpl> & model)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( //TODO: reenable when relevant (object.parentFrame == -1) ||
+    assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
            (model.frames[object.parentFrame].type == pinocchio::BODY)  );
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( //TODO: reenable when relevant (object.parentFrame == -1) ||
+    assert( //TODO: reenable when relevant (object.parentFrame == -1) ||
            (model.frames[object.parentFrame].parent == object.parentJoint) );
     
     GeomIndex idx = (GeomIndex) (ngeoms ++);
@@ -82,7 +82,7 @@ namespace pinocchio
 
   inline const std::string& GeometryModel::getGeometryName(const GeomIndex index) const
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( index < (GeomIndex)geometryObjects.size() );
+    assert( index < (GeomIndex)geometryObjects.size() );
     return geometryObjects[index].name;
   }
 
@@ -168,7 +168,7 @@ namespace pinocchio
   
   inline void GeometryModel::addCollisionPair (const CollisionPair & pair)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (pair.first < ngeoms) && (pair.second < ngeoms) );
+    assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
     if (!existCollisionPair(pair)) { collisionPairs.push_back(pair); }
   }
   
@@ -189,7 +189,7 @@ namespace pinocchio
   
   inline void GeometryModel::removeCollisionPair (const CollisionPair & pair)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( (pair.first < ngeoms) && (pair.second < ngeoms) );
+    assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
 
     CollisionPairVector::iterator it = std::find(collisionPairs.begin(),
                                                  collisionPairs.end(),
@@ -217,19 +217,19 @@ namespace pinocchio
 
   inline void GeometryData::activateCollisionPair(const PairIndex pairId,const bool flag)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( pairId < activeCollisionPairs.size() );
+    assert( pairId < activeCollisionPairs.size() );
     activeCollisionPairs[pairId] = flag;
   }
   
   inline void GeometryData::activateCollisionPair(const PairIndex pairId)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( pairId < activeCollisionPairs.size() );
+    assert( pairId < activeCollisionPairs.size() );
     activeCollisionPairs[pairId] = true;
   }
 
   inline void GeometryData::deactivateCollisionPair(const PairIndex pairId)
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME( pairId < activeCollisionPairs.size() );
+    assert( pairId < activeCollisionPairs.size() );
     activeCollisionPairs[pairId] = false;
   }
 

src/multibody/joint/joint-common-operations.hpp

@@ -57,7 +57,7 @@ namespace pinocchio
                     const Scalar & offset,
                     const Eigen::MatrixBase<ConfigVectorOut> & dest)
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == dest.size());
+      assert(q.size() == dest.size());
       PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest).noalias() = scaling * q + ConfigVectorOut::Constant(dest.size(),offset);
     }
   };

src/multibody/joint/joint-composite.hxx

@@ -58,8 +58,8 @@ namespace pinocchio
   inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::
   calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(joints.size() > 0);
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(data.joints.size() == joints.size());
+    assert(joints.size() > 0);
+    assert(data.joints.size() == joints.size());
     
     typedef JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo;
 
@@ -133,8 +133,8 @@ namespace pinocchio
          const Eigen::MatrixBase<ConfigVectorType> & qs,
          const Eigen::MatrixBase<TangentVectorType> & vs) const
   {
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(joints.size() > 0);
-    PINOCCHIO_ASSERT_THROW_AT_RUNTIME(jdata.joints.size() == joints.size());
+    assert(joints.size() > 0);
+    assert(jdata.joints.size() == joints.size());
     
     typedef JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo;
 

src/multibody/joint/joint-free-flyer.hpp

@@ -218,8 +218,8 @@ namespace pinocchio
       typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
 
       ConstQuaternionMap quat(q_joint.template tail<4>().data());
-      //PINOCCHIO_ASSERT_THROW_AT_RUNTIME(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
+      //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
+      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
       
       M.rotation(quat.matrix());
       M.translation(q_joint.template head<3>());

src/multibody/joint/joint-mimic.hpp

@@ -87,7 +87,7 @@ namespace pinocchio
     template<typename VectorLike>
     JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == nv());
+      assert(v.size() == nv());
       JointMotion jm = m_constraint * v;
       return jm * m_scaling_factor;
     }

src/multibody/joint/joint-planar.hpp

@@ -239,7 +239,7 @@ namespace pinocchio
       {
         typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;
         typedef Eigen::Matrix<Scalar, 3, Derived::ColsAtCompileTime> MatrixReturnType;
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(F.rows()==6);
+        assert(F.rows()==6);
 
         MatrixReturnType result(3, F.cols ());
         result.template topRows<2>() = F.template topRows<2>();

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -471,7 +471,7 @@ namespace pinocchio
     : axis(x,y,z)
     {
       axis.normalize();
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Translation axis is not unitary");
+      assert(isUnitary(axis) && "Translation axis is not unitary");
     }
     
     template<typename Vector3Like>
@@ -479,7 +479,7 @@ namespace pinocchio
     : axis(axis)
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Translation axis is not unitary");
+      assert(isUnitary(axis) && "Translation axis is not unitary");
     }
 
     JointDataDerived createData() const { return JointDataDerived(axis); }

src/multibody/joint/joint-prismatic.hpp

@@ -291,7 +291,7 @@ namespace pinocchio
     JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
     {
       EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == 1);
+      assert(v.size() == 1);
       return JointMotion(v[0]);
     }
 
@@ -334,7 +334,7 @@ namespace pinocchio
       typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
       operator*(const Eigen::MatrixBase<Derived> & F )
       {
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(F.rows()==6);
+        assert(F.rows()==6);
         return F.row(LINEAR+axis);
       }
 

src/multibody/joint/joint-revolute-unaligned.hpp

@@ -494,7 +494,7 @@ namespace pinocchio
     : axis(x,y,z)
     {
       axis.normalize();
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Rotation axis is not unitary");
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
     }
     
     template<typename Vector3Like>
@@ -502,7 +502,7 @@ namespace pinocchio
     : axis(axis)
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Rotation axis is not unitary");
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
     }
 
     JointDataDerived createData() const { return JointDataDerived(axis); }

src/multibody/joint/joint-revolute-unbounded-unaligned.hpp

@@ -113,7 +113,7 @@ namespace pinocchio
     : axis(x,y,z)
     {
       axis.normalize();
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Rotation axis is not unitary");
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
     }
     
     template<typename Vector3Like>
@@ -121,7 +121,7 @@ namespace pinocchio
     : axis(axis)
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "Rotation axis is not unitary");
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
     }
 
     JointDataDerived createData() const { return JointDataDerived(axis); }

src/multibody/joint/joint-revolute.hpp

@@ -134,7 +134,7 @@ namespace pinocchio
         }
         default:
         {
-          PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(false, "must nerver happened");
+          assert(false && "must nerver happened");
           break;
         }
       }
@@ -183,7 +183,7 @@ namespace pinocchio
         }
         default:
         {
-          PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(false, "must nerver happened");
+          assert(false && "must nerver happened");
           break;
         }
       }
@@ -402,7 +402,7 @@ namespace pinocchio
       typename ConstraintForceSetOp<ConstraintRevoluteTpl,Derived>::ReturnType
       operator*(const Eigen::MatrixBase<Derived> & F) const
       {
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(F.rows()==6);
+        assert(F.rows()==6);
         return F.row(ANGULAR + axis);
       }
     }; // struct TransposeConst

src/multibody/joint/joint-spherical.hpp

@@ -225,7 +225,7 @@ namespace pinocchio
       const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
       operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
       {
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(F.rows()==6);
+        assert(F.rows()==6);
         return F.derived().template middleRows<3>(Inertia::ANGULAR);
       }
     };
@@ -410,8 +410,8 @@ namespace pinocchio
       typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
 
       ConstQuaternionMap quat(q_joint.derived().data());
-      //PINOCCHIO_ASSERT_THROW_AT_RUNTIME(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
+      //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
+      assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
       
       M.rotation(quat.matrix());
       M.translation().setZero();

src/multibody/joint/joint-translation.hpp

@@ -298,7 +298,7 @@ namespace pinocchio
       const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
       operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
       {
-        PINOCCHIO_ASSERT_THROW_AT_RUNTIME(F.rows()==6);
+        assert(F.rows()==6);
         return F.derived().template middleRows<3>(LINEAR);
       }
       

src/multibody/liegroup/cartesian-product-variant.hpp

@@ -94,9 +94,9 @@ namespace pinocchio
                         const Eigen::MatrixBase<Velocity_t> & v,
                         const Eigen::MatrixBase<ConfigOut_t> & qout) const
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == m_nq);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == m_nv);
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == m_nq);
+      assert(q.size() == m_nq);
+      assert(v.size() == m_nv);
+      assert(qout.size() == m_nq);
      
       ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout);
       Index id_q = 0, id_v = 0;

src/multibody/liegroup/cartesian-product.hpp

@@ -111,7 +111,7 @@ namespace pinocchio
     void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
                                          const Eigen::MatrixBase<Jacobian_t> & J) const
     {
-      PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(J.rows() == nq() && J.cols() == nv(), "J is not of the right dimension");
+      assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
       Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
       J_.topRightCorner(lg1_.nq(),lg2_.nv()).setZero();
       J_.bottomLeftCorner(lg2_.nq(),lg1_.nv()).setZero();