print slightly modified
added a simulink diagram for testing the invDyn function