Commit 4a28d1e7 authored by Wolfgang Merkt's avatar Wolfgang Merkt
Browse files

Revert assert changes to multibody and codegen

parent ac267812
......@@ -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;
......
......@@ -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());
......
......@@ -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;
......
......@@ -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
......
......@@ -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;
......
......@@ -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;
}
......
......@@ -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);
}
};
......
......@@ -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;
......
......@@ -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>());
......
......@@ -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;
}
......
......@@ -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>();
......
......@@ -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); }
......
......@@ -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);
}
......
......@@ -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); }
......
......@@ -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); }
......
......@@ -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
......
......@@ -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();
......
......@@ -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);
}
......
......@@ -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;
......
......@@ -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();
......
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