robotology · traversaro · Apr 20, 2020 · Apr 9, 2020 · Apr 9, 2020
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,31 +8,34 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ## [Unreleased]
 
 ### Added
+- Added a new function to `iDynTreeWrappers` for the function `getWorldTransformsAsHomogeneous`.
+- Added functions for having a MATLAB iDynTree Visualizer in `iDynTreeWrappers`. Some time optimization has been performed (https://github.com/robotology/idyntree/issues/659).
+- Added `bindings` for `getWorldTransformsAsHomogeneous` function.
+- Added function `getWorldTransformsAsHomogeneous` that gives a vector of Matrix4x4 based on a vector of strings containing the frame transforms.
 - Added `bindings` for handling `linkSolidShapes` properly (https://github.com/robotology/idyntree/issues/656).
 - Added `bindings` for `InverseKinematics` (https://github.com/robotology/idyntree/pull/633).
 - Implement `cbegin()` / `cend()` and `begin()` / `end()` methods for `VectorDynSize` and `VectorFixSize` (https://github.com/robotology/idyntree/pull/646).
 
 ## [1.0.5] - 2020-04-03
 
-### Fixed 
-- Fix find_package(iDynTree) when iDynTree is built with IDYNTREE_USES_ASSIMP ON and BUILD_SHARED_LIBS OFF (https://github.com/robotology/idyntree/pull/667). 
-
+### Fixed
+- Fix find_package(iDynTree) when iDynTree is built with IDYNTREE_USES_ASSIMP ON and BUILD_SHARED_LIBS OFF (https://github.com/robotology/idyntree/pull/667).
 
 ## [1.0.4] - 2020-04-02
 
-### Fixed 
-- Further fix for configuration compilation with Assimp >= 5.0.0 (https://github.com/robotology/idyntree/pull/666). 
+### Fixed
+- Further fix for configuration compilation with Assimp >= 5.0.0 (https://github.com/robotology/idyntree/pull/666).
 
 ## [1.0.3] - 2020-04-01
 
-### Fixed 
-- Fixed configuration and compilation with Assimp >= 5.0.0 (https://github.com/robotology/idyntree/pull/661). 
+### Fixed
+- Fixed configuration and compilation with Assimp >= 5.0.0 (https://github.com/robotology/idyntree/pull/661).
 - Fixed runtime errors of the MATLAB bindings on Windows and compatibility with MATLAB 2020a (https://github.com/robotology/idyntree/pull/664).
 
 ## [1.0.2] - 2020-02-21
 
 ### Fixed
-- Remove spurious inclusion of Eigen headers in ExtendedKalmanFilter.h public header, that could create probles when using that header in a downstream project that does not use Eigen (https://github.com/robotology/idyntree/pull/639).
+- Remove spurious inclusion of Eigen headers in ExtendedKalmanFilter.h public header, that could create problems when using that header in a downstream project that does not use Eigen (https://github.com/robotology/idyntree/pull/639).
 - Added find_dependency(OsqpEigen) and find_dependency(LibXml2) when iDynTree is compiled as a static library, fixing the use of iDynTree on Windows (https://github.com/robotology/idyntree/pull/642).
 
 ### Changed

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,6 +1,6 @@
 # Copyright (C) 2013-2018 Fondazione Istituto Italiano di Tecnologia
-# 
-# Licensed under either the GNU Lesser General Public License v3.0 : 
+#
+# Licensed under either the GNU Lesser General Public License v3.0 :
 # https://www.gnu.org/licenses/lgpl-3.0.html
 # or the GNU Lesser General Public License v2.1 :
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
@@ -40,7 +40,7 @@ if(NOT IDYNTREE_ONLY_DOCS)
 
     # Add external libraries that are embedded in iDynTree
     # source tree, if necessary (by default does not adds
-    # anything) feel free to check teh logic inside
+    # anything) feel free to check the logic inside
     add_subdirectory(extern)
 
     include(AddInstallRPATHSupport)

diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i
@@ -8,6 +8,9 @@
 // Wrap the std::vector<std::string> params
 %template(StringVector) std::vector<std::string>;
 
+// Wrap the std::vector<std::int> params
+%template(IntVector) std::vector<int>;
+
 // Ignore some methods to avoid warnings
 %include "./ignore.i"
 
@@ -302,8 +305,8 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 
 %include "joints.i"
 
-%template(SolidshapesVector) std::vector<iDynTree::SolidShape*>;
-%template(linksSolidshapesVector) std::vector< std::vector<iDynTree::SolidShape *>>;
+%template(SolidShapesVector) std::vector<iDynTree::SolidShape*>;
+%template(LinksSolidShapesVector) std::vector< std::vector<iDynTree::SolidShape *>>;
 
 
 // Kinematics & Dynamics related functions
@@ -350,6 +353,13 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 // High level interfaces
 %include "iDynTree/KinDynComputations.h"
 
+#ifdef SWIGMATLAB
+%include "./matlab/matlab_mat4x4vec.i"
+#endif
+
+%template(Matrix4x4Vector) std::vector<iDynTree::Matrix4x4>;
+
+
 // Visualization
 %include "iDynTree/Visualizer.h"
 

diff --git a/bindings/matlab/+iDynTreeWrappers/README.md b/bindings/matlab/+iDynTreeWrappers/README.md
@@ -22,9 +22,9 @@ For a Matlab/Octave user, it may be sometimes counterintuitive to use C++ based
     baseOrigin   = baseOrigin_iDyntree.toMatlab;
     jointPos     = jointPos_iDyntree.toMatlab;
     baseVel      = baseVel_iDyntree.toMatlab;
-    jointVel     = jointVel_iDyntree.toMatlab; 
+    jointVel     = jointVel_iDyntree.toMatlab;
     basePose     = [baseRotation, baseOrigin;
-                       0,  0,  0,  1]; 
+                       0,  0,  0,  1];
 ```
 
 The purpose of the wrapper is therefore to provide a simpler and easy-to-use interface for Matlab/Octave users who wants to use iDyntree inside Matlab/Octave, also helping in designing code which is less error-prone and easier to debug (e.g. in case the interface of an iDyntree function will change in the future).
@@ -42,30 +42,31 @@ The purpose of the wrapper is therefore to provide a simpler and easy-to-use int
 - [getCentroidalTotalMomentum](getCentroidalTotalMomentum.m)
 - [getRobotState](getRobotState.m)
 - [getWorldBaseTransform](getWorldBaseTransform.m)
-- [getBaseTwist](getBaseTwist.m) 
-- [getModelVel](getModelVel.m) 
+- [getBaseTwist](getBaseTwist.m)
+- [getModelVel](getModelVel.m)
 - [getFrameVelocityRepresentation](getFrameVelocityRepresentation.m)
 - [getNrOfDegreesOfFreedom](getNrOfDegreesOfFreedom.m)
 - [getFloatingBase](getFloatingBase.m)
 - [getFrameIndex](getFrameIndex.m)
 - [getFrameName](getFrameName.m)
-- [getWorldTransform](getWorldTransform.m) 
-- [getRelativeTransform](getRelativeTransform.m) 
-- [getRelativeJacobian](getRelativeJacobian.m) 
+- [getWorldTransform](getWorldTransform.m)
+- [getWorldTransformsAsHomogeneous](getWorldTransformsAsHomogeneous.m)
+- [getRelativeTransform](getRelativeTransform.m)
+- [getRelativeJacobian](getRelativeJacobian.m)
 - [getFreeFloatingMassMatrix](getFreeFloatingMassMatrix.m)
 - [getFrameBiasAcc](getFrameBiasAcc.m)
 - [getFrameFreeFloatingJacobian](getFrameFreeFloatingJacobian.m)
-- [getCenterOfMassPosition](getCenterOfMassPosition.m) 
-- [generalizedBiasForces](generalizedBiasForces.m) 
+- [getCenterOfMassPosition](getCenterOfMassPosition.m)
+- [generalizedBiasForces](generalizedBiasForces.m)
 - [generalizedGravityForces](generalizedGravityForces.m)
-- [getCenterOfMassJacobian](getCenterOfMassJacobian.m) 
-- [getCenterOfMassVelocity](getCenterOfMassVelocity.m) 
- 
+- [getCenterOfMassJacobian](getCenterOfMassJacobian.m)
+- [getCenterOfMassVelocity](getCenterOfMassVelocity.m)
+
 ### Set the model-related quantities
- 
-- [setJointPos](setJointPos.m) 
+
+- [setJointPos](setJointPos.m)
 - [setFrameVelocityRepresentation](setFrameVelocityRepresentation.m)
-- [setFloatingBase](setFloatingBase.m) 
+- [setFloatingBase](setFloatingBase.m)
 - [setRobotState](setRobotState.m)
 
 ## Visualizer class
@@ -75,3 +76,15 @@ Not proper wrappers, they wrap more than one method of the class each. **Require
 - [initializeVisualizer](initializeVisualizer.m)
 - [visualizerSetup](visualizerSetup.m)
 - [updateVisualizer](updateVisualizer.m)
+
+## Matlab Native visualization
+
+Not actual wrappers, they use the iDynTreeWrappers in combination with the MATLAB patch plotting functions to visualize the robot.
+**Disclaimers**:
+- This visualization **has not been tested** with Octave.
+- At the moment, there is **no support** for .dae mesh files.
+
+- [prepareVisualization](prepareVisualization.m)
+- [updateVisualization](updateVisualization.m)
+- [getMeshes](getMeshes.m)
+- [plotMeshInWorld](plotMeshInWorld.m)
diff --git a/bindings/matlab/+iDynTreeWrappers/getMeshes.m b/bindings/matlab/+iDynTreeWrappers/getMeshes.m
@@ -0,0 +1,118 @@
+function [linkMeshInfo,map]=getMeshes(model,meshFilePrefix)
+% We use the iDyntree information to obtain the files containing the meshes
+% and we link them to their link names
+% Gets the mesh information for each link in the model.
+%     - Iputs:
+%         - `model` : iDyntree model loaded from a URDF.
+%         - `meshFilePrefix` : Path in which we can find the meshes. As an example the path to the mesh in a iCub urdf is `'package://iCub/meshes/simmechanics/sim_sea_2-5_root_link_prt-binary.stl'
+% `. `meshFilePrefix` should replace package to allow to find the rest of the path.
+%     - Outputs:
+%         - `map`        : Cell array having both the names of the meshes and the associated link
+%         - `linkMeshInfo` : Struct array that contain 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.
+%
+% NOTE: at the moment only STL files are supported.
+%
+% 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.
+
+
+
+% get the linkSolidShapes containing the mesh information
+visual=model.visualSolidShapes;
+linkSolidShapesV=visual.linkSolidShapes;
+% get number of links
+numberOfLinks=linkSolidShapesV.size;
+linkMeshInfo=struct('meshInfo',{},'linkName',{});
+% get pointer to beggining of vector
+iterator=linkSolidShapesV.begin;
+% iterate getting the name of the mesh and the name of the link
+count=1;
+link_with_no_visual=[];
+for links=1:numberOfLinks    
+    linkName=model.getLinkName(links-1);
+    linkMeshInfo(links).linkName=linkName;
+    solidarray=iterator.next;
+    solids_number=size(solidarray,2);    
+    meshInfo=struct('meshFile',{},'mesh_triangles',{},'link_H_geom',{});
+    for solids=1:solids_number
+        if solidarray{solids}.isExternalMesh
+            externalMesh=solidarray{solids}.asExternalMesh;
+            scale=externalMesh.scale.toMatlab;
+            meshName=split(externalMesh.filename,':');
+            meshFile=meshName{2};
+            % Import an STL mesh, returning a PATCH-compatible face-vertex structure
+            if strcmp('package',meshName{1})
+                mesh_triangles = stlread([meshFilePrefix meshFile]);
+            else
+                mesh_triangles = stlread(meshFile);
+            end
+            meshInfo(solids).meshFile=meshFile;
+            meshInfo(solids).scale=scale';
+        else
+            meshInfo(solids).scale=[1,1,1];  
+            if solidarray{solids}.isCylinder
+                meshInfo(solids).meshFile='cylinder';
+                length=solidarray{solids}.asCylinder.length;
+                radius=solidarray{solids}.asCylinder.radius;
+                mesh_triangles=calculateMeshFromCylinder(length,radius);
+            end
+            if solidarray{solids}.isBox
+                meshInfo(solids).meshFile='box';
+                box_dimensions(1)=solidarray{solids}.asBox.x;
+                box_dimensions(2)=solidarray{solids}.asBox.y;
+                box_dimensions(3)=solidarray{solids}.asBox.z;
+                mesh_triangles=calculateMeshFromBox(box_dimensions);
+            end
+            if solidarray{solids}.isSphere
+                meshInfo(solids).meshFile='sphere';                
+                radius=solidarray{solids}.asSphere.radius;
+                mesh_triangles=calculateMeshFromSphere(radius);
+            end
+        end
+        link_H_geom=solidarray{solids}.link_H_geometry.asHomogeneousTransform.toMatlab;
+        meshInfo(solids).link_H_geom=link_H_geom;
+        meshInfo(solids).mesh_triangles=mesh_triangles;              
+        map(count,:)=[{meshFile},{linkName}];
+        count=count+1;
+    end
+    linkMeshInfo(links).meshInfo=meshInfo;
+    if solids_number==0
+       link_with_no_visual=[ link_with_no_visual links];
+    end
+end
+% clean links with no visuals
+linkMeshInfo(link_with_no_visual)=[];
+
+end
+%% Create points and connectivity list for each geometry
+
+function [mesh_triangles]=calculateMeshFromSphere(radius)
+[X,Y,Z]=sphere;
+X = X * radius;
+Y = Y * radius;
+Z = Z * radius;
+[F,V]=mesh2tri(X,Y,Z,'f');
+mesh_triangles.Points=V;
+mesh_triangles.ConnectivityList = F;
+end
+
+function [mesh_triangles]=calculateMeshFromBox(box_dimensions)
+mesh_triangles.Points = [0 0 0;1 0 0;1 1 0;0 1 0;0 0 1;1 0 1;1 1 1;0 1 1];
+mesh_triangles.Points(:,1)=mesh_triangles.Points(:,1)*box_dimensions(1)-box_dimensions(1)/2;
+mesh_triangles.Points(:,2)=mesh_triangles.Points(:,2)*box_dimensions(2)-box_dimensions(2)/2;
+mesh_triangles.Points(:,3)=mesh_triangles.Points(:,3)*box_dimensions(3)-box_dimensions(3)/2;
+mesh_triangles.ConnectivityList = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
+end
+
+function [mesh_triangles]=calculateMeshFromCylinder(length,radius)
+[X,Y,Z] = cylinder;
+X = X * radius;
+Y = Y * radius;
+Z = Z * length-length/2;
+[F,V]=mesh2tri(X,Y,Z,'f');
+mesh_triangles.Points=V;
+mesh_triangles.ConnectivityList = F;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m
@@ -14,27 +14,23 @@
     %
     % OUTPUTS: - w_H_frame: [4 x 4] from frame to world transformation matrix.
     %
-    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    % Author : Gabriele Nava (gabriele.nava@iit.it), 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.
 
     %% ------------Initialization----------------
 
-    % get the transformation between the frame and the world 
-    w_H_frame_iDyntree = KinDynModel.kinDynComp.getWorldTransform(frameName);  
-    w_R_frame_iDyntree = w_H_frame_iDyntree.getRotation;
-    framePos_iDyntree  = w_H_frame_iDyntree.getPosition;
-
-    % covert to Matlab format
-    w_R_frame          = w_R_frame_iDyntree.toMatlab;
-    framePos           = framePos_iDyntree.toMatlab;
-    w_H_frame          = [w_R_frame, framePos;
-                           0,   0,   0,   1];                     
+    % get the transformation between the frame and the world in Matlab
+    % format
+    w_H_frame = KinDynModel.kinDynComp.getWorldTransform(frameName).asHomogeneousTransform.toMatlab;  
+
     % Debug output
     if KinDynModel.DEBUG
-
+        w_R_frame          = w_H_frame(1:3,1:3);
+
         disp('[getWorldTransform]: debugging outputs...')
 
          % w_R_frame must be a valid rotation matrix

diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldTransformsAsHomogeneous.m b/bindings/matlab/+iDynTreeWrappers/getWorldTransformsAsHomogeneous.m
@@ -0,0 +1,69 @@
+function w_H_frames = getWorldTransformsAsHomogeneous(KinDynModel,frameNames)
+
+% GETWORLDTRANSFORMASHOMOGENEOUS gets the transformation matrices between the specified
+%                        frames and the inertial reference frame.
+%
+% This matlab function wraps a functionality of the iDyntree library.
+% For further info see also: https://github.com/robotology/idyntree
+%
+% FORMAT:  w_H_frames = getWorldTransformsAsHomogeneous(KinDynModel,frameName)
+%
+% INPUTS:  - frameNames: a string vector that specifies the frames w.r.t. compute the
+%                       transfomation matrix;
+%          - KinDynModel: a structure containing the loaded model and additional info.
+%
+% OUTPUTS: - w_H_frames: [size(frameNames) x 4 x 4] from frame to world transformation matrices.
+%
+% 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.
+
+%% ------------Initialization----------------
+
+% get the transformation between the frame and the world in Matlab
+% format
+transforms_idyn=KinDynModel.kinDynComp.getWorldTransformsAsHomogeneous(frameNames);
+w_H_frames= transforms_idyn.toMatlab();
+for it=1:frameNames.size
+    w_H_frame=squeeze(w_H_frames(it,:,:));
+
+    % Debug output
+    if KinDynModel.DEBUG
+        w_R_frame          = w_H_frame(1:3,1:3);
+
+        disp('[getWorldTransform]: debugging outputs...')
+
+        % w_R_frame must be a valid rotation matrix
+        if det(w_R_frame) < 0.9 || det(w_R_frame) > 1.1
+
+            error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+        end
+
+        IdentityMatr = w_H_frame(1:3,1:3)*w_H_frame(1:3,1:3)';
+
+        for kk = 1:size(IdentityMatr, 1)
+
+            for jj = 1:size(IdentityMatr, 1)
+
+                if jj == kk
+
+                    if abs(IdentityMatr(kk,jj)-1) > 0.9
+
+                        error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.1
+
+                        error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+                    end
+                end
+            end
+        end
+        disp('[getWorldTransform]: done.')
+    end
+
+end
+
+end