Commit 59ed233c authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[unittest][test_config] remove test_config

parent 95eac1ad
......@@ -18,7 +18,7 @@ ADD_DEFINITIONS(-DDEBUG=2)
SET(tests
test_constructor
test_config
#test_config
# dummy
# test_djj
# test_dyn
......
/*--------STD-------------*/
#include <sstream>
/*-----------BOOST TEST SUITE-------------*/
#define BOOST_TEST_MODULE sot_dynamic_config
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/output_test_stream.hpp>
/*-----------SOT DYNAMIC ------------*/
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/core/debug.hh>
/*-----------DYNAMIC GRAPH ------------*/
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/exception-abstract.hh>
/*-----------PINOCCHIO-------------*/
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
using namespace dynamicgraph::sot;
#define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
for (unsigned i = 0; i < N; ++i) \
for (unsigned j = 0; j < M; ++j) \
BOOST_REQUIRE_CLOSE(LEFT (i, j), RIGHT (i, j), TOLERANCE)
/* ----- TEST SIGNAL CLASS -----*/
BOOST_AUTO_TEST_CASE (config)
{
/*-----------------------CONSTRUCTOR-----------------------------------------*/
Dynamic dynamic_("sot_dynamic_test");
/*------------------------CONFIG-------------------------------------------*/
//Create Empty Robot
dynamic_.createRobot();
BOOST_CHECK_EQUAL(dynamic_.m_model.nbody,1);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[0].c_str(),"universe"),0);
//Parse urdf file
dynamic_.setUrdfFile("urdf/two_link.urdf");
dynamic_.parseUrdfFile();
dynamic_.displayModel();
BOOST_CHECK_EQUAL(dynamic_.m_model.nbody,3);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[0].c_str(),"universe"),0);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[1].c_str(),"JOINT1"),0);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[2].c_str(),"JOINT2"),0);
//CreateJoint and AddBody
se3::SE3 joint_3_pos(Eigen::Matrix3d::Identity(),
(Eigen::Vector3d() << 1,0,0).finished());
dynamic_.createJoint("JOINT3","JointModelRZ",joint_3_pos.toHomogeneousMatrix());
dynamic_.addBody("CHILD2","JOINT3","CHILD3");
BOOST_CHECK_EQUAL(dynamic_.m_model.nbody,4);
BOOST_CHECK_EQUAL(dynamic_.m_model.nq,3);
BOOST_CHECK_EQUAL(std::strcmp(dynamic_.m_model.names[3].c_str(),"JOINT3"),0);
//Setter and Getter
dynamic_.setMass("CHILD3",10);
BOOST_CHECK_EQUAL(dynamic_.m_model.inertias[3].mass(),10);
Eigen::Vector3d vx; vx<<0.5,0,0;
dynamic_.setLocalCenterOfMass("CHILD3", vx);
MATRIX_BOOST_REQUIRE_CLOSE(3, 1, dynamic_.m_model.inertias[3].lever(), vx, 0.0001);
dynamic_.setInertiaMatrix("CHILD3",Eigen::Matrix3d::Identity());
MATRIX_BOOST_REQUIRE_CLOSE(3, 3, dynamic_.m_model.inertias[3].inertia(),
Eigen::Matrix3d::Identity(), 0.0001);
dynamicgraph::Vector result_vec;
dynamic_.setDofBounds("JOINT3",0,0,1.57);
dynamic_.getLowerPositionLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,Eigen::Vector3d::Zero(), 0.0001);
dynamic_.getUpperPositionLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 3.14,3.14,1.57).finished(), 0.0001);
dynamic_.setLowerPositionLimit("JOINT3",(Eigen::Matrix<double,1,1>() << 0.2).finished());
dynamic_.getLowerPositionLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 0,0,0.2).finished(), 0.0001);
dynamic_.setLowerPositionLimit("JOINT3",0.12);
dynamic_.getLowerPositionLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 0,0,0.12).finished(), 0.0001);
dynamic_.setUpperPositionLimit("JOINT3",2.12);
dynamic_.getUpperPositionLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 3.14,3.14,2.12).finished(), 0.0001);
dynamic_.setMaxVelocityLimit("JOINT3",9.12);
dynamic_.getUpperVelocityLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 10,10,9.12).finished(), 0.0001);
dynamic_.setMaxEffortLimit("JOINT3",9.12);
dynamic_.getMaxEffortLimits(result_vec,0);
MATRIX_BOOST_REQUIRE_CLOSE(3,1,result_vec,
(Eigen::Vector3d() << 12,12,9.12).finished(), 0.0001);
}
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