Port a Simulink model from Whole-Body Toolbox v4 to v5

WBT4_to_WBT5.sh

#!/bin/bash
set -e
set -u

Color_Off='\e[0m'
BRed='\e[1;31m'
BBlue='\e[1;34m'
BGreen='\e[1;32m'

function msg()
{
    echo -e "$BGreen==>$Color_Off $1"
}

function msg2()
{
    echo -e "  $BBlue->$Color_Off $1"
}

function err()
{
    echo -e "$BRed==>$Color_Off $1"
}

function err_exit()
{
    err "$1" >&2
    exit 1
}

function print_help()
{
    echo "Usage: $0 [OPTIONS] ... [COMMAND]"
    echo
    echo "Helper script for migrating a Simulink controller from WBT v4 to WBT v5."
    echo
    echo "Mandatory arguments:"
    echo "  -i    Simulink model to migrate (.mdl file)"
    echo "  -o    Output mdl file"
    echo
    echo "Example:"
    echo "$0 -i model_WBT4.mdl -o model_WBT5.mdl"
    echo
    echo "Diego Ferigo: <diego.ferigo@iit.it>"
    echo "Dynamic Interaction Control - Italian Institute of Technology"
}

while getopts :o:i: OPT ; do
    case $OPT in
        i) INPUT_FILE=$OPTARG ;;
        o) OUTPUT_FILE=$OPTARG ;;
        \?)
            print_help
            exit 1
        ;;
    esac
done

if [ -z ${INPUT_FILE:-} ] ; then
    err "Input file not passed"
    echo
    print_help
    exit 1
fi

if [ -z ${OUTPUT_FILE:-} ] ; then
    err_exit "Output file not passed"
fi

if [ ${INPUT_FILE##*.} != "mdl" ] ; then
    err "The input file must have the mdl extension"
    echo
    print_help
    exit 1
fi

if [ ${OUTPUT_FILE##*.} != "mdl" ] ; then
    err "The output file must have the mdl extension"
    echo
    print_help
    exit 1
fi

msg "Processing ${INPUT_FILE##*/} model"
msg2 "Creating copy of the input mdl"

if [ -f ${OUTPUT_FILE} ] ; then
    err_exit "${OUTPUT_FILE##*/} already existys. Aborting."
fi

cp ${INPUT_FILE} ${OUTPUT_FILE} || err_exit "Failed to create ${OUTPUT_FILE}"

for pattern in wholeBodyStates wholeBodyModel wholeBodyActuators ; do
    msg2 "Processing $pattern blocks"
    sed -i "s/${pattern}/${pattern#wholeBody}/g" ${OUTPUT_FILE}
done

function trim_spaces()
{
    stringTrimmed="${1//[[:space:]]/}"
    sed -i "s/$1/$stringTrimmed/g" ${OUTPUT_FILE}
}

msg2 "Renaming the library name to WholeBodyToolbox"
trim_spaces "Whole Body Toolbox"

msg2 "Renaming blocks to remove whitespaces"
trim_spaces "Get Measurement"
trim_spaces "Get Motor Measurement"
trim_spaces "Get Limits"
trim_spaces "Forward Kinematics"
trim_spaces "Remote Inverse Kinematics"
trim_spaces "Inverse Kinematics"
trim_spaces "Relative Transform"
trim_spaces "DotJ Nu"
trim_spaces "Mass Matrix"
trim_spaces "Centroidal Momentum"
trim_spaces "Inverse Dynamics"
trim_spaces "Get Bias Forces"
trim_spaces "Get Gravity Forces"
trim_spaces "Set References"
trim_spaces "Set Motor References"
trim_spaces "Set Motor Parameters"
trim_spaces "Yarp Read"
trim_spaces "Yarp Write"
trim_spaces "Minimum Jerk Trajectory Generator"
trim_spaces "Discrete Filter"
trim_spaces "Match Signal Sizes"

msg "The model compatible with WBT v5 has been created in ${OUTPUT_FILE}"
msg "Success"