Add matlab/octave high level wrappers

README.md

@@ -167,6 +167,10 @@ for example because you modified some iDynTree classes, you can install the expe
 version of Swig with Matlab support from https://github.com/robotology-dependencies/swig/ (branch `matlab`) and then enable Matlab bindings generation with the `IDYNTREE_GENERATE_MATLAB` options.
 For more info on how to modify the matlab bindings, see https://github.com/robotology/idyntree/blob/master/doc/dev/faqs.md#how-to-add-wrap-a-new-class-or-function-with-swig .
 
+##### Matlab/Octave high level wrappers
+They are a collection of Matlab/Octave functions that wraps the functionalities of (mainly) the iDyntree class `KinDynComputations` into functions with a typical Matlab/Octave interface. The purpose of the high-level wrappers is to provide a simpler and easy-to-use interface for Matlab/Octave users who want 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). More details and a complete list of the wrappers can be found in the [wrappers README](/bindings/matlab/+iDynTreeWrappers/README.md).
+
+**Usage**: the wrappers package is installed together with the iDyntree bindings when compiling iDyntree with option `IDYNTREE_USES_MATLAB` or `IDYNTREE_USES_OCTAVE` set to `ON`. The functions can be called from Matlab/Octave using the namespace `iDynTreeWrappers`, i.e. `iDynTreeWrappers.name_of_the_corresponding_iDynTree_method`.
 
 ## Tutorials
 | Topic  | C++ | Matlab | Python |

bindings/matlab/+iDynTreeWrappers/README.md

