[travis][unitTests] add configuration and constructor tests; sync travis

.travis.yml

@@ -15,10 +15,15 @@ compiler:
 matrix:
   allow_failures:
   - compiler: clang
+
 before_install:
 - git submodule update --init --recursive
+#Add robotpkg and pinocchio dependencies
+- sudo sh -c "echo \"deb http://robotpkg.openrobots.org/packages/debian precise robotpkg\" >> /etc/apt/sources.list "
+- curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
 - sudo apt-get update -qq
-- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx python-numpy
+- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev 
+  python-sphinx python-numpy libtinyxml-dev robotpkg-console-bridge robotpkg-urdfdom-headers robotpkg-urdfdom robotpkg-eigenpy
 - sudo pip install cpp-coveralls
 language: cpp
 notifications:

.travis/build

@@ -14,10 +14,11 @@ rm -rf "$build_dir" "$install_dir"
 mkdir -p "$build_dir"
 mkdir -p "$install_dir"
 
+
 # Setup environment variables.
-export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$install_dir/lib:/opt/openrobots/lib:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
-export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH"
 export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
 
 install_dependency()
@@ -42,7 +43,6 @@ install_pinocchio_devel()
     make install
 }
 
-# Retrieve jrl-mathtools
 install_pinocchio_devel
 install_dependency proyan/dynamic-graph
 install_dependency proyan/dynamic-graph-python

unitTesting/CMakeLists.txt

@@ -24,6 +24,14 @@ test_config
 #  test_results
 )
 
+
+#----------------------------------------------------
+# Install procedure for the urdf files
+#----------------------------------------------------
+
+FILE(COPY ${CMAKE_CURRENT_SOURCE_DIR}/two_link.urdf
+  DESTINATION urdf)
+
 SET(test_dyn_plugins_dependencies dynamic)
 
 # Make Boost.Test generates the main function in test cases.

unitTesting/test_config.cpp

+
+/*--------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/dynamic.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/multibody/parser/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");
+  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);
+}
+
+

unitTesting/test_constructor.cpp

+
+/*--------STD-------------*/
+#include <sstream>
+
+/*-----------BOOST TEST SUITE-------------*/
+#define BOOST_TEST_MODULE sot_dynamic_constructor
+#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/dynamic.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/multibody/parser/urdf.hpp>
+
+
+using namespace dynamicgraph::sot;
+
+/* ----- TEST SIGNAL CLASS -----*/
+
+BOOST_AUTO_TEST_CASE (constructor)
+{
+  /*-----------------------CONSTRUCTOR-----------------------------------------*/
+  Dynamic dynamic_("sot_dynamic_test");
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointPositionSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::position"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerPositionSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffposition"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointVelocitySIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::velocity"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerVelocitySIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffvelocity"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.jointAccelerationSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::acceleration"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.freeFlyerAccelerationSIN.getName().c_str(),"sotDynamic(sot_dynamic_test)::input(vector)::ffacceleration"),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.newtonEulerSINTERN.getName().c_str(),"sotDynamic(sot_dynamic_test)::intern(dummy)::newtoneuler" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.zmpSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::zmp" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.JcomSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::Jcom" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.comSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::com" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::inertia" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.footHeightSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(double)::footHeight" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperJlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperJl" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.lowerJlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::lowerJl" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperVlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperVl" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.upperTlSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::upperTl" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.inertiaRotorSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(matrix)::inertiaRotor" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.MomentaSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::momenta" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.AngularMomentumSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::angularmomentum" ),0);
+  BOOST_CHECK_EQUAL(std::strcmp(dynamic_.dynamicDriftSOUT.getName().c_str(),"sotDynamic(sot_dynamic_test)::output(vector)::dynamicDrift" ),0);
+}

unitTesting/two_link.urdf

+<?xml version="1.0"?>
+<!--
+   simple_humanoid URDF model
+
+   FIXME: fill missing data: sole, gripper and sensors
+  -->
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="test_two_link">
+
+  <link name="base_link"/>
+  <joint name="JOINT1" type="revolute"> 
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="CHILD1"/>
+    <origin xyz="1 0 0"/>
+    <limit effort="12" lower="0" upper="3.14" velocity="10"/>
+  </joint>
+
+
+  <link name="CHILD1">
+    <inertial>
+      <origin xyz="0.5 0 0" rpy="0 0 0"/>
+      <mass value="10"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+    </inertial>
+  </link>
+    <joint name="JOINT2" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="CHILD1"/>
+    <child link="CHILD2"/>
+    <origin xyz="1 0 0"/>
+    <limit effort="12" lower="0" upper="3.14" velocity="10"/>
+  </joint>
+  <link name="CHILD2">
+    <inertial>
+      <origin xyz="0.5 0 0" rpy="0 0 0"/>
+      <mass value="20"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+    </inertial>
+  </link>
+<!--
+  <link name="CHILD3">
+    <inertial>
+      <origin xyz="0.5 0 0" rpy="0 0 0"/>
+      <mass value="20"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+    </inertial>
+  </link>
+  <link name="CHILD4">
+    <inertial>
+      <origin xyz="0.5 0 0" rpy="0 0 0"/>
+      <mass value="20"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+    </inertial>
+  </link>
+  <link name="CHILD5">
+    <inertial>
+      <origin xyz="0.5 0 0" rpy="0 0 0"/>
+      <mass value="20"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+    </inertial>
+  </link>
+  <joint name="JOINT3" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="CHILD2"/>
+    <child link="CHILD3"/>
+    <origin xyz="1 0 0"/>
+    <limit effort="100" lower="0" upper="3.14" velocity="10"/>
+  </joint>
+  <joint name="JOINT4" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="CHILD3"/>
+    <child link="CHILD4"/>
+    <origin xyz="1 0 0"/>
+    <limit effort="100" lower="0" upper="3.14" velocity="10"/>
+  </joint>
+  <joint name="JOINT5" type="revolute">
+    <axis xyz="0 0 1"/>
+    <parent link="CHILD4"/>
+    <child link="CHILD5"/>
+    <origin xyz="1 0 0"/>
+    <limit effort="100" lower="0" upper="3.14" velocity="10"/>
+  </joint>
+-->
+</robot>