function []=updateVisualization(KinDynModel,Visualizer)
% We use the transform handlers to update the robot image using the
% iDyntree kinematics
% Updates the figure image with the new robot state. To be launched after `setRobotState` has been used.
%     - Inputs:
%           - `KinDynModel`: iDyntreewrappers main variable. Contains the model.
%           - `visualizer` : variable output from the `prepareVisualization` function. It contains the relevant variables linkNames,transforms and NOBJ
%               - `linkNames`  : variable that contains the link names.
%               - `transforms` : variable that contains transform objects that are parent of the meshes.
%               - `NOBJ` : variable that contains the number of visual objects to update.
%
% Author : Francisco Andrade (franciscojavier.andradechavez@iit.it)
%
% SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
% SPDX-License-Identifier: BSD-3-Clause

transforms_idyn=KinDynModel.kinDynComp.getWorldTransformsAsHomogeneous(Visualizer.linkNames_idyn);
w_H_links= transforms_idyn.toMatlab();
for it=1:Visualizer.NOBJ
    Visualizer.transforms(it).Matrix=squeeze(w_H_links(it,:,:));
end