@@ -0,0 +1,77 @@
+# idyntree high-level-wrappers
+
+A collection of Matlab/Octave functions that wraps the functionalities of (mainly) the iDyntree class `KinDynComputations`. For further information on the single wrapper, refer to the description included in each function.
+
+## Motivations
+
+For a Matlab/Octave user, it may be sometimes counterintuitive to use C++ based formalism inside Matlab/Octave. Furthermore, there are common iDyntree functions that require several lines of code in order to be used correctly. E.g. see the `getRobotState` function:
+
+```
+    basePose_iDyntree   = iDynTree.Transform();
+    jointPos_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    baseVel_iDyntree    = iDynTree.Twist();
+    jointVel_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    gravityVec_iDyntree = iDynTree.Vector3();
+
+    KinDynModel.kinDynComp.getRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree);
+
+    baseRotation_iDyntree = basePose_iDyntree.getRotation;
+    baseOrigin_iDyntree   = basePose_iDyntree.getPosition;
+
+    baseRotation = baseRotation_iDyntree.toMatlab;
+    baseOrigin   = baseOrigin_iDyntree.toMatlab;
+    jointPos     = jointPos_iDyntree.toMatlab;
+    baseVel      = baseVel_iDyntree.toMatlab;
+    jointVel     = jointVel_iDyntree.toMatlab; 
+    basePose     = [baseRotation, baseOrigin;
+                       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).
+
+## KinDynComputations class
+
+### Load the reduced model
+
+- [loadReducedModel](loadReducedModel.m)
+
+### Retrieve data from the model
+
+- [getJointPos](getJointPos.m)
+- [getJointVel](getJointVel.m)
+- [getCentroidalTotalMomentum](getCentroidalTotalMomentum.m)
+- [getRobotState](getRobotState.m)
+- [getWorldBaseTransform](getWorldBaseTransform.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) 
+- [getFreeFloatingMassMatrix](getFreeFloatingMassMatrix.m)
+- [getFrameBiasAcc](getFrameBiasAcc.m)
+- [getFrameFreeFloatingJacobian](getFrameFreeFloatingJacobian.m)
+- [getCenterOfMassPosition](getCenterOfMassPosition.m) 
+- [generalizedBiasForces](generalizedBiasForces.m) 
+- [generalizedGravityForces](generalizedGravityForces.m)
+- [getCenterOfMassJacobian](getCenterOfMassJacobian.m) 
+- [getCenterOfMassVelocity](getCenterOfMassVelocity.m) 
+
+### Set the model-related quantities
+
+- [setJointPos](setJointPos.m) 
+- [setFrameVelocityRepresentation](setFrameVelocityRepresentation.m)
+- [setFloatingBase](setFloatingBase.m) 
+- [setRobotState](setRobotState.m)
+
+## Visualizer class
+
+Not proper wrappers, they wrap more than one method of the class each. **Requirements:** compile iDyntree with Irrlicht `(IDYNTREE_USES_IRRLICHT = ON)`.
+
+- [initializeVisualizer](initializeVisualizer.m)
+- [visualizerSetup](visualizerSetup.m)
+- [updateVisualizer](updateVisualizer.m)

bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m

@@ -0,0 +1,39 @@
+function h = generalizedBiasForces(KinDynModel)
+
+    % GENERALIZEDBIASFORCES retrieves the generalized bias forces from 
+    %                            the reduced model. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  h = generalizedBiasForces(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - h: [6+ndof x 1] generalized bias accelerations.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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----------------
+
+    % create the vector that must be populated with the bias forces
+    h_iDyntree = iDynTree.FreeFloatingGeneralizedTorques(KinDynModel.kinDynComp.model);
+
+    % get the bias forces
+    ack = KinDynModel.kinDynComp.generalizedBiasForces(h_iDyntree);
+
+    % check for errors
+    if ~ack
+        error('[generalizedBiasForces]: unable to get the bias forces from the reduced model.')
+    end
+
+    % convert to Matlab format: compute the base bias acc (h_b) and the
+    % joint bias acc (h_s) and concatenate them
+    h_b = h_iDyntree.baseWrench.toMatlab;
+    h_s = h_iDyntree.jointTorques.toMatlab;   
+    h   = [h_b;h_s];
+end

bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m

@@ -0,0 +1,39 @@
+function g = generalizedGravityForces(KinDynModel)
+
+    % GENERALIZEDGRAVITYFORCES retrieves the generalized gravity forces 
+    %                               given the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  g = generalizedGravityForces(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - g: [6+ndof x 1] generalized gravity forces.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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----------------
+
+    % create the vector that must be populated with the gravity forces
+    g_iDyntree = iDynTree.FreeFloatingGeneralizedTorques(KinDynModel.kinDynComp.model);
+
+    % get the gravity forces
+    ack = KinDynModel.kinDynComp.generalizedGravityForces(g_iDyntree);
+
+    % check for errors
+    if ~ack  
+        error('[generalizedGravityForces]: unable to get the gravity forces from the reduced model.')
+    end
+
+    % convert to Matlab format: compute the base gravity forces (g_b) and the
+    % joint gravity forces (g_j) and concatenate them
+    g_b = g_iDyntree.baseWrench.toMatlab;
+    g_s = g_iDyntree.jointTorques.toMatlab;   
+    g   = [g_b; g_s];
+end

bindings/matlab/+iDynTreeWrappers/getBaseTwist.m

@@ -0,0 +1,27 @@
+function baseVel = getBaseTwist(KinDynModel)
+
+    % GETBASETWIST retrieves the robot base velocity from the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  baseVel = getBaseTwist(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - baseVel: [6 x 1] vector of base linear and angular velocity;
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 base velocities
+    baseVel_iDyntree = KinDynModel.kinDynComp.getBaseTwist();
+
+    % convert to Matlab format
+    baseVel = baseVel_iDyntree.toMatlab;
+end

bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m

@@ -0,0 +1,35 @@
+function J_CoM = getCenterOfMassJacobian(KinDynModel)
+
+    % GETCENTEROFMASSJACOBIAN retrieves the CoM jacobian.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  J_CoM = getCenterOfMassJacobian(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - J_CoM: [3 x 6+ndof] CoM free floating Jacobian.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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----------------
+
+    % create the matrix that must be populated with the jacobian map
+    J_CoM_iDyntree = iDynTree.MatrixDynSize(3,KinDynModel.NDOF+6);
+
+    % get the free floating jacobian
+    ack = KinDynModel.kinDynComp.getCenterOfMassJacobian(J_CoM_iDyntree);
+
+    % check for errors
+    if ~ack    
+        error('[getCenterOfMassJacobian]: unable to get the CoM Jacobian from the reduced model.')
+    end
+
+    % convert to Matlab format
+    J_CoM = J_CoM_iDyntree.toMatlab;
+end

bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m

@@ -0,0 +1,27 @@
+function posCoM = getCenterOfMassPosition(KinDynModel)
+
+    % GETCENTEROFMASSPOSITION retrieves the CoM position in world coordinates.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  posCoM = getCenterOfMassPosition(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - posCoM: [3 x 1] CoM position w.r.t. world frame.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 CoM position
+    posCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassPosition(); 
+
+    % covert to matlab
+    posCoM = posCoM_iDyntree.toMatlab;
+end

bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m

@@ -0,0 +1,27 @@
+function velCoM = getCenterOfMassVelocity(KinDynModel)
+
+    % GETCENTEROFMASSVELOCITY retrieves the CoM velocity in world coordinates.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  velCoM = getCenterOfMassVelocity(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - velCoM: [3 x 1] CoM velocity w.r.t. world frame.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 CoM velocity
+    velCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassVelocity(); 
+
+    % covert to matlab
+    velCoM = velCoM_iDyntree.toMatlab;
+end

bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m

@@ -0,0 +1,29 @@
+function totalMomentum = getCentroidalTotalMomentum(KinDynModel)
+
+    % GETCENTROIDALTOTALMOMENTUM retrieves the centroidal momentum from the reduced
+    %                                 model. The quantity is expressed in a frame that depends
+    %                                 on the 'FrameVelocityReperesentation' settings.                               
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  totalMomentum = getCentroidalTotalMomentum(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - totalMomentum: [6 x 1] vector of linear and angular momentum.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 momentum
+    totalMomentum_iDyntree = KinDynModel.kinDynComp.getCentroidalTotalMomentum();
+
+    % convert to Matlab format
+    totalMomentum = totalMomentum_iDyntree.toMatlab;
+end

bindings/matlab/+iDynTreeWrappers/getFloatingBase.m

@@ -0,0 +1,25 @@
+function baseLinkName = getFloatingBase(KinDynModel)
+
+    % GETFLOATINGBASE retrieves the floating base link name from the 
+    %                      reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  baseLinkName = getFloatingBase(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - baseLinkName: name of the base link.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 name of the floating base link
+    baseLinkName = KinDynModel.kinDynComp.getFloatingBase();  
+end

bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m

@@ -0,0 +1,29 @@
+function JDot_nu_frame = getFrameBiasAcc(KinDynModel,frameName)
+
+    % GETFRAMEBIASACC gets the bias accelerations of a specified frame. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  JDot_nu_frame = getFrameBiasAcc(KinDynModel,frameName)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - frameName: a string that specifies the frame w.r.t. compute the 
+    %                       bias accelerations, or the associated ID;     
+    %
+    % OUTPUTS: - JDot_nu_frame: [6 x 6+ndof] frame bias accelerations.
+    %
+    % Author : Gabriele Nava (gabriele.nava@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 bias acc
+    JDot_nu_frame_iDyntree = KinDynModel.kinDynComp.getFrameBiasAcc(frameName);
+
+    % convert to Matlab format
+    JDot_nu_frame = JDot_nu_frame_iDyntree.toMatlab;
+end