Add setWorldBaseTransform function to KinDynComputations and high-level Matlab wrappers

bindings/matlab/+iDynTreeWrappers/README.md

@@ -65,6 +65,7 @@ The purpose of the wrapper is therefore to provide a simpler and easy-to-use int
 ### Set the model-related quantities
 
 - [setJointPos](setJointPos.m)
+- [setWorldBaseTransform](setWorldBaseTransform.m)
 - [setFrameVelocityRepresentation](setFrameVelocityRepresentation.m)
 - [setFloatingBase](setFloatingBase.m)
 - [setRobotState](setRobotState.m)

bindings/matlab/+iDynTreeWrappers/setWorldBaseTransform.m

@@ -0,0 +1,106 @@
+function [] = setWorldBaseTransform(KinDynModel,basePose)
+
+    % SETWORLDBASETRANSFORM sets the joints configuration for kino-dynamic 
+    %                       computations.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT: [] = setWorldBaseTransform(KinDynModel,basePose)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - basePose: [4 x 4] from base frame to world frame transform;
+    %
+    % Author : Lorenzo Moretti ([email protected])
+    %
+    % SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+% SPDX-License-Identifier: BSD-3-Clause
+
+    %% ------------Initialization----------------
+
+    % Debug input
+    if KinDynModel.DEBUG
+
+        disp('[setWorldBaseTransform]: debugging inputs...')
+
+        % basePose must be a valid transformation matrix
+        if size(basePose,1) ~= 4 || size(basePose,2) ~= 4
+
+            error('[setWorldBaseTransform]: basePose is not a 4x4 matrix.')
+        end
+
+        for ii = 1:4
+
+            if ii < 4
+
+                if abs(basePose(4,ii)) > 0.0001 
+
+                    error('[setWorldBaseTransform]: the last line of basePose is not [0,0,0,1].')
+                end
+            else
+                if abs(basePose(4,ii)) > 1.0001 || abs(basePose(4,ii)) < 0.9999
+
+                    error('[setWorldBaseTransform]: the last line of basePose is not [0,0,0,1].')
+                end
+            end
+        end
+
+        % baseRotation = basePose(1:3,1:3) must be a valid rotation matrix
+        if det(basePose(1:3,1:3)) < 0.9 || det(basePose(1:3,1:3)) > 1.1
+
+            error('[setWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+        end
+
+        IdentityMatr = basePose(1:3,1:3)*basePose(1:3,1:3)';
+
+        for kk = 1:size(IdentityMatr, 1)
+
+            for jj = 1:size(IdentityMatr, 1)
+
+                if jj == kk
+
+                    if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+
+                        error('[setWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.01
+
+                        error('[setWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+                    end
+                end
+            end   
+        end   
+
+        disp('[setWorldBaseTransform]: done.')     
+    end
+
+    % define the quantities required to set the floating base
+    baseRotation_iDyntree = iDynTree.Rotation();
+    baseOrigin_iDyntree   = iDynTree.Position();
+    basePose_iDyntree     = iDynTree.Transform();
+
+    % set the element of the rotation matrix and of the base
+    % position vector
+    for k = 0:2
+
+        baseOrigin_iDyntree.setVal(k,basePose(k+1,4));
+
+        for j = 0:2
+
+            baseRotation_iDyntree.setVal(k,j,basePose(k+1,j+1));                   
+        end
+    end      
+
+    % add the rotation matrix and the position to basePose_iDyntree
+    basePose_iDyntree.setRotation(baseRotation_iDyntree);
+    basePose_iDyntree.setPosition(baseOrigin_iDyntree);
+
+    % set the world base transform
+    ack = KinDynModel.kinDynComp.setWorldBaseTransform(basePose_iDyntree);
+
+    % check for errors
+    if ~ack  
+        error('[setWorldBaseTransform]: unable to set the world base transform.')
+    end  
+end

bindings/matlab/CMakeLists.txt

@@ -115,6 +115,7 @@ if(IDYNTREE_USES_MATLAB)
           ${IDYNTREE_GENERATE_MATLAB}
           ${MEX_BINDINGS_SOURCE_DIR})
 
+  target_include_directories(${target_name} PRIVATE ${Matlab_INCLUDE_DIRS})
   target_link_libraries(${target_name} ${IDYNTREE_LIBRARIES} ${Matlab_MEX_LIBRARY} ${Matlab_MX_LIBRARY})
 
   # Install the generated front-end to ${CMAKE_INSTALL_PREFIX}/mex

bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m

@@ -7,55 +7,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1340, varargin{:});
+        tmp = iDynTreeMEX(1342, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1341, self);
+        iDynTreeMEX(1343, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1342, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1343, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1345, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1346, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1345, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1347, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1346, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1348, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1347, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1348, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1355, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1356, self, varargin{:});
     end
   end
   methods(Static)

bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m

@@ -1,3 +1,3 @@
 function varargout = ArticulatedBodyAlgorithm(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1487, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1489, varargin{:});
 end

bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m

@@ -9,28 +9,18 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1465, varargin{:});
+        tmp = iDynTreeMEX(1467, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1466, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1468, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1467, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1469, self, varargin{:});
     end
     function varargout = S(self, varargin)
