Merge branch 'unittest' into 'master'

Added unittest code and CI setup

See merge request gepetto/example-robot-data!1

.gitignore

+*.pyc
+*.pyd
+*.pyo

.gitlab-ci.yml

+include: http://rainboard.laas.fr/project/example-robot-data/.gitlab-ci.yml

CMakeLists.txt

 CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
 
 INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/test.cmake)
 
+SET(PROJECT_NAMESPACE gepetto)
 SET(PROJECT_NAME example-robot-data)
 SET(PROJECT_DESCRIPTION "Set of robot URDFs for benchmarking and developed examples.")
 
-SET(PROJECT_URL https://gepgitlab.laas.fr/gepetto/${PROJECT_NAME})
+SET(PROJECT_URL https://gepgitlab.laas.fr/${PROJECT_NAMESPACE}/${PROJECT_NAME})
 
 SETUP_PROJECT()
 
 INSTALL(DIRECTORY hyq_description DESTINATION share/${PROJECT_NAME})
 INSTALL(DIRECTORY talos_data DESTINATION share/${PROJECT_NAME})
 
+ADD_SUBDIRECTORY(unittest)
+
 SETUP_PROJECT_FINALIZE()

README.md

@@ -5,5 +5,36 @@ Example robot URDFs
 
 This repository includes a set of robot descriptions that are aimed to be used in benchmarking. These source files do not intend to substitute original their repositories.
 
+
 **Authors:** Carlos Mastalli <br />
 **With additional support from the Gepetto team at LAAS-CNRS.**
+
+[![pipeline status](https://gepgitlab.laas.fr/gepetto/example-robot-data/badges/master/build.svg)](https://gepgitlab.laas.fr/Gepetto/example-robot-data/commits/master)
+
+
+## <img align="center" height="20" src="https://i.imgur.com/x1morBF.png"/> Installation
+You can install this package throught robotpkg. robotpkg is a package manager tailored for robotics softwares. It greatly simplifies the release of new versions along with the management of their dependencies. You just need to add the robotpkg apt repository to your sources.list and then use `sudo apt install robotpkg-example-robot-data`:
+
+### Add robotpkg apt repository
+If you have never added robotpkg as a softwares repository, please follow first the instructions from 1 to 4. Otherwise, go directly to instruction 5. Those instructions are similar to the installation procedures presented in [http://robotpkg.openrobots.org/debian.html](http://robotpkg.openrobots.org/debian.html).
+
+1. Check your distribution codename in a terminal with the following command:
+
+		$ lsb_release -c
+		Codename:       xenial
+
+2. Add robotpkg as source repository to apt:
+
+		sudo sh -c "echo 'deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg' >> /etc/apt/sources.list.d/robotpkg.list"
+
+3. Register the authentication certificate of robotpkg:
+
+		curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
+
+4. You need to run at least once apt update to fetch the package descriptions:
+
+		sudo apt-get update
+
+5. The installation of example-robot-data:
+
+		sudo apt install robotpkg-example-robot-data

unittest/CMakeLists.txt

+SET(${PROJECT_NAME}_PYTHON_TESTS
+  load
+  )
+
+FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+  ADD_PYTHON_UNIT_TEST("py-${TEST}" "unittest/test_${TEST}.py")
+ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
+

unittest/test_load.py

+#!/usr/bin/env python2
+
+import unittest
+
+from unittest_utils import loadHyQ, loadTalos, loadTalosArm
+
+
+class RobotTestCase(unittest.TestCase):
+    ROBOT = None
+    NQ = None
+    NV = None
+
+    def test_nq(self):
+        model = self.ROBOT.model
+        self.assertEqual(model.nq, self.NQ, "Wrong nq value.")
+
+    def test_nv(self):
+        model = self.ROBOT.model
+        self.assertEqual(model.nv, self.NV, "Wrong nv value.")
+
+    def test_q0(self):
+        self.assertTrue(hasattr(self.ROBOT, "q0"), "It doesn't have q0")
+
+
+class TalosArmTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.NQ = 7
+    RobotTestCase.NV = 7
+
+
+class TalosArmFloatingTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalosArm()
+    RobotTestCase.NQ = 14
+    RobotTestCase.NV = 13
+
+
+class TalosTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadTalos()
+    RobotTestCase.NQ = 39
+    RobotTestCase.NV = 38
+
+
+class HyQTest(RobotTestCase):
+    RobotTestCase.ROBOT = loadHyQ()
+    RobotTestCase.NQ = 19
+    RobotTestCase.NV = 18
+
+
+if __name__ == '__main__':
+    unittest.main()

unittest/unittest_utils.py

+from os.path import exists, join
+
+import numpy as np
+
+import pinocchio
+from pinocchio.robot_wrapper import RobotWrapper
+
+
+def getModelPath(subpath):
+    for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
+        if exists(join(path, subpath.strip('/'))):
+            print "using %s as modelPath" % path
+            return path
+    raise IOError('%s not found' % (subpath))
+
+
+def readParamsFromSrdf(robot, SRDF_PATH, verbose):
+    rmodel = robot.model
+
+    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
+    rmodel.armature = \
+        np.multiply(rmodel.rotorInertia.flat,
+                    np.square(rmodel.rotorGearRatio.flat))
+    try:
+        pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
+        robot.q0.flat[:] = \
+            rmodel.referenceConfigurations["half_sitting"].copy()
+    except:
+        print "loadReferenceConfigurations did not work. Please check your \
+            Pinocchio Version"
+        try:
+            pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
+            robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
+        except:
+            robot.q0.flat[:] = pinocchio.neutral(rmodel)
+    return
+
+
+def loadTalosArm():
+    URDF_FILENAME = "talos_left_arm.urdf"
+    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
+    SRDF_FILENAME = "talos.srdf"
+    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+    # Load URDF file
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
+
+    # Load SRDF file
+    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
+    return robot
+
+
+def loadTalos():
+    URDF_FILENAME = "talos_reduced.urdf"
+    SRDF_FILENAME = "talos.srdf"
+    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
+    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+    # Load URDF file
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
+    # Load SRDF file
+    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
+    assert((robot.model.armature[:6] == 0.).all())
+    return robot
+
+
+def loadHyQ():
+    URDF_FILENAME = "hyq_no_sensors.urdf"
+    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
+    # TODO define default position inside srdf
+    robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -
+                         0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
+    robot.q0[2] = 0.57750958
+    robot.model.referenceConfigurations["half_sitting"] = robot.q0
+    return robot