Commit 17897a88 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

fix unittest with dintegrate API

parent d75444f5
......@@ -235,81 +235,48 @@ BOOST_AUTO_TEST_CASE ( dIntegrate_assignementop_test )
//SETTO
dIntegrate(model,qs,vs,results[0],ARG0);
dIntegrate<SETTO>(model,qs,vs,results[1],ARG0);
dIntegrate(model,qs,vs,results[1],ARG0,SETTO);
BOOST_CHECK(results[0].isApprox(results[1]));
//ADDTO
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG0);
dIntegrate<ADDTO>(model,qs,vs,results[1],ARG0);
dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
dIntegrate(model,qs,vs,results[1],ARG0,ADDTO);
BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
//RMTO
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG0);
dIntegrate<RMTO>(model,qs,vs,results[1],ARG0);
dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
dIntegrate(model,qs,vs,results[1],ARG0,RMTO);
BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
//MUL_LEFT
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG0);
dIntegrate<APPLY_ON_THE_LEFT>(model,qs,vs,results[1],ARG0);
BOOST_CHECK(results[1].isApprox(results[0] * results[2]));
//MUL_RIGHT
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG0);
dIntegrate<APPLY_ON_THE_RIGHT>(model,qs,vs,results[1],ARG0);
BOOST_CHECK(results[1].isApprox(results[2] * results[0]));
//SETTO
results[0].setZero();
results[1].setZero();
dIntegrate(model,qs,vs,results[0],ARG1);
dIntegrate<SETTO>(model,qs,vs,results[1],ARG1);
dIntegrate(model,qs,vs,results[1],ARG1,SETTO);
BOOST_CHECK(results[0].isApprox(results[1]));
//ADDTO
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG1);
dIntegrate<ADDTO>(model,qs,vs,results[1],ARG1);
dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
dIntegrate(model,qs,vs,results[1],ARG1,ADDTO);
BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
//RMTO
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG1);
dIntegrate<RMTO>(model,qs,vs,results[1],ARG1);
dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
dIntegrate(model,qs,vs,results[1],ARG1,RMTO);
BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
//MUL_LEFT
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG1);
dIntegrate<APPLY_ON_THE_LEFT>(model,qs,vs,results[1],ARG1);
BOOST_CHECK(results[1].isApprox(results[0] * results[2]));
//MUL_RIGHT
results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
results[2] = results[1];
results[0].setZero();
dIntegrate<SETTO>(model,qs,vs,results[0],ARG1);
dIntegrate<APPLY_ON_THE_RIGHT>(model,qs,vs,results[1],ARG1);
BOOST_CHECK(results[1].isApprox(results[2] * results[0]));
}
BOOST_AUTO_TEST_CASE ( integrate_difference_test )
......
......@@ -316,8 +316,8 @@ struct LieGroup_Jintegrate{
ConfigVector_t q_v = lg.integrate (q, v);
JacobianMatrix_t Jq, Jv;
lg.dIntegrate_dq (q, v, Jq);
lg.dIntegrate_dv (q, v, Jv);
lg.dIntegrate_dq (q, v, Jq, SETTO);
lg.dIntegrate_dv (q, v, Jv, SETTO);
const Scalar eps = 1e-6;
for (int i = 0; i < v.size(); ++i)
......@@ -361,7 +361,7 @@ struct LieGroup_JintegrateJdifference{
JacobianMatrix_t Jd_qb, Ji_v;
lg.template dDifference<ARG1> (qa, qb, Jd_qb);
lg.template dIntegrate <ARG1> (qa, v , Ji_v );
lg.template dIntegrate <ARG1, SETTO> (qa, v , Ji_v );
BOOST_CHECK_MESSAGE ((Jd_qb * Ji_v).isIdentity(),
"Jd_qb\n" <<
......
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