Integrate new workflow

.travis.yml

@@ -0,0 +1,54 @@
+sudo: required
+dist: trusty
+language: cpp
+cache: ccache
+
+before_script:
+  # Start in the parent directory of icub-model-generator
+  - cd ..
+  - sudo apt-get install  --assume-yes --force-yes python-lxml python-yaml python-numpy python-setuptools
+  # probably python on the path return a python interpreter and the find_package(PythonInterp) in CMake another,
+  # let's install both debian packages and pip packages to be sure
+  - sudo pip install lxml numpy pyyaml
+  # install urdf_parser_py
+  - git clone https://github.com/ros/urdf_parser_py
+  - cd urdf_parser_py
+  - sudo python setup.py install
+  - cd ../
+  # install simmechanics-to-urdf
+  - git clone https://github.com/robotology/simmechanics-to-urdf
+  - cd simmechanics-to-urdf
+  - sudo python setup.py install
+  - cd ../
+  # use superbuild for getting icub-model-generator C++ dependencies
+  - git clone https://github.com/robotology/codyco-superbuild
+  - cd codyco-superbuild
+  # install dependencies using the codyco-superbuild script
+  - chmod +x ./.ci/travis-deps.sh
+  - sh .ci/travis-deps.sh
+  - mkdir build
+  - cd build
+  - export CMAKE_PREFIX_PATH=`pwd`/install
+  # using the YCM_EP_MAINTAINER_MODE variable to enable the subproject-dependees target
+  - cmake -DCODYCO_TRAVIS_CI:BOOL=ON  -DYCM_EP_MAINTAINER_MODE:BOOL=ON -DNON_INTERACTIVE_BUILD:BOOL=TRUE  ..
+  - cmake --build .
+  - pwd
+  - cd ../..
+  # Install sdformat
+  - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu trusty main" > /etc/apt/sources.list.d/gazebo-latest.list'
+  - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
+  - sudo apt-get update
+  - sudo apt-get install --assume-yes --force-yes libsdformat4-dev
+  # Prepare icub-model-generator build
+  - cd icub-model-generator
+  - mkdir build
+  - cd build
+  - cmake ..
+
+script:
+  - sudo cmake --build .
+  - ctest --output-on-failure
+
+notifications:
+  email:
+    - pegua1@gmail.com

CMakeLists.txt

@@ -1,3 +1,37 @@
-cmake_minimum_required(VERSION 2.6)
+# Copyright: (C) 2016 iCub Facility - Fondazione Istituto Italiano di Tecnologia
+# Authors: Silvio Traversaro
+# CopyPolicy: Released under the terms of the GNU LGPL v2+.
 
-add_subdirectory(src)
+cmake_minimum_required(VERSION 2.8.11)
+
+project(icub-model-generator)
+
+# Give error if add_dependencies is called on a non-existing target
+if(POLICY CMP0046)
+  cmake_policy(SET CMP0046 NEW)
+endif()
+
+option(ICUB_MODEL_GENERATE_DH "Generate models using the model generation pipeline from Denativ Hartenberg models" ON)
+option(ICUB_MODEL_GENERATE_SIMMECHANICS "Generate models using the model generation pipeline from simmechanics" ON)
+
+if( ICUB_MODEL_GENERATE_DH )
+  add_subdirectory(dh)
+endif()
+
+if( ICUB_MODEL_GENERATE_SIMMECHANICS )
+  add_subdirectory(simmechanics)
+endif()
+
+add_custom_target(generate-models ALL)
+
+if(TARGET generate-models-dh)
+  add_dependencies(generate-models generate-models-dh)
+endif()
+
+if(TARGET generate-models-simmechanics)
+  add_dependencies(generate-models generate-models-simmechanics)
+endif()
+
+include( CTest )
+enable_testing()
+add_subdirectory(tests)

README.md

@@ -1,38 +1,10 @@
-icub-model-generator
-==================
+# icub-model-generator
 
-This software repository is still in alpha stage, not ready for normal use. 
+Resources and programs to generated models (URDF,SDF) of the iCub robot. 
 
-Resources (./data/ folder) and programs (./src/) folder to generated models (URDF,SDF) of the iCub robot .
+There are currently two different pipelines to generated the models. 
 
-Installation
-------------
+The generation system from Denavit Hartenberg parametsrs based on data on robot models hardcoded in the iKin/iDyn source code and meshes postprocessed by ISIR at UPMC, Paris, available in `dh` directory.
 