-      narginchk(1, 2)
-      if nargin==1
-        nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1468, self);
-      else
-        nargoutchk(0, 0)
-        iDynTreeMEX(1469, self, varargin{1});
-      end
-    end
-    function varargout = U(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -40,7 +30,7 @@
         iDynTreeMEX(1471, self, varargin{1});
       end
     end
-    function varargout = D(self, varargin)
+    function varargout = U(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -50,7 +40,7 @@
         iDynTreeMEX(1473, self, varargin{1});
       end
     end
-    function varargout = u(self, varargin)
+    function varargout = D(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -60,7 +50,7 @@
         iDynTreeMEX(1475, self, varargin{1});
       end
     end
-    function varargout = linksVel(self, varargin)
+    function varargout = u(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -70,7 +60,7 @@
         iDynTreeMEX(1477, self, varargin{1});
       end
     end
-    function varargout = linksBiasAcceleration(self, varargin)
+    function varargout = linksVel(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -80,7 +70,7 @@
         iDynTreeMEX(1479, self, varargin{1});
       end
     end
-    function varargout = linksAccelerations(self, varargin)
+    function varargout = linksBiasAcceleration(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -90,7 +80,7 @@
         iDynTreeMEX(1481, self, varargin{1});
       end
     end
-    function varargout = linkABIs(self, varargin)
+    function varargout = linksAccelerations(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -100,7 +90,7 @@
         iDynTreeMEX(1483, self, varargin{1});
       end
     end
-    function varargout = linksBiasWrench(self, varargin)
+    function varargout = linkABIs(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
@@ -110,9 +100,19 @@
         iDynTreeMEX(1485, self, varargin{1});
       end
     end
+    function varargout = linksBiasWrench(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1486, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1487, self, varargin{1});
+      end
+    end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1486, self);
+        iDynTreeMEX(1488, self);
         self.SwigClear();
       end
     end

bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m

@@ -7,30 +7,30 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1731, self);
+        varargout{1} = iDynTreeMEX(1733, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1732, self, varargin{1});
+        iDynTreeMEX(1734, self, varargin{1});
       end
     end
     function varargout = m_angular_velocity(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1733, self);
+        varargout{1} = iDynTreeMEX(1735, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1734, self, varargin{1});
+        iDynTreeMEX(1736, self, varargin{1});
       end
     end
     function varargout = m_gyroscope_bias(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1735, self);
+        varargout{1} = iDynTreeMEX(1737, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1736, self, varargin{1});
+        iDynTreeMEX(1738, self, varargin{1});
       end
     end
     function self = AttitudeEstimatorState(varargin)
@@ -39,14 +39,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1737, varargin{:});
+        tmp = iDynTreeMEX(1739, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1738, self);
+        iDynTreeMEX(1740, self);
         self.SwigClear();
       end
     end