Automatize urdf model generation (#9)

ami-iit · Oct 27, 2023 · 3e205fc · 3e205fc
1 parent 3277608
commit 3e205fc
Show file tree

Hide file tree

Showing 25 changed files with 16,481 additions and 21 deletions.
diff --git a/.github/workflows/CI-iRonCub-models.yml b/.github/workflows/CI-iRonCub-models.yml
@@ -0,0 +1,89 @@
+name: Debug iRonCub Models
+
+on:
+  push:
+  pull_request:
+  schedule:
+    - cron: '0 3 * * 1'
+
+jobs:
+
+  build:
+    name: 'iRonCub Model Build'
+    runs-on: ubuntu-latest
+
+    steps:
+
+    - uses: actions/checkout@v3    
+    - uses: conda-incubator/setup-miniconda@v2
+      with:
+        miniforge-variant: Mambaforge
+        miniforge-version: latest
+        channels: conda-forge,robotology
+        channel-priority: true
+        environment-file: environment.yml
+
+    - name: Conda status
+      shell: bash -l {0}
+      run: |
+        conda info
+        conda list
+        conda config --show-sources
+        conda config --show
+        printenv | sort
+
+    - name: Setup git
+      shell: bash -l {0}
+      run: |
+        git config --global --add safe.directory /__w/component_ironcub/component_ironcub
+        git config --global user.name "GitHub Actions"
+        git config --global user.email "[email protected]"
+        git status
+
+    - name: Build models
+      shell: bash -l {0}
+      run: |
+        mkdir build
+        cd build
+        cmake .. -DENABLE_MK1_MODEL_GENERATION=ON -DENABLE_MK1_1_MODEL_GENERATION=ON
+        make
+
+    - name: Compare URDFs
+      shell: bash -l {0}
+      if: github.event_name != 'pull_request'
+      run: |
+        git status
+        if git diff-index --quiet HEAD; then
+            echo $'\nThe generated URDFs are equal\n'
+         else
+            echo "::warning::The generated URDFs are different"
+            git diff
+         fi
+
+    - name: Print robot mass
+      shell: bash -l {0}
+      run: |
+        find models -name "model.urdf" | while read -r URDF_FILE; do
+          echo ""
+          echo "Processing: $URDF_FILE"
+          idyntree-model-info -m "$URDF_FILE" --total-mass
+        done
+
+    - name: Push URDFs
+      shell: bash -l {0}
+      if: github.event_name == 'push' &&  github.ref != 'refs/heads/master' && contains(github.event.head_commit.message, '[urdf]')
+      run: |
+        cd models
+        git status
+        git add *.urdf
+        git pull
+        if git diff-index --quiet HEAD; then
+            echo "No URDF uploaded"
+         else
+            echo "New URDF uploaded"
+            # create commit message
+            git_hash=$(git rev-parse --short "$GITHUB_SHA")
+            echo "Automatic build of iRonCub_Model.urdf - Triggered by ${git_hash}" >> ${GITHUB_WORKSPACE}/deploy_commit_message
+            git commit -a -F ${GITHUB_WORKSPACE}/deploy_commit_message
+            git push
+         fi
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -36,6 +36,8 @@ elseif(UNIX)
 	set(SETUP_SCRIPT_EXTENSION ".sh")
 endif()
 
+option(ENABLE_MK1_MODEL_GENERATION "Re-Generate iRonCub-Mk1 urdf model." FALSE)
+option(ENABLE_MK1_1_MODEL_GENERATION "Re-Generate iRonCub-Mk1_1 urdf model." FALSE)
 option(INSTALL_IRONCUB_MODELS "Install iRonCub models in CMAKE_INSTALL_PREFIX." TRUE)
 option(INSTALL_LIBRARY "Install iRonCub gazebo plugin and simulink library." TRUE)
 option(USE_NONLINEAR_JET_DYNAMICS_PLUGIN "Install the nonlinear dynamics of the jet" FALSE)

diff --git a/README.md b/README.md
@@ -29,10 +29,8 @@ A quick way to install the dependencies is via [conda package manager](https://d
 Please refer to [the documentation in `robotology-superbuild`](https://github.com/robotology/robotology-superbuild/blob/7d79a44e90fbcedf137ab6c5c1d83b943d6e6839/doc/conda-forge.md) to install and configure a conda distribution. Then, once your environment is set, you can run the following command to install the required dependencies.
 
 ```sh
-mamba create -n <conda-environment-name>
+mamba env create -n <conda-environment-name> --file environment.yml
 mamba activate  <conda-environment-name>
-mamba install -c conda-forge compilers cmake pkg-config make ninja
-mamba install -c conda-forge -c robotology icub-models idyntree-matlab-bindings wb-toolbox whole-body-controllers whole-body-estimators gazebo gazebo-yarp-plugins opencv
 ```
 
 The installation procedure has been tested on Ubuntu 20.04 and Windows 10.
@@ -59,6 +57,10 @@ cmake .. -G"Visual Studio 16 2019" -DCMAKE_INSTALL_PREFIX="\path\to\desired\inst
 cmake --build . --config Release --target INSTALL
 ```
 
+> [!warning]
+> If you haven't configured [`git lfs`](https://git-lfs.com/), run `git lfs install` and `git lfs pull` inside the `ironcub_mk1_software` folder.
+
+
 ## Running the repo on Windows
 
 To run the repository on Windows it is necessary to properly setup a Windows terminal where commands are executed. First, create a file `setup.bat` with the following content:

diff --git a/environment.yml b/environment.yml
@@ -0,0 +1,25 @@
+name: ironcub_env
+channels:
+  - conda-forge
+  - robotology
+dependencies:
+  - git
+  - git-lfs
+  - compilers 
+  - cmake 
+  - pkg-config 
+  - make 
+  - ninja
+  - icub-models 
+  - idyntree-matlab-bindings
+  - wb-toolbox 
+  - whole-body-controllers 
+  - whole-body-estimators 
+  - gazebo 
+  - gazebo-yarp-plugins 
+  - opencv
+  - pyyaml=5
+  - catkin_pkg
+  - pip:
+    - git+https://github.com/ros/urdf_parser_py@31474b9baaf7c3845b40e5a9aa87d5900a2282c3
+    - git+https://github.com/robotology/[email protected]
diff --git a/models/CMakeLists.txt b/models/CMakeLists.txt
@@ -1,6 +1,4 @@
 # Copyright: (C) 2018 iCub Facility - Fondazione Istituto Italiano di Tecnologia
-# Authors: Luca Fiorio
 
 add_subdirectory(iRonCub-Mk1_v1)
 add_subdirectory(iRonCub-Mk1_1_v1)
-
diff --git a/models/iRonCub-Mk1_1_v1/CMakeLists.txt b/models/iRonCub-Mk1_1_v1/CMakeLists.txt
@@ -1,5 +1,20 @@
 # Copyright: (C) 2018 iCub Facility - Fondazione Istituto Italiano di Tecnologia
-# Authors: Luca Fiorio
 # CopyPolicy: Released under the terms of the GNU LGPL v2+.
 
+if(ENABLE_MK1_1_MODEL_GENERATION)
+	project(ironcub-model-generator)
+
+	# Give error if add_dependencies is called on a non-existing target
+	if(POLICY CMP0046)
+	  cmake_policy(SET CMP0046 NEW)
+	endif()
+
+	set(BUILD_PREFIX "models/iRonCub-Mk1_1_v1/iRonCub")
+	add_subdirectory(resources)
+
+	add_custom_target(generate-models-Mk1_1 ALL)
+
+	add_dependencies(generate-models-Mk1_1 generate-models-simmechanics-Mk1_1)
+endif()
+
 add_subdirectory(iRonCub)
diff --git a/models/iRonCub-Mk1_1_v1/iRonCub/CMakeLists.txt b/models/iRonCub-Mk1_1_v1/iRonCub/CMakeLists.txt
@@ -1,5 +1,4 @@
 # Copyright: (C) 2018 iCub Facility - Fondazione Istituto Italiano di Tecnologia
-# Authors: Luca Fiorio
 
 if(INSTALL_IRONCUB_MODELS)
 	set(robotname iRonCub-Mk1_1_v1)

diff --git a/models/iRonCub-Mk1_1_v1/resources/CMakeLists.txt b/models/iRonCub-Mk1_1_v1/resources/CMakeLists.txt
@@ -0,0 +1,197 @@
+find_package(PythonInterp REQUIRED)
+
+# Generate URDF models for
+# iRonCub robots using the simmechanics-to-urdf script
+macro(generate_iRonCub_simmechanics)
+    set(options INCREASE_INERTIA_FOR_GAZEBO REMOVE_FEET_FT_SENSORS)
+    set(oneValueArgs YARP_ROBOT_NAME SIMMECHANICS_XML YAML_TEMPLATE CSV_TEMPLATE MODEL_URDF_NAME MESH_FORMAT_EXTENSION)
+    set(multiValueArgs)
+    cmake_parse_arguments(GIVTWO "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    # Generate the YAML file from the CMake configuration
+    set(CHEST_ASSIGNED_MASS 11.145012)
+    if(GIVTWO_INCREASE_INERTIA_FOR_GAZEBO)
+      set(GAZEBO_ASSIGNED_INERTIAS
+"assignedInertias:
+  - linkName: r_hip_3
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: r_ankle_2
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_hip_3
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_ankle_2
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: r_shoulder_1
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: r_shoulder_2
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: r_shoulder_3
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: r_elbow_1
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_shoulder_1
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_shoulder_2
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_shoulder_3
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: l_elbow_1
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: neck_1
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+  - linkName: neck_2
+    xx: 0.01
+    yy: 0.01
+    zz: 0.01
+")
+      set(LOWERBODY_JOINT_DAMPING 1.0)
+      set(UPPERBODY_JOINT_DAMPING 1.0)
+      set(ASSIGNED_COLLISION_GEOMETRIES
+"assignedCollisionGeometry:
+  - linkName: r_foot
+    geometricShape:
+      shape: box
+      size: 0.16 0.072 0.001
+      origin: \"0.03 0.005 0.014 0.0 0.0 0.0\"
+  - linkName: l_foot
+    geometricShape:
+      shape: box
+      size: 0.16 0.072 0.001
+      origin: \"0.03 -0.005 0.014 0.0 0.0 0.0\"
+")
+    else()
+      set(GAZEBO_ASSIGNED_INERTIAS "")
+      set(LOWERBODY_JOINT_DAMPING 0.223)
+      set(UPPERBODY_JOINT_DAMPING 0.06)
+      set(ASSIGNED_COLLISION_GEOMETRIES "")
+    endif()
+
+    set(MESH_FILE_FORMAT "filenameformatchangeext: \"package://iRonCub-Mk1_1_v1/iRonCub/meshes/${GIVTWO_MESH_FORMAT_EXTENSION}/%s.${GIVTWO_MESH_FORMAT_EXTENSION}\"")
+    set(CONF_FILE_DIR "iRonCub-Mk1_1_v1/iRonCub/conf")
+    set(ANKLE_PITCH_ROM "-35,35")
+    set(REVERSE_ROTATION_AXIS
+"reverseRotationAxis:
+  l_shoulder_roll
+  l_elbow
+  l_hip_yaw
+  l_knee
+  l_ankle_pitch
+  r_hip_pitch
+  r_ankle_roll
+  r_shoulder_pitch
+  r_shoulder_yaw
+  torso_pitch
+  neck_roll
+") 
+
+
+  if(GIVTWO_REMOVE_FEET_FT_SENSORS)
+    set(FEET_FT_SENSORS "")
+    SET(STR_FEET_FT_SENSORS "no-feet-ft")
+  else()
+    set(FEET_FT_SENSORS 
+    "  - jointName: l_foot_ft_sensor
+    directionChildToParent: Yes
+    sensorBlobs:
+    - |
+        <plugin name=\"left_foot_ft_plugin\" filename=\"libgazebo_yarp_forcetorque.so\">
+          <yarpConfigurationFile>model://${CONF_FILE_DIR}/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
+        </plugin>
+  - jointName: r_foot_ft_sensor
+    directionChildToParent: Yes
+    sensorBlobs:
+    - |
+        <plugin name=\"right_foot_ft_plugin\" filename=\"libgazebo_yarp_forcetorque.so\">
+          <yarpConfigurationFile>model://${CONF_FILE_DIR}/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
+        </plugin>")
+    SET(STR_FEET_FT_SENSORS "feetFt")
+  endif()
+
+    set(GENERATED_YAML_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${GIVTWO_YARP_ROBOT_NAME}_${GIVTWO_MESH_FORMAT_EXTENSION}_${STR_FEET_FT_SENSORS}.yaml)
+    configure_file(${CMAKE_CURRENT_SOURCE_DIR}/data/${GIVTWO_YAML_TEMPLATE}
+                   ${GENERATED_YAML_LOCATION}
+                   @ONLY)
+    set(GENERATED_CSV_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${GIVTWO_YARP_ROBOT_NAME}.csv)
+    configure_file(${CMAKE_CURRENT_SOURCE_DIR}/data/${GIVTWO_CSV_TEMPLATE}
+                   ${GENERATED_CSV_LOCATION}
+                   @ONLY)
+
+    add_custom_command(OUTPUT ${GIVTWO_YARP_ROBOT_NAME}_${GIVTWO_MESH_FORMAT_EXTENSION}_${STR_FEET_FT_SENSORS}.urdf
+                       COMMAND simmechanics_to_urdf
+                       ARGS ${CMAKE_CURRENT_SOURCE_DIR}/data/${GIVTWO_SIMMECHANICS_XML}
+                            --output xml
+                            --yaml ${GENERATED_YAML_LOCATION}
+                            --csv-joints ${GENERATED_CSV_LOCATION}
+                            --outputfile ${GIVTWO_YARP_ROBOT_NAME}_${GIVTWO_MESH_FORMAT_EXTENSION}_${STR_FEET_FT_SENSORS}.urdf
+                       MAIN_DEPENDENCY "${CMAKE_CURRENT_SOURCE_DIR}/data/${GIVTWO_SIMMECHANICS_XML}"
+                       DEPENDS  "${GENERATED_YAML_LOCATION}"
+                                "${GENERATED_CSV_LOCATION}")
+    add_custom_command(OUTPUT "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/${GIVTWO_MODEL_URDF_NAME}"
+                           MAIN_DEPENDENCY "${GIVTWO_YARP_ROBOT_NAME}_${GIVTWO_MESH_FORMAT_EXTENSION}_${STR_FEET_FT_SENSORS}.urdf"
+                           COMMAND ${CMAKE_COMMAND} -E
+                                   copy "${GIVTWO_YARP_ROBOT_NAME}_${GIVTWO_MESH_FORMAT_EXTENSION}_${STR_FEET_FT_SENSORS}.urdf" "${CMAKE_BINARY_DIR}/../${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/${GIVTWO_MODEL_URDF_NAME}")
+
+    list(APPEND model-simmechanics-generated-models "${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${GIVTWO_YARP_ROBOT_NAME}/${GIVTWO_MODEL_URDF_NAME}")
+endmacro()
+
+set(model-simmechanics-generated-models "")
+
+generate_iRonCub_simmechanics(YARP_ROBOT_NAME iRonCub-Mk1_1_Gazebo_v1
+                              SIMMECHANICS_XML "IRONCUB-MK1_1_SIM_MODEL.xml"
+                              YAML_TEMPLATE "IRONCUB-MK1_1_simmechanics_options.yaml.in"
+                              CSV_TEMPLATE "IRONCUB-MK1_1_joint_parameters.csv.in"
+                              MODEL_URDF_NAME "model.urdf"
+                              MESH_FORMAT_EXTENSION "obj"
+                              INCREASE_INERTIA_FOR_GAZEBO)
+
+generate_iRonCub_simmechanics(YARP_ROBOT_NAME iRonCub-Mk1_1_Gazebo_v1
+                              SIMMECHANICS_XML "IRONCUB-MK1_1_SIM_MODEL.xml"
+                              YAML_TEMPLATE "IRONCUB-MK1_1_simmechanics_options.yaml.in"
+                              CSV_TEMPLATE "IRONCUB-MK1_1_joint_parameters.csv.in"
+                              MODEL_URDF_NAME "model_stl.urdf"
+                              MESH_FORMAT_EXTENSION "stl"
+                              INCREASE_INERTIA_FOR_GAZEBO)
+
+generate_iRonCub_simmechanics(YARP_ROBOT_NAME iRonCub-Mk1_1_v1
+                              SIMMECHANICS_XML "IRONCUB-MK1_1_SIM_MODEL.xml"
+                              YAML_TEMPLATE "IRONCUB-MK1_1_simmechanics_options.yaml.in"
+                              CSV_TEMPLATE "IRONCUB-MK1_1_joint_parameters.csv.in"
+                              MODEL_URDF_NAME "model.urdf"
+                              MESH_FORMAT_EXTENSION "obj")
+
+generate_iRonCub_simmechanics(YARP_ROBOT_NAME iRonCub-Mk1_1_v1
+                              SIMMECHANICS_XML "IRONCUB-MK1_1_SIM_MODEL.xml"
+                              YAML_TEMPLATE "IRONCUB-MK1_1_simmechanics_options.yaml.in"
+                              CSV_TEMPLATE "IRONCUB-MK1_1_joint_parameters.csv.in"
+                              MODEL_URDF_NAME "model_stl.urdf"
+                              MESH_FORMAT_EXTENSION "stl")
+
+add_custom_target(generate-models-simmechanics-Mk1_1
+                  ALL
+                  DEPENDS ${model-simmechanics-generated-models})