Commit 849fbd67 authored by jcarpent's avatar jcarpent
Browse files

[All] Uniformize naming

parent a6ec3039
...@@ -137,7 +137,7 @@ namespace tsid ...@@ -137,7 +137,7 @@ namespace tsid
int &rows, int &cols); int &rows, int &cols);
template<typename Derived> template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x) inline bool isFinite(const Eigen::MatrixBase<Derived>& x)
{ {
return ( (x - x).array() == (x - x).array()).all(); return ( (x - x).array() == (x - x).array()).all();
} }
......
...@@ -34,7 +34,7 @@ using namespace std; ...@@ -34,7 +34,7 @@ using namespace std;
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A) #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)
const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo"; const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";
......
...@@ -31,7 +31,7 @@ ...@@ -31,7 +31,7 @@
#include <tsid/utils/statistics.hpp> #include <tsid/utils/statistics.hpp>
#define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B) #define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B)
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A) #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
......
...@@ -43,7 +43,7 @@ using namespace std; ...@@ -43,7 +43,7 @@ using namespace std;
using namespace Eigen; using namespace Eigen;
using namespace tsid::robots; using namespace tsid::robots;
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A) #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)
const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo"; const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";
...@@ -95,13 +95,13 @@ BOOST_AUTO_TEST_CASE ( test_task_se3_equality ) ...@@ -95,13 +95,13 @@ BOOST_AUTO_TEST_CASE ( test_task_se3_equality )
BOOST_CHECK(constraint.rows()==6); BOOST_CHECK(constraint.rows()==6);
BOOST_CHECK(constraint.cols()==robot.nv()); BOOST_CHECK(constraint.cols()==robot.nv());
REQUIRE_FINITE(constraint.matrix()); REQUIRE_FINITE(constraint.matrix());
BOOST_REQUIRE(is_finite(constraint.vector())); BOOST_REQUIRE(isFinite(constraint.vector()));
pseudoInverse(constraint.matrix(), Jpinv, 1e-4); pseudoInverse(constraint.matrix(), Jpinv, 1e-4);
Vector dv = Jpinv * constraint.vector(); Vector dv = Jpinv * constraint.vector();
BOOST_REQUIRE(is_finite(Jpinv)); BOOST_REQUIRE(isFinite(Jpinv));
BOOST_CHECK(MatrixXd::Identity(6,6).isApprox(constraint.matrix()*Jpinv)); BOOST_CHECK(MatrixXd::Identity(6,6).isApprox(constraint.matrix()*Jpinv));
if(!is_finite(dv)) if(!isFinite(dv))
{ {
cout<< "Jpinv" << Jpinv.transpose() <<endl; cout<< "Jpinv" << Jpinv.transpose() <<endl;
cout<< "b" << constraint.vector().transpose() <<endl; cout<< "b" << constraint.vector().transpose() <<endl;
...@@ -110,12 +110,12 @@ BOOST_AUTO_TEST_CASE ( test_task_se3_equality ) ...@@ -110,12 +110,12 @@ BOOST_AUTO_TEST_CASE ( test_task_se3_equality )
v += dt*dv; v += dt*dv;
q = se3::integrate(robot.model(), q, dt*v); q = se3::integrate(robot.model(), q, dt*v);
BOOST_REQUIRE(is_finite(v)); BOOST_REQUIRE(isFinite(v));
BOOST_REQUIRE(is_finite(q)); BOOST_REQUIRE(isFinite(q));
t += dt; t += dt;
error = task.position_error().norm(); error = task.position_error().norm();
BOOST_REQUIRE(is_finite(task.position_error())); BOOST_REQUIRE(isFinite(task.position_error()));
BOOST_CHECK(error <= error_past); BOOST_CHECK(error <= error_past);
error_past = error; error_past = error;
...@@ -173,23 +173,23 @@ BOOST_AUTO_TEST_CASE ( test_task_com_equality ) ...@@ -173,23 +173,23 @@ BOOST_AUTO_TEST_CASE ( test_task_com_equality )
const ConstraintBase & constraint = task.compute(t, q, v, data); const ConstraintBase & constraint = task.compute(t, q, v, data);
BOOST_CHECK(constraint.rows()==3); BOOST_CHECK(constraint.rows()==3);
BOOST_CHECK(constraint.cols()==robot.nv()); BOOST_CHECK(constraint.cols()==robot.nv());
BOOST_REQUIRE(is_finite(constraint.matrix())); BOOST_REQUIRE(isFinite(constraint.matrix()));
BOOST_REQUIRE(is_finite(constraint.vector())); BOOST_REQUIRE(isFinite(constraint.vector()));
pseudoInverse(constraint.matrix(), Jpinv, 1e-5); pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
Vector dv = Jpinv * constraint.vector(); Vector dv = Jpinv * constraint.vector();
BOOST_REQUIRE(is_finite(Jpinv)); BOOST_REQUIRE(isFinite(Jpinv));
BOOST_CHECK(MatrixXd::Identity(constraint.rows(),constraint.rows()).isApprox(constraint.matrix()*Jpinv)); BOOST_CHECK(MatrixXd::Identity(constraint.rows(),constraint.rows()).isApprox(constraint.matrix()*Jpinv));
BOOST_REQUIRE(is_finite(dv)); BOOST_REQUIRE(isFinite(dv));
v += dt*dv; v += dt*dv;
q = se3::integrate(robot.model(), q, dt*v); q = se3::integrate(robot.model(), q, dt*v);
BOOST_REQUIRE(is_finite(v)); BOOST_REQUIRE(isFinite(v));
BOOST_REQUIRE(is_finite(q)); BOOST_REQUIRE(isFinite(q));
t += dt; t += dt;
error = task.position_error().norm(); error = task.position_error().norm();
BOOST_REQUIRE(is_finite(task.position_error())); BOOST_REQUIRE(isFinite(task.position_error()));
BOOST_CHECK((error - error_past) <= 1e-4); BOOST_CHECK((error - error_past) <= 1e-4);
error_past = error; error_past = error;
...@@ -245,23 +245,23 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture ) ...@@ -245,23 +245,23 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
const ConstraintBase & constraint = task.compute(t, q, v, data); const ConstraintBase & constraint = task.compute(t, q, v, data);
BOOST_CHECK(constraint.rows()==na); BOOST_CHECK(constraint.rows()==na);
BOOST_CHECK(constraint.cols()==robot.nv()); BOOST_CHECK(constraint.cols()==robot.nv());
BOOST_REQUIRE(is_finite(constraint.matrix())); BOOST_REQUIRE(isFinite(constraint.matrix()));
BOOST_REQUIRE(is_finite(constraint.vector())); BOOST_REQUIRE(isFinite(constraint.vector()));
pseudoInverse(constraint.matrix(), Jpinv, 1e-5); pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
Vector dv = Jpinv * constraint.vector(); Vector dv = Jpinv * constraint.vector();
BOOST_REQUIRE(is_finite(Jpinv)); BOOST_REQUIRE(isFinite(Jpinv));
BOOST_CHECK(MatrixXd::Identity(na,na).isApprox(constraint.matrix()*Jpinv)); BOOST_CHECK(MatrixXd::Identity(na,na).isApprox(constraint.matrix()*Jpinv));
BOOST_REQUIRE(is_finite(dv)); BOOST_REQUIRE(isFinite(dv));
v += dt*dv; v += dt*dv;
q = se3::integrate(robot.model(), q, dt*v); q = se3::integrate(robot.model(), q, dt*v);
BOOST_REQUIRE(is_finite(v)); BOOST_REQUIRE(isFinite(v));
BOOST_REQUIRE(is_finite(q)); BOOST_REQUIRE(isFinite(q));
t += dt; t += dt;
error = task.position_error().norm(); error = task.position_error().norm();
BOOST_REQUIRE(is_finite(task.position_error())); BOOST_REQUIRE(isFinite(task.position_error()));
BOOST_CHECK(error <= error_past); BOOST_CHECK(error <= error_past);
error_past = error; error_past = error;
......
...@@ -45,7 +45,7 @@ using namespace tsid::robots; ...@@ -45,7 +45,7 @@ using namespace tsid::robots;
using namespace std; using namespace std;
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(is_finite(A), #A<<": "<<A) #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)
#define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B) #define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B)
#define REQUIRE_TASK_FINITE(task) REQUIRE_FINITE(task.getConstraint().matrix()); \ #define REQUIRE_TASK_FINITE(task) REQUIRE_FINITE(task.getConstraint().matrix()); \
......
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