working inverse dynamics and updated main and integration and result plotting