Make visualization time efficient

bindings/matlab/Visualization/prepareVisualization.m

@@ -1,4 +1,4 @@
-function [transforms,linkNames,map,linkMeshInfo,meshHandles,parent,mainHandler]=prepareVisualization(KinDynModel,meshFilePrefix)
+function [transforms,linkNames,linkNames_idyn,map,linkMeshInfo,meshHandles,parent,mainHandler]=prepareVisualization(KinDynModel,meshFilePrefix)
 % Creates the figures, loads the meshes, handles some visual effects, and creates the transform objects
 % - Iputs:
 %   - `KinDynModel` : iDyntreewrappers main variable. Contains the model.
@@ -7,6 +7,8 @@
 % - Outputs:
 %   - `transforms` : Is the transform objects array. There is one transform for each link
 %   - `linkNames`  : The name of the links in the same order of the transforms
+%   - `linkNames_iDyn`  : The name of the links in the same order of the
+%   transforms in a iDyntree string vector variable
 %   - `map`       : Cell array having both the names of the meshes and the associated link
 %   - `linkMeshInfo` : Contains the link name and a struct (meshInfo) that contains the name of file or if is a simple geometry, the triangulation ( edges and vertices of the mesh ) and the link to geometry transform.
 %   - `meshHandles` : Has the handles of the mesh objects.
@@ -30,10 +32,13 @@
 mainHandler=figure;
 parent=gca;
 hold on
+linkNames_idyn=iDynTree.StringVector();
+
 for it=1:numberOfLinks
     w_H_link=iDynTreeWrappers.getWorldTransform(KinDynModel,linkMeshInfo(it).linkName);
     [meshHandles(it,:),transforms(it)]=plotMeshInWorld(linkMeshInfo(it),w_H_link);
     linkNames{it}=linkMeshInfo(it).linkName;
+    linkNames_idyn.push_back(linkNames{it});
 end
 
 % Add a camera light, and tone down the specular highlighting

bindings/matlab/Visualization/testGetWorldTransformTimes.m

@@ -54,7 +54,7 @@
 getTHomo=toc;
 tic
 iter=T3_temp.begin;
-for c=1:length(linkNames)-1
+for c=1:length(linkNames)
     T3(c,:,:)=iter.next.toMatlab;   
 end
 homoTime=toc;

bindings/matlab/Visualization/updateVisualization.m

@@ -4,17 +4,25 @@
 % 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.
-%           - `linkNames`  : The `linkNames` variable output from the `prepareVisualization` function.
+%           - `linkNames`  : The `linkNames` variable output from the `prepareVisualization` function that contains the link names.
 %           - `transforms` : The `transforms` variable output from the `prepareVisualization` function.
-% 
+%
 % Author : Francisco Andrade (franciscojavier.andradechavez@iit.it)
 %
 % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
 % This software may be modified and distributed under the terms of the
 % GNU Lesser General Public License v2.1 or any later version.
 
-
-for it=1:length(linkNames)
-    w_H_link=iDynTreeWrappers.getWorldTransform(KinDynModel,linkNames{it});
-    transforms(it).Matrix=w_H_link;
+if iscellstr(linkNames) % sending an cell array with the link names
+    for it=1:length(linkNames)
+        w_H_link=iDynTreeWrappers.getWorldTransform(KinDynModel,linkNames{it});
+        transforms(it).Matrix=w_H_link;
+    end    
+else % expecting an iDyntree.StringVector. This is more time efficient
+    transforms_idyn=KinDynModel.kinDynComp.getWorldTransformsAsHomogeneous(linkNames);
+    iter=transforms_idyn.begin;
+    for it=1:length(KinDynModel.kinDynComp.getNrOfLinks)
+        w_H_link=iter.next.toMatlab;
+        transforms(it).Matrix=w_H_link;
+    end    
 end

bindings/matlab/Visualization/visualizeRobot.m

@@ -1,3 +1,7 @@
+% This script is an example on how to use the matlab Idyntree visualization
+
+% insert the path where the meshes may be found, it repaces what is require
+% d to complete the path inside the urdf visual mesh field.
 meshFilePrefix=[getenv('ROBOTOLOGY_SUPERBUILD_SOURCE_DIR') '/build-linux/install/share'];
 
 % create KinDynModel variable from another script
@@ -6,19 +10,47 @@
 % create vector of positions
 joints_positions=zeros(KinDynModel.NDOF,1);
 
-[transforms,linkNames,map,linkMeshInfo,meshHandles,parent,mainHandler]=prepareVisualization(KinDynModel,meshFilePrefix);
+% Prepare figure, handles and variables required for the update
+[transforms,linkNames,linkNames_idyn,map,linkMeshInfo,meshHandles,parent,mainHandler]=prepareVisualization(KinDynModel,meshFilePrefix);
+% Note: For default visualization these variables can be ignored:
+%       map,linkMeshInfo,meshHandles,parent,mainHandler.
 
 % generate random positions for the robot
 a = -pi/2;
 b = pi/2;
-randVector=(b-a).*rand(size(KinDynModel.NDOF))+ a;
+randVector=(b-a).*rand(KinDynModel.NDOF,1)+ a;
 
 % update only joints of arms and head to avoid ackward positions.
 joints_positions(16:end)=randVector(16:end);
 
 % update kinematics
 iDynTreeWrappers.setRobotState(KinDynModel,eye(4),joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]);
 
-updateVisualization(KinDynModel,linkNames,transforms);
-
-
+%%  Compare time efficiency of the update function
+% using iDyntree string vector
+tic
+ updateVisualization(KinDynModel,linkNames_idyn,transforms);
+stringVector_time=toc
+
+% using a string cell array with the link names
+tic
+ updateVisualization(KinDynModel,linkNames,transforms);
+cellArray_time=toc
+
+axis tight
+
+%% Try some custom changes
+toModify=meshHandles(1).modelMesh;
+% wireFrame style
+color=toModify.FaceColor;
+toModify.FaceColor='none';
+toModify.EdgeColor=color;
+faces=toModify.Faces;
+vertices=toModify.Vertices;
+reducepatch(toModify,0.005)
+
+% go back to normal
+toModify.Vertices=vertices;
+toModify.Faces=faces;
+toModify.FaceColor=color;
+toModify.EdgeColor='none';