-##### Dependencies
-- [YARP](https://github.com/robotology/yarp)
-- [ICUB](https://github.com/robotology/icub-main)
-- [iDynTree](https://github.com/robotology-playground/iDynTree)
-- [SDFormat](https://bitbucket.org/osrf/sdformat)
-
-The easiest way to install this repository with almost all its dependencies is to use the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). The SDFormat dependency is not 
-installed by the codyco-superbuild, but it should be already if you have installed the development files Gazebo (the
-one that you need to compile [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins).
-
-
-Example
--------
-
-### Model generation
-After compiling and installing this repository, you can run:
-```
-icub_urdf_sdf_generator --data_directory /path/to/codyco-superbuild/data/ --output_directory /path/to/icub-models/
-```
-Where the `/path/to/codyco-superbuild/data/ ` directory is the path to the data subdirectory of this repository. 
-`/path/to/icub-models/` is an arbitrary directory where you want to store the generated URDF and SDF models for the various iCubs (iCubGenova01, iCubGenova03, etc, etc).
-The models available in `/path/to/icub-models/icub-models/urdf` are the one extracted directly from the iDyn models. 
-
-### Use of generated models in Gazebo
-In the directory `/path/to/icub-models/icub-models/gazebo-models` you will find the SDF models generated for use with the Gazebo simulator, and the URDF models that match the kinematic and dynamic parameters of the SDF models. As the `icub_urdf_sdf_generator` generates also the proper configuration files to create a Gazebo model database, you can simply add `/path/to/icub-models/icub-models/gazebo-models` to the enviroment variable `GAZEBO_MODEL_PATH`.
-
-**Warning: currently to use the database of generated models of the iCub you should also install the [icub-gazebo](https://github.com/robotology-playground/icub-gazebo) repository, as the generated models take configuration files and meshes from the `iCub` model present in the `icub-gazebo` repository.**
+The generation system from actual CAD models converted to simmechanics file, described in http://wiki.icub.org/wiki/Creo_Mechanism_to_URDF , available in the `simmechanics` directory.
 

dh/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8.11)
+
+# Compile the actual dh --> URDF converter
+add_subdirectory(generator)
+
+
+# Generate URDF models for
+# v1/v2 robots using the simmechanics-to-urdf script
+macro(generate_icub_dh)
+    set(options)
+    set(oneValueArgs YARP_ROBOT_NAME)
+    set(multiValueArgs PARTS_VERSIONS)
+    cmake_parse_arguments(GIDH "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+
+    add_custom_command(OUTPUT ${GIDH_YARP_ROBOT_NAME}.urdf
+                       COMMAND icub_urdf_sdf_from_dh_generator
+                       ARGS ${GIDH_PARTS_VERSIONS}
+                            --outputfile ${GIDH_YARP_ROBOT_NAME}.urdf
+                            --YARP_ROBOT_NAME ${GIDH_YARP_ROBOT_NAME}
+                            --data_directory ${CMAKE_CURRENT_SOURCE_DIR}/data/
+                       MAIN_DEPENDENCY "${CMAKE_CURRENT_SOURCE_DIR}/data/urdf_paris/icub/icub.xml")
+
+
+    # instead of just copyng, we also modify the com on the fly
+    add_custom_command(OUTPUT "${CMAKE_SOURCE_DIR}/generated/robots/${GIDH_YARP_ROBOT_NAME}/model.urdf"
+                       MAIN_DEPENDENCY "${GIDH_YARP_ROBOT_NAME}.urdf"
+                       COMMAND ${CMAKE_COMMAND} -E copy "${GIDH_YARP_ROBOT_NAME}.urdf"
+                                                         "${CMAKE_SOURCE_DIR}/generated/robots/${GIDH_YARP_ROBOT_NAME}/model.urdf")
+
+
+    list(APPEND model-dh-generated-models "${CMAKE_SOURCE_DIR}/generated/robots/${GIDH_YARP_ROBOT_NAME}/model.urdf")
+endmacro()
+
+set(model-dh-generated-models "")
+generate_icub_dh(YARP_ROBOT_NAME iCubGenova03
+                 PARTS_VERSIONS --headV2 --armsV2 --legsV1 --feetV2)
+
+generate_icub_dh(YARP_ROBOT_NAME iCubParis01
+                 PARTS_VERSIONS --headV1 --armsV1 --legsV1 --feetV2)
+
+generate_icub_dh(YARP_ROBOT_NAME iCubParis02
+                 PARTS_VERSIONS --headV2 --armsV2 --legsV1 --feetV2 --iCubParis02)
+
+add_custom_target(generate-models-dh
+                  ALL
+                  DEPENDS ${model-dh-generated-models})
+
+
+
+

dh/README.md

@@ -0,0 +1,38 @@
+DH iCub Model Generator
+==================
+
+This software repository is still in alpha stage, not ready for normal use. 
+
+Resources (./data/ folder) and programs (./src/) folder to generated models (URDF,SDF) of the iCub robot from DH parameters.
+
+Installation
+------------
+
+##### Dependencies
+- [YARP](https://github.com/robotology/yarp)
+- [ICUB](https://github.com/robotology/icub-main)
+- [iDynTree](https://github.com/robotology-playground/iDynTree)
+- [SDFormat](https://bitbucket.org/osrf/sdformat)
+
+The easiest way to install this repository with almost all its dependencies is to use the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). The SDFormat dependency is not 
+installed by the codyco-superbuild, but it should be already if you have installed the development files Gazebo (the
+one that you need to compile [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins).
+
+
+Example
+-------
+
+### Model generation
+After compiling and installing this repository, you can run:
+```
+icub_urdf_sdf_generator --data_directory /path/to/codyco-superbuild/data/ --output_directory /path/to/icub-models/
+```
+Where the `/path/to/codyco-superbuild/data/ ` directory is the path to the data subdirectory of this repository. 
+`/path/to/icub-models/` is an arbitrary directory where you want to store the generated URDF and SDF models for the various iCubs (iCubGenova01, iCubGenova03, etc, etc).
+The models available in `/path/to/icub-models/icub-models/urdf` are the one extracted directly from the iDyn models. 
+
+### Use of generated models in Gazebo
+In the directory `/path/to/icub-models/icub-models/gazebo-models` you will find the SDF models generated for use with the Gazebo simulator, and the URDF models that match the kinematic and dynamic parameters of the SDF models. As the `icub_urdf_sdf_generator` generates also the proper configuration files to create a Gazebo model database, you can simply add `/path/to/icub-models/icub-models/gazebo-models` to the enviroment variable `GAZEBO_MODEL_PATH`.
+
+**Warning: currently to use the database of generated models of the iCub you should also install the [icub-gazebo](https://github.com/robotology-playground/icub-gazebo) repository, as the generated models take configuration files and meshes from the `iCub` model present in the `icub-gazebo` repository.**
+

dh/generator/CMakeLists.txt

@@ -0,0 +1,38 @@
+# Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia - iCub Facility
+# CopyPolicy: Released under the terms of the GNU GPL v2.0.
+
+find_package(YCM REQUIRED)
+find_package(YARP REQUIRED)
+find_package(ICUB REQUIRED)
+
+find_package(urdfdom REQUIRED)
+find_package(orocos_kdl REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(TinyXML REQUIRED)
+
+find_package(SDFormat REQUIRED)
+find_package(iDynTree 0.3.5 REQUIRED)
+
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")
+# Add Gazebo CXX flags, to support sdformat 3 reckless dependency on C++11
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SDFormat_CXX_FLAGS}")
+
+# add YARP include directories
+include_directories(${urdfdom_INCLUDE_DIR}
+                    ${orocos_kdl_INCLUDE_DIR}
+                    ${EIGEN3_INCLUDE_DIR} ${YARP_INCLUDE_DIR} ${iDynTree_INCLUDE_DIRS}
+                    ${iDyn_INCLUDE_DIRS}
+                    ${YARP_INCLUDE_DIRS}
+                    ${skinDynLib_INCLUDE_DIRS} ${SDFormat_INCLUDE_DIRS}
+                    ${urdfdom_INCLUDE_DIRS})
+
+add_library(urdf_from_dh_utils STATIC urdf_utils.h
+                                      urdf_utils.cpp
+                                      urdf_sdf_from_dh_utils.h
+                                      urdf_sdf_from_dh_utils.cpp)
+
+add_executable(icub_urdf_sdf_from_dh_generator icub_urdf_sdf_from_dh_generator.cpp)
+
+target_link_libraries(icub_urdf_sdf_from_dh_generator urdf_from_dh_utils ${SDFormat_LIBRARIES} ${iDynTree_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} ${kdl_codyco_LIBRARIES} ${orocos_kdl_LIBRARIES} ${urdfdom_LIBRARIES} ${iDynTree_LIBRARIES} ${TinyXML_LIBRARIES})
+
+install(TARGETS icub_urdf_sdf_from_dh_generator DESTINATION bin)

dh/generator/deprecated_icub_urdf_sdf_generator.cpp

@@ -0,0 +1,112 @@
+/**
+ * Copyright  (C)  2014 CoDyCo Project
+ * Author: Silvio Traversaro
+ * website: http://www.codyco.eu
+ */
+
+#include "urdf_sdf_from_dh_utils.h"
+
+void printTree(urdf::LinkSharedPtr link,int level = 0)
+{
+  level+=2;
+  int count = 0;
+  for (std::vector< urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+  {
+    if (*child)
+    {
+      for(int j=0;j<level;j++) std::cout << "  "; //indent
+      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << " with joint: " << (*child)->parent_joint->name << std::endl;
+      // first grandchild
+      printTree(*child,level);
+    }
+    else
+    {
+      for(int j=0;j<level;j++) std::cout << " "; //indent
+      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
+    }
+  }
+
+}
+
+void printJoints(boost::shared_ptr<urdf::ModelInterface> urdf)
+{
+    std::cout << "printing " << urdf->joints_.size() << " size" << std::endl;
+    for(std::map<std::string,boost::shared_ptr<urdf::Joint> >::const_iterator it = urdf->joints_.begin(); it != urdf->joints_.end(); it++ ) {
+        std::cout << it->first << " : " << it->second->name << std::endl;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    bool status = true;
+
+    yarp::os::Property opt;
+
+    opt.fromCommand(argc,argv);
+
+    if( opt.check("help") || !opt.check("output_directory") || !opt.check("data_directory") ) {
+        std::cerr << "Utility for generating URDF and SDF models for iCub robots" << std::endl;
+        std::cerr << "Creating one URDF with data from CAD, and a SDF directory with parameters modified to work in Gazebo (with a URDF that matches the modified simulated model)" << std::endl;
+        std::cerr << "Usage: \t icub_urdf_sdf_generator --data_directory ./data/ --output_directory ./icub-models/" << std::endl;
+        std::cerr << "Additional options: --min_mass and --min_inertia are used to select the minimum mass and minimum inertia to a give to a link in Gazebo model (default: mass 0.1, inertia: 0.01)" << std::endl;
+        return EXIT_FAILURE;
+    }
+
+    std::string output_directory = opt.find("output_directory").asString();
+    std::string data_directory = opt.find("data_directory").asString();
+
+    if( *(output_directory.rbegin()) != '/' ) {
+        output_directory = output_directory + "/";
+    }
+
+    if( *(data_directory.rbegin()) != '/' ) {
+        data_directory = data_directory + "/";
+    }
+
+    double mass_epsilon = 0.1;
+    double inertia_epsilon = 0.01;
+
+    if( opt.check("min_mass" )  ) mass_epsilon = opt.find("min_mass").asDouble();
+    if( opt.check("min_inertia" )  ) inertia_epsilon = opt.find("min_inertia").asDouble();
+
+    //Generate model database
+    std::vector<std::string> robot_names;
+
+    //Generating model for black iCub
+    //                          robot_name     directory    head   legs   feet    Paris02  gazeboSim
+    bool simple_meshes = false;
+    if( !generate_iCub_model("iCubGenova01",output_directory, 2    , 2    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("iCubGenova01");
+
+    //Generating model for red iCub
+    if( !generate_iCub_model("iCubGenova03",output_directory, 2    , 1    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("iCubGenova03");
+
+    if( !generate_iCub_model("iCubParis01",output_directory, 1    , 1    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("iCubParis01");
+
+    //Generating model for red iCub
+    if( !generate_iCub_model("iCubParis02",output_directory, 2    , 1    , 2   , true, false,  simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("iCubParis02");
+
+    //Generating model for Darmastad iCub
+    if( !generate_iCub_model("iCubDarmstadt01",output_directory, 2    , 2    , 2  , false, false,  simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("iCubDarmstad01");
+
+    //Generating model for icubGazeboSim
+    if( !generate_iCub_model("icubGazeboSim",output_directory, 2    , 1    , 2   , false, true, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
+    robot_names.push_back("icubGazeboSim");
+
+    //Generating model for icubGazeboSim
+    if( !generate_iCub_model("icubGazeboSimNoFT",output_directory, 2    , 1    , 2   , false, true, simple_meshes , data_directory,mass_epsilon,inertia_epsilon,true) ) return EXIT_FAILURE;
+    robot_names.push_back("icubGazeboSimNoFT");
+
+
+    std::cerr << "iCub model files successfully created" << std::endl;
+
+    std::cerr << "Generating gazebo database" << std::endl;
+    generate_gazebo_database(robot_names,output_directory);
+
+    return EXIT_SUCCESS;
+}
+