From 0794920fc32db91d4af9191d27d13f65fcfdfb45 Mon Sep 17 00:00:00 2001 From: Gabriele Date: Mon, 15 Apr 2019 13:45:28 +0200 Subject: [PATCH 1/6] added matlab/octave high level wrappers --- bindings/matlab/+iDynTreeWrappers/README.md | 77 ++++++ .../+iDynTreeWrappers/generalizedBiasForces.m | 36 +++ .../generalizedGravityForces.m | 36 +++ .../matlab/+iDynTreeWrappers/getBaseTwist.m | 24 ++ .../getCenterOfMassJacobian.m | 32 +++ .../getCenterOfMassPosition.m | 24 ++ .../getCenterOfMassVelocity.m | 24 ++ .../getCentroidalTotalMomentum.m | 26 ++ .../+iDynTreeWrappers/getFloatingBase.m | 22 ++ .../+iDynTreeWrappers/getFrameBiasAcc.m | 26 ++ .../getFrameFreeFloatingJacobian.m | 35 +++ .../matlab/+iDynTreeWrappers/getFrameIndex.m | 22 ++ .../matlab/+iDynTreeWrappers/getFrameName.m | 22 ++ .../getFrameVelocityRepresentation.m | 49 ++++ .../getFreeFloatingMassMatrix.m | 56 +++++ .../matlab/+iDynTreeWrappers/getJointPos.m | 32 +++ .../matlab/+iDynTreeWrappers/getJointVel.m | 32 +++ .../matlab/+iDynTreeWrappers/getModelVel.m | 33 +++ .../getNrOfDegreesOfFreedom.m | 35 +++ .../+iDynTreeWrappers/getRelativeJacobian.m | 38 +++ .../+iDynTreeWrappers/getRelativeTransform.m | 90 +++++++ .../matlab/+iDynTreeWrappers/getRobotState.m | 78 ++++++ .../+iDynTreeWrappers/getWorldBaseTransform.m | 64 +++++ .../+iDynTreeWrappers/getWorldTransform.m | 65 +++++ .../+iDynTreeWrappers/initializeVisualizer.m | 39 +++ .../+iDynTreeWrappers/loadReducedModel.m | 60 +++++ .../+iDynTreeWrappers/setFloatingBase.m | 26 ++ .../setFrameVelocityRepresentation.m | 52 ++++ .../matlab/+iDynTreeWrappers/setJointPos.m | 49 ++++ .../matlab/+iDynTreeWrappers/setRobotState.m | 230 ++++++++++++++++++ .../+iDynTreeWrappers/updateVisualizer.m | 124 ++++++++++ .../+iDynTreeWrappers/visualizerSetup.m | 50 ++++ bindings/matlab/CMakeLists.txt | 12 +- bindings/matlab/tests/CMakeLists.txt | 6 +- .../matlab/tests/highLevelWrappersSmokeTest.m | 155 ++++++++++++ 35 files changed, 1777 insertions(+), 4 deletions(-) create mode 100644 bindings/matlab/+iDynTreeWrappers/README.md create mode 100644 bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m create mode 100644 bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getBaseTwist.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFloatingBase.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameIndex.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameName.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getJointPos.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getJointVel.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getModelVel.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getRobotState.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m create mode 100644 bindings/matlab/+iDynTreeWrappers/getWorldTransform.m create mode 100644 bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m create mode 100644 bindings/matlab/+iDynTreeWrappers/loadReducedModel.m create mode 100644 bindings/matlab/+iDynTreeWrappers/setFloatingBase.m create mode 100644 bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m create mode 100644 bindings/matlab/+iDynTreeWrappers/setJointPos.m create mode 100644 bindings/matlab/+iDynTreeWrappers/setRobotState.m create mode 100644 bindings/matlab/+iDynTreeWrappers/updateVisualizer.m create mode 100644 bindings/matlab/+iDynTreeWrappers/visualizerSetup.m create mode 100644 bindings/matlab/tests/highLevelWrappersSmokeTest.m diff --git a/bindings/matlab/+iDynTreeWrappers/README.md b/bindings/matlab/+iDynTreeWrappers/README.md new file mode 100644 index 00000000000..c2b50532dff --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/README.md @@ -0,0 +1,77 @@ +# idyntree high-level-wrappers + +A collection of Matlab 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 user, it may be sometimes counterintuitive to use C++ based formalism inside Matlab. 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 users who wants to use iDyntree inside Matlab, 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) diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m new file mode 100644 index 00000000000..e68da93cdeb --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m @@ -0,0 +1,36 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: h = generalizedBiasForces(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - h: [ndof+6 x 1] generalized bias accelerations. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------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 diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m new file mode 100644 index 00000000000..25a23e21dbb --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m @@ -0,0 +1,36 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: g = generalizedGravityForces(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - g: [nDof+6 x 1] generalized gravity forces. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------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 diff --git a/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m new file mode 100644 index 00000000000..34836d5a3fe --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m @@ -0,0 +1,24 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the base velocities + baseVel_iDyntree = KinDynModel.kinDynComp.getBaseTwist(); + + % convert to Matlab format + baseVel = baseVel_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m new file mode 100644 index 00000000000..e046320e2a6 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m @@ -0,0 +1,32 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: J_CoM = getCenterOfMassJacobian(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - J_CoM: [3 x ndof+6] CoM free floating Jacobian. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------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 diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m new file mode 100644 index 00000000000..b82dce36760 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m @@ -0,0 +1,24 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the CoM position + posCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassPosition(); + + % covert to matlab + posCoM = posCoM_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m new file mode 100644 index 00000000000..8e9bfc808b7 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m @@ -0,0 +1,24 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the CoM velocity + velCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassVelocity(); + + % covert to matlab + velCoM = velCoM_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m new file mode 100644 index 00000000000..2c1e1b35925 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m @@ -0,0 +1,26 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the momentum + totalMomentum_iDyntree = KinDynModel.kinDynComp.getCentroidalTotalMomentum(); + + % convert to Matlab format + totalMomentum = totalMomentum_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m new file mode 100644 index 00000000000..a2107ecb75c --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m @@ -0,0 +1,22 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the name of the floating base link + baseLinkName = KinDynModel.kinDynComp.getFloatingBase(); +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m new file mode 100644 index 00000000000..60c1de7340a --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m @@ -0,0 +1,26 @@ +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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % 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 ndof+6] frame bias accelerations. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------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 diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m new file mode 100644 index 00000000000..5601dca420a --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m @@ -0,0 +1,35 @@ +function J_frame = getFrameFreeFloatingJacobian(KinDynModel,frameName) + + % GETFRAMEFREEFLOATINGJACOBIAN gets the free floating jacobian of a + % specified frame. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: J_frame = getFrameFreeFloatingJacobian(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 + % jacobian matrix, or the associated ID; + % + % OUTPUTS: - J_frame: [6 x ndof+6] frame free floating Jacobian. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the matrix that must be populated with the jacobian map + J_frame_iDyntree = iDynTree.MatrixDynSize(6,KinDynModel.NDOF+6); + + % get the free floating jacobian + ack = KinDynModel.kinDynComp.getFrameFreeFloatingJacobian(frameName,J_frame_iDyntree); + + % check for errors + if ~ack + error('[getFrameFreeFloatingJacobian]: unable to get the Jacobian from the reduced model.') + end + + % convert to Matlab format + J_frame = J_frame_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m new file mode 100644 index 00000000000..91043b9042a --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m @@ -0,0 +1,22 @@ +function frameID = getFrameIndex(KinDynModel,frameName) + + % GETFRAMEINDEX gets the index corresponding to a given frame name. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: frameID = getFrameIndex(KinDynModel,frameName) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % - frameName: a string specifying a valid frame name. + % + % OUTPUTS: - frameID: the ID associated to the given frame name. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the ID of the given frame + frameID = KinDynModel.kinDynComp.getFrameIndex(frameName); +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameName.m b/bindings/matlab/+iDynTreeWrappers/getFrameName.m new file mode 100644 index 00000000000..c0640dec18b --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFrameName.m @@ -0,0 +1,22 @@ +function frameName = getFrameName(KinDynModel,frameID) + + % GETFRAMENAME gets the name corresponding to a given frame index. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: frameName = getFrameName(KinDynModel,frameID) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % - frameID: the ID associated to the given frame name. + % + % OUTPUTS: - frameName: a string specifying a valid frame name. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the ID of the given frame + frameName = KinDynModel.kinDynComp.getFrameName(frameID); +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m new file mode 100644 index 00000000000..9b5c780ce9a --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m @@ -0,0 +1,49 @@ +function frameVelRepr = getFrameVelocityRepresentation(KinDynModel) + + % GETFRAMEVELOCITYREPRESENTATION retrieves the current frame velocity + % representation. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: frameVelRepr = getFrameVelocityRepresentation(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - frameVelRepr: a string with one of the following possible values: + % 'mixed', 'body', 'inertial'; + % + % Possible frame velocity representations: + % + % 0 = INERTIAL_FIXED_REPRESENTATION + % + % 1 = BODY_FIXED_REPRESENTATION + % + % 2 = MIXED_REPRESENTATION + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + frameVelRepr_idyntree = KinDynModel.kinDynComp.getFrameVelocityRepresentation(); + + % output the current frame velocity representation + switch frameVelRepr_idyntree + + case 2 + + frameVelRepr = 'mixed'; + + case 1 + + frameVelRepr = 'body'; + + case 0 + + frameVelRepr = 'inertial'; + + otherwise + error('[setFrameVelocityRepresentation]: frameVelRepr is not a valid string.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m new file mode 100644 index 00000000000..fb262324f38 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m @@ -0,0 +1,56 @@ +function M = getFreeFloatingMassMatrix(KinDynModel) + + % GETFREEFLOATINGMASSMATRIX retrieves the free floating mass matrix. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: M = getFreeFloatingMassMatrix(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - M: [ndof+6 x ndof+6] free floating mass matrix. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the matrix that must be populated with the mass info + M_iDyntree = iDynTree.MatrixDynSize(KinDynModel.NDOF+6,KinDynModel.NDOF+6); + + % get the mass matrix + ack = KinDynModel.kinDynComp.getFreeFloatingMassMatrix(M_iDyntree); + + % check for errors + if ~ack + error('[getFreeFloatingMassMatrix]: unable to retrieve the mass matrix from the reduced model.') + end + + % convert to Matlab format + M = M_iDyntree.toMatlab; + + % debug output + if KinDynModel.DEBUG + + disp('[getFreeFloatingMassMatrix]: debugging outputs...') + + % check mass matrix symmetry and positive definiteness + M_symm_error = norm(M -(M + M')/2); + M_symm_eig = eig((M + M')/2); + + if M_symm_error > 0.1 + + error('[getFreeFloatingMassMatrix]: M is not symmetric') + end + + for k = 1:length(M_symm_eig) + + if M_symm_eig(k) < 0 + + error('[getFreeFloatingMassMatrix]: M is not positive definite') + end + end + disp('[getFreeFloatingMassMatrix]: done.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getJointPos.m b/bindings/matlab/+iDynTreeWrappers/getJointPos.m new file mode 100644 index 00000000000..c42fa3b825b --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getJointPos.m @@ -0,0 +1,32 @@ +function jointPos = getJointPos(KinDynModel) + + % GETJOINTPOS retrieves the joints configuration from the reduced model. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: jointPos = getJointPos(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - jointPos: [ndof x 1] vector of joint positions. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the vector that must be populated with the joint positions + jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + + % get the joints positions + ack = KinDynModel.kinDynComp.getJointPos(jointPos_iDyntree); + + % check for errors + if ~ack + error('[getJointPos]: unable to retrieve the joint positions from the reduced model.') + end + + % convert to Matlab format + jointPos = jointPos_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getJointVel.m b/bindings/matlab/+iDynTreeWrappers/getJointVel.m new file mode 100644 index 00000000000..48b8c01fd53 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getJointVel.m @@ -0,0 +1,32 @@ +function jointVel = getJointVel(KinDynModel) + + % GETJOINTVEL retrieves joint velocities from the reduced model. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: jointVel = getJointVel(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - jointVel: [ndof x 1] vector of joint velocities. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the vector that must be populated with the joint velocities + jointVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + + % get the joints velocities + ack = KinDynModel.kinDynComp.getJointVel(jointVel_iDyntree); + + % check for errors + if ~ack + error('[getJointVel]: unable to retrieve the joint velocities from the reduced model.') + end + + % convert to Matlab format + jointVel = jointVel_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getModelVel.m b/bindings/matlab/+iDynTreeWrappers/getModelVel.m new file mode 100644 index 00000000000..ba2b1aafbff --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getModelVel.m @@ -0,0 +1,33 @@ +function stateVel = getModelVel(KinDynModel) + + % GETMODELVEL gets the joints and floating base velocities from the + % reduced model. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: stateVel = getModelVel(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - stateVel: [ndof+6 x 1] vector of joints and base velocities. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the vector that must be populated with the stae velocities + stateVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + + % get the joints velocities + ack = KinDynModel.kinDynComp.getModelVel(stateVel_iDyntree); + + % check for errors + if ~ack + error('[getModelVel]: unable to retrieve the state velocities from the reduced model.') + end + + % convert to Matlab format + stateVel = stateVel_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m new file mode 100644 index 00000000000..8c62c6231bd --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m @@ -0,0 +1,35 @@ +function nDof = getNrOfDegreesOfFreedom(KinDynModel) + + % GETNROFDEGREESOFFREEDOM gets the dimension of the joint space. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: nDof = getNrOfDegreesOfFreedom(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - nDof: number of DoFs of the system. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the number of DoF + nDof = KinDynModel.kinDynComp.getNrOfDegreesOfFreedom(); + + % Debug output + if KinDynModel.DEBUG + + disp('[getNrOfDegreesOfFreedom]: debugging outputs...') + + % check nDof is not empty + if isempty(nDof) + + error('[getNrOfDegreesOfFreedom]: nDof is empty.') + end + + disp('[getNrOfDegreesOfFreedom]: done.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m new file mode 100644 index 00000000000..bd1e3058d2a --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m @@ -0,0 +1,38 @@ +function J_frameVel = getRelativeJacobian(KinDynModel,frameVelID,frameRefID) + + % GETRELATIVEJACOBIAN gets the relative jacobian, i.e. the matrix that + % maps the velocity of frameVel expressed w.r.t. + % frameRef, to the joint velocity. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: J_frameVel = getRelativeJacobian(KinDynModel,frameVelID,frameRefID) + % + % INPUTS: - frameRefID: a number that specifies the frame w.r.t. the velocity + % of frameVel is expressed; + % - frameVelID: a number that specifies the frame whose velocity + % is the one mapped by the jacobian; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - J_frameVel: [6 x ndof] frameVel Jacobian. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % create the matrix that must be populated with the jacobian map + J_frameVel_iDyntree = iDynTree.MatrixDynSize(6,KinDynModel.NDOF); + + % get the relative jacobian + ack = KinDynModel.kinDynComp.getRelativeJacobian(frameVelID,frameRefID,J_frameVel_iDyntree); + + % check for errors + if ~ack + error('[getRelativeJacobian]: unable to get the relative jacobian from the reduced model.') + end + + % covert to Matlab format + J_frameVel = J_frameVel_iDyntree.toMatlab; +end diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m new file mode 100644 index 00000000000..e4d1dc217f6 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m @@ -0,0 +1,90 @@ +function frame1_H_frame2 = getRelativeTransform(KinDynModel,frame1Name,frame2Name) + + % GETRELATIVETRANSFORM gets the transformation matrix between two specified + % frames. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: frame1_H_frame2 = getRelativeTransform(KinDynModel,frame1Name,frame2Name) + % + % INPUTS: - frame1Name: a string that specifies the frame w.r.t. compute the + % transfomation matrix, or the associated ID; + % - frame2Name: a string that specifies the frame w.r.t. compute the + % transfomation matrix, or the associated ID; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - frame1_H_frame2: [4 x 4] from frame2 to frame1 transformation matrix. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the transformation between the frame 1 and 2 + frame1_H_frame2_iDyntree = KinDynModel.kinDynComp.getRelativeTransform(frame1Name,frame2Name); + frame1_R_frame2_iDyntree = frame1_H_frame2_iDyntree.getRotation; + framePos_iDyntree = frame1_H_frame2_iDyntree.getPosition; + + % covert to Matlab format + frame1_R_frame2 = frame1_R_frame2_iDyntree.toMatlab; + framePos = framePos_iDyntree.toMatlab; + frame1_H_frame2 = [frame1_R_frame2,framePos; + 0, 0, 0, 1]; + + % Debug output + if KinDynModel.DEBUG + + disp('[getRelativeTransform]: debugging outputs...') + + % frame1_H_frame2 must be a valid transformation matrix + if size(frame1_H_frame2,1) ~= 4 || size(frame1_H_frame2,2) ~= 4 + + error('[getRelativeTransform]: frame1_H_frame2 is not a 4x4 matrix.') + end + + for ii = 1:4 + + if ii < 4 + + if abs(frame1_H_frame2(4,ii)) > 0.0001 + + error('[getRelativeTransform]: the last line of frame1_H_frame2 is not [0,0,0,1].') + end + else + if abs(frame1_H_frame2(4,ii)) > 1.0001 || abs(frame1_H_frame2(4,ii)) < 0.9999 + + error('[getRelativeTransform]: the last line of frame1_H_frame2 is not [0,0,0,1].') + end + end + end + + % frame1_R_frame2 = frame1_H_frame2(1:3,1:3) must be a valid rotation matrix + if det(frame1_H_frame2(1:3,1:3)) < 0.9 || det(frame1_H_frame2(1:3,1:3)) > 1.1 + + error('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.') + end + + IdentityMatr = frame1_H_frame2(1:3,1:3)*frame1_H_frame2(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('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.') + end + else + if abs(IdentityMatr(kk,jj)) > 0.01 + + error('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.') + end + end + end + end + disp('[getRelativeTransform]: done.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getRobotState.m b/bindings/matlab/+iDynTreeWrappers/getRobotState.m new file mode 100644 index 00000000000..712380658c2 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getRobotState.m @@ -0,0 +1,78 @@ +function [basePose,jointPos,baseVel,jointVel] = getRobotState(KinDynModel) + + % GETROBOTSTATE gets the floating base system state from the reduced model. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: [basePose,jointPos,baseVel,jointVel] = getRobotState(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - basePose: [4 x 4] from base frame to world frame transform; + % - jointPos: [ndof x 1] vector of joint positions; + % - baseVel: [6 x 1] vector of base velocity; + % - jointVel: [ndof x 1] vector of joint velocities. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % define all the iDyntree required quantities + basePose_iDyntree = iDynTree.Transform(); + jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + baseVel_iDyntree = iDynTree.Twist(); + jointVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + gravityVec_iDyntree = iDynTree.Vector3(); + + % get the floating base state + KinDynModel.kinDynComp.getRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree); + + % get the base position and orientation + baseRotation_iDyntree = basePose_iDyntree.getRotation; + baseOrigin_iDyntree = basePose_iDyntree.getPosition; + + % covert to Matlab format + baseRotation = baseRotation_iDyntree.toMatlab; + baseOrigin = baseOrigin_iDyntree.toMatlab; + basePose = [baseRotation, baseOrigin; + 0, 0, 0, 1]; + jointPos = jointPos_iDyntree.toMatlab; + baseVel = baseVel_iDyntree.toMatlab; + jointVel = jointVel_iDyntree.toMatlab; + + % Debug output + if KinDynModel.DEBUG + + disp('[getRobotState]: debugging outputs...') + + % 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('[getRobotState]: 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('[getRobotState]: baseRotation is not a valid rotation matrix.') + end + else + if abs(IdentityMatr(kk,jj)) > 0.01 + + error('[getRobotState]: baseRotation is not a valid rotation matrix.') + end + end + end + end + disp('[getRobotState]: done.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m new file mode 100644 index 00000000000..17611538471 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m @@ -0,0 +1,64 @@ +function basePose = getWorldBaseTransform(KinDynModel) + + % GETWORLDBASETRANSFORM gets the transformation matrix between the base frame + % and the inertial reference frame. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: basePose = getWorldBaseTransform(KinDynModel) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - basePose: [4 x 4] from base to world transformation matrix. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % get the transformation between the base and the world frames + basePose_iDyntree = KinDynModel.kinDynComp.getWorldBaseTransform(); + baseRotation_iDyntree = basePose_iDyntree.getRotation; + baseOrigin_iDyntree = basePose_iDyntree.getPosition; + + % covert to Matlab format + baseRotation = baseRotation_iDyntree.toMatlab; + baseOrigin = baseOrigin_iDyntree.toMatlab; + basePose = [baseRotation, baseOrigin; + 0, 0, 0, 1]; + % Debug output + if KinDynModel.DEBUG + + disp('[getWorldBaseTransform]: debugging outputs...') + + % 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('[getWorldBaseTransform]: 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('[getWorldBaseTransform]: baseRotation is not a valid rotation matrix.') + end + else + if abs(IdentityMatr(kk,jj)) > 0.01 + + error('[getWorldBaseTransform]: baseRotation is not a valid rotation matrix.') + end + end + end + end + + disp('[getWorldBaseTransform]: done.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m new file mode 100644 index 00000000000..48002e3e9b7 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m @@ -0,0 +1,65 @@ +function w_H_frame = getWorldTransform(KinDynModel,frameName) + + % GETWORLDTRANSFORM gets the transformation matrix between a specified + % frame and the inertial reference frame. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: w_H_frame = getWorldTransform(KinDynModel,frameName) + % + % INPUTS: - frameName: a string that specifies the frame w.r.t. compute the + % transfomation matrix, or the associated ID; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % OUTPUTS: - w_H_frame: [4 x 4] from frame to world transformation matrix. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------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]; + % Debug output + if KinDynModel.DEBUG + + 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 diff --git a/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m new file mode 100644 index 00000000000..e69564dacbb --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m @@ -0,0 +1,39 @@ +function Visualizer = initializeVisualizer(KinDynModel,debugMode) + + % INITIALIZEVISUALIZER opens the iDyntree visualizer and loads the reduced + % model into the visualizer. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). + % + % FORMAT: Visualizer = initializeVisualizer(KinDynModel,debugMode) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % - debugMode: if TRUE, the visualizer is used in "debug" mode; + % + % OUTPUTS: - Visualizer: a structure containing the visualizer and its options. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + Visualizer.viz = iDynTree.Visualizer(); + + % load the model in the visualizer + ack = Visualizer.viz.addModel(KinDynModel.kinDynComp.model(),'viz1'); + + % check for errors + if ~ack + error('[initializeVisualizer]: unable to load the model in the visualizer.') + end + + % draw the model + Visualizer.viz.draw(); + + % if DEBUG option is set to TRUE, all the wrappers related to the + % iDyntree visualizer will be run in DEBUG mode. + Visualizer.DEBUG = debugMode; +end diff --git a/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m new file mode 100644 index 00000000000..ad88ca7b549 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m @@ -0,0 +1,60 @@ +function KinDynModel = loadReducedModel(jointList,baseLinkName,modelPath,modelName,debugMode) + + % LOADREDUCEDMODEL loads the urdf model of the rigid multi-body system. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: KinDynModel = loadReducedModel(jointList,baseLinkName,modelPath,modelName,debugMode) + % + % INPUTS: - jointList: cell array containing the list of joints to be used + % in the reduced model; + % - baseLinkName: a string that specifies link which is considered + % as the floating base; + % - modelPath: a string that specifies the path to the urdf model; + % - modelName: a string that specifies the model name; + % - debugMode: if TRUE, the wrappers are used in "debug" mode; + % + % OUTPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % + % Author: Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + disp(['[loadReducedModel]: loading the following model: ',[modelPath,modelName]]); + + % if DEBUG option is set to TRUE, all the wrappers will be run in debug + % mode. Wrappers concerning iDyntree simulator have their own debugger + KinDynModel.DEBUG = debugMode; + + % retrieve the link that will be used as the floating base + KinDynModel.BASE_LINK = baseLinkName; + + % load the list of joints to be used in the reduced model + jointList_idyntree = iDynTree.StringVector(); + + for k = 1:length(jointList) + + jointList_idyntree.push_back(jointList{k}); + end + + % only joints specified in the joint list will be considered in the model + modelLoader = iDynTree.ModelLoader(); + reducedModel = modelLoader.model(); + + modelLoader.loadReducedModelFromFile([modelPath,modelName], jointList_idyntree); + + % get the number of degrees of freedom of the reduced model + KinDynModel.NDOF = reducedModel.getNrOfDOFs(); + + % initialize the iDyntree KinDynComputation class, that will be used for + % computing the floating base system state, dynamics, and kinematics + KinDynModel.kinDynComp = iDynTree.KinDynComputations(); + + KinDynModel.kinDynComp.loadRobotModel(reducedModel); + + % set the floating base link + KinDynModel.kinDynComp.setFloatingBase(KinDynModel.BASE_LINK); + + disp(['[loadReducedModel]: loaded model: ',[modelPath,modelName],', number of joints: ',num2str(KinDynModel.NDOF)]); +end diff --git a/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m new file mode 100644 index 00000000000..d75408457fa --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m @@ -0,0 +1,26 @@ +function [] = setFloatingBase(KinDynModel,floatBaseLinkName) + + % SETFLOATINGBASE sets the link that is used as floating base. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: [] = setFloatingBase(KinDynModel,floatBaseLinkName) + % + % INPUTS: - floatBaseLinkName: a string with the name of the link to be + % used as floating base; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % set the floating base link + ack = KinDynModel.kinDynComp.setFloatingBase(floatBaseLinkName); + + % check for errors + if ~ack + error('[setFloatingBase]: unable to set the floating base link.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m new file mode 100644 index 00000000000..93e6f0df2b9 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m @@ -0,0 +1,52 @@ +function [] = setFrameVelocityRepresentation(KinDynModel,frameVelRepr) + + % SETFRAMEVELOCITYREPRESENTATION sets the frame velocity representation. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: [] = setFrameVelocityRepresentation(KinDynModel,frameVelRepr) + % + % INPUTS: - frameVelRepr: a string with one of the following values: + % 'mixed', 'body', 'inertial'; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % Mapping for the frame velocity reperesentation + % + % 0 = INERTIAL_FIXED_REPRESENTATION + % + % 1 = BODY_FIXED_REPRESENTATION + % + % 2 = MIXED_REPRESENTATION + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + switch frameVelRepr + + case 'mixed' + + frameVelRepr_idyntree = iDynTree.MIXED_REPRESENTATION; + + case 'body' + + frameVelRepr_idyntree = iDynTree.BODY_FIXED_REPRESENTATION; + + case 'inertial' + + frameVelRepr_idyntree = iDynTree.INERTIAL_FIXED_REPRESENTATION; + + otherwise + error('[setFrameVelocityRepresentation]: frameVelRepr is not a valid string.') + end + + % set the desired frameVelRepr + ack = KinDynModel.kinDynComp.setFrameVelocityRepresentation(frameVelRepr_idyntree); + + % check for errors + if ~ack + error('[setFrameVelocityRepresentation]: unable to set the frame velocity representation.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/setJointPos.m b/bindings/matlab/+iDynTreeWrappers/setJointPos.m new file mode 100644 index 00000000000..0988e39ed52 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/setJointPos.m @@ -0,0 +1,49 @@ +function [] = setJointPos(KinDynModel,jointPos) + + % SETJOINTPOS sets the joints configuration for kino-dynamic + % computations. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: [] = setJointPos(KinDynModel,jointPos) + % + % INPUTS: - jointPos: [ndof x 1] vector representing the joints + % configuration in radians; + % - KinDynModel: a structure containing the loaded model and additional info. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % Debug input + if KinDynModel.DEBUG + + disp('[setJointPos]: debugging inputs...') + + % check joints position vector size + if length(jointPos) ~= KinDynModel.NDOF + + error('[setJointPos]: the length of jointPos is not KinDynModel.NDOF') + end + + disp('[setJointPos]: done.') + end + + % convert the joint position to a dynamic size vector + jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + + for k = 0:length(jointPos)-1 + + jointPos_iDyntree.setVal(k,jointPos(k+1)); + end + + % set the current joint positions + ack = KinDynModel.kinDynComp.setJointPos(jointPos_iDyntree); + + % check for errors + if ~ack + error('[setJointPos]: unable to set the joint positions.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/setRobotState.m b/bindings/matlab/+iDynTreeWrappers/setRobotState.m new file mode 100644 index 00000000000..0d635ce5575 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/setRobotState.m @@ -0,0 +1,230 @@ +function [] = setRobotState(varargin) + + % SETROBOTSTATE sets the system state. The system state is composed of: + % + % - joints configuration and velocity plus gravity vector + % for fixed-base systems; + % - base pose and velocity, joints configuration and + % velocity plus gravity vector for floating-base systems. + % + % The gravity vector expresses the gravity acceleration + % in the inertial frame. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % FORMAT: Floating base system: + % + % [] = setRobotState(KinDynModel,basePose,jointPos,baseVel,jointVel,gravAcc) + % + % Fixed base system: + % + % [] = setRobotState(KinDynModel,jointPos,jointVel,gravAcc) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % - basePose: [4 x 4] from base frame to world frame transform; + % - jointPos: [nDof x 1] vector representing the joints configuration + % in radians; + % - baseVel: [6 x 1] vector of base velocities [lin, ang]; + % - jointVel: [ndof x 1] vector of joints velocities; + % - gravAcc: [3 x 1] vector of the gravity acceleration in the + % inertial frame. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + KinDynModel = varargin{1}; + + % check the number of inputs + switch nargin + + case 6 + + basePose = varargin{2}; + jointPos = varargin{3}; + baseVel = varargin{4}; + jointVel = varargin{5}; + gravAcc = varargin{6}; + + % Debug inputs + if KinDynModel.DEBUG + + disp('[setRobotState]: debugging inputs...') + + % basePose must be a valid transformation matrix + if size(basePose,1) ~= 4 || size(basePose,2) ~= 4 + + error('[setRobotState]: basePose is not a 4x4 matrix.') + end + + for ii = 1:4 + + if ii < 4 + + if abs(basePose(4,ii)) > 0.0001 + + error('[setRobotState]: 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('[setRobotState]: 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('[setRobotState]: 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('[setRobotState]: baseRotation is not a valid rotation matrix.') + end + else + if abs(IdentityMatr(kk,jj)) > 0.01 + + error('[setRobotState]: baseRotation is not a valid rotation matrix.') + end + end + end + end + + % debug gravity vector size + if length(gravAcc) ~= 3 + + error('[setRobotState]: the length of gravAcc vector is not 3.') + end + + % check base velocity vector size + if length(baseVel) ~= 6 + + error('[setRobotState]: the length of baseVel is not 6.') + end + + % check joints position vector size + if length(jointPos) ~= KinDynModel.NDOF + + error('[setRobotState]: the length of jointPos is not KinDynModel.NDOF') + end + + % check joints velocity vector size + if length(jointVel) ~= KinDynModel.NDOF + + error('[setRobotState]: the length of jointVel is not KinDynModel.NDOF') + end + + disp('[setRobotState]: done.') + end + + % define the quantities required to set the floating base + baseRotation_iDyntree = iDynTree.Rotation(); + baseOrigin_iDyntree = iDynTree.Position(); + basePose_iDyntree = iDynTree.Transform(); + baseVel_iDyntree = iDynTree.Twist(); + + % 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 base velocities + for k = 0:5 + + baseVel_iDyntree.setVal(k,baseVel(k+1)); + end + + case 4 + + jointPos = varargin{2}; + jointVel = varargin{3}; + gravAcc = varargin{4}; + + % Debug inputs + if KinDynModel.DEBUG + + disp('[setRobotState]: debugging inputs...') + + % debug gravity vector + if length(gravAcc) ~= 3 + + error('[setRobotState]: the length of gravAcc vector is not 3.') + end + + % check joints position vector size + if length(jointPos) ~= KinDynModel.NDOF + + error('[setRobotState]: the length of jointPos is not KinDynModel.NDOF') + end + + % check joints velocity vector size + if length(jointVel) ~= KinDynModel.NDOF + + error('[setRobotState]: the length of jointVel is not KinDynModel.NDOF') + end + + disp('[setRobotState]: done.') + end + + otherwise + error('[setRobotState]: wrong number of inputs.') + end + + % define all the remaining quantities required for setting the system state + jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + jointVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + gravityVec_iDyntree = iDynTree.Vector3(); + + % set joints position and velocity + for k = 0:length(jointPos)-1 + + jointVel_iDyntree.setVal(k,jointVel(k+1)); + jointPos_iDyntree.setVal(k,jointPos(k+1)); + end + + % set the gravity vector + for k = 0:2 + + gravityVec_iDyntree.setVal(k,gravAcc(k+1)); + end + + % set the current robot state + switch nargin + + case 4 + + ack = KinDynModel.kinDynComp.setRobotState(jointPos_iDyntree,jointVel_iDyntree,gravityVec_iDyntree); + + case 6 + + ack = KinDynModel.kinDynComp.setRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree); + end + + % check for errors + if ~ack + error('[setRobotState]: unable to set the robot state.') + end +end diff --git a/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m new file mode 100644 index 00000000000..23d1a3c58e6 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m @@ -0,0 +1,124 @@ +function [] = updateVisualizer(Visualizer,KinDynModel,jointPos,basePose) + + % UPDATEVISUALIZER updates the iDyntree visualizer with the current + % base pose and joints position. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). + % + % FORMAT: [] = updateVisualizer(Visualizer,KinDynModel,jointPos,basePose) + % + % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. + % - Visualizer: a structure containing the visualizer and its options. + % - jointPos: [ndof x 1] vector representing the joints + % configuration in radians; + % - basePose: [4 x 4] from base frame to world frame transform. + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % Debug input + if Visualizer.DEBUG + + disp('[updateVisualizer]: debugging inputs...') + + % basePose must be a valid transformation matrix + if size(basePose,1) ~= 4 || size(basePose,2) ~= 4 + + error('[updateVisualizer]: basePose is not a 4x4 matrix.') + end + + for ii = 1:4 + + if ii < 4 + + if abs(basePose(4,ii)) > 0.0001 + + error('[updateVisualizer]: 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('[updateVisualizer]: 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('[updateVisualizer]: 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('[updateVisualizer]: baseRotation is not a valid rotation matrix.') + end + else + if abs(IdentityMatr(kk,jj)) > 0.01 + + error('[updateVisualizer]: baseRotation is not a valid rotation matrix.') + end + end + end + end + + % check joint position vector size + if length(jointPos) ~= KinDynModel.NDOF + + error('[updateVisualizer]: the length of jointPos is not KinDynModel.NDOF') + end + + disp('[updateVisualizer]: done.') + end + + % convert joints position to a dynamic size vector + jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF); + + for k = 0:length(jointPos)-1 + + jointPos_iDyntree.setVal(k,jointPos(k+1)); + end + + % define the quantities required to set the floating base pose + baseRotation_iDyntree = iDynTree.Rotation(); + baseOrigin_iDyntree = iDynTree.Position(); + basePose_iDyntree = iDynTree.Transform(); + + % set the elements 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 w_H_b_iDyntree + basePose_iDyntree.setRotation(baseRotation_iDyntree); + basePose_iDyntree.setPosition(baseOrigin_iDyntree); + + % set the current joints position and world-to-base transform + ack = Visualizer.viz.modelViz(0).setPositions(basePose_iDyntree,jointPos_iDyntree); + + % check for errors + if ~ack + error('[updateVisualizer]: unable to update the visualizer.') + end + + Visualizer.viz.draw(); +end diff --git a/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m new file mode 100644 index 00000000000..dab11e163c3 --- /dev/null +++ b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m @@ -0,0 +1,50 @@ +function [] = visualizerSetup(Visualizer,disableViewInertialFrame,lightDir,cameraPos,cameraTarget) + + % VISUALIZERSETUP modifies the visualization environment according to the + % user specifications. + % + % This matlab function wraps a functionality of the iDyntree library. + % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % + % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). + % + % FORMAT: [] = visualizerSetup(Visualizer,disableViewInertialFrame,lightDir,cameraPos,cameraTarget) + % + % INPUTS: - Visualizer: a structure containing the visualizer and its options. + % - disableViewInertialFrame: boolean for disabling the view of the + % inertial frame; + % - lightDir: [3 x 1] vector describing the light direction; + % - cameraPos: [3 x 1] vector describing the camera position; + % - cameraTarget: [3 x 1] vector describing the camera target; + % + % Author : Gabriele Nava (gabriele.nava@iit.it) + % Genova, Nov 2018 + + %% ------------Initialization---------------- + + % disable environmental features + if disableViewInertialFrame + + enviroment = Visualizer.viz.enviroment(); + ack = enviroment.setElementVisibility('root_frame',false); + + % check for errors + if ~ack + error('[visualizerSetup]: unable to disable the inertial frame view.') + end + end + + % set lights + sun = Visualizer.viz.enviroment().lightViz('sun'); + lightDir_idyntree = iDynTree.Direction(); + lightDir_idyntree.fromMatlab(lightDir); + sun.setDirection(lightDir_idyntree); + + % set camera + cam = Visualizer.viz.camera(); + cam.setPosition(iDynTree.Position(cameraPos(1),cameraPos(2),cameraPos(3))); + cam.setTarget(iDynTree.Position(cameraTarget(1),cameraTarget(2),cameraTarget(3))); + + % draw the model + Visualizer.viz.draw(); +end diff --git a/bindings/matlab/CMakeLists.txt b/bindings/matlab/CMakeLists.txt index f7f9bcf8213..5e80f3dc221 100644 --- a/bindings/matlab/CMakeLists.txt +++ b/bindings/matlab/CMakeLists.txt @@ -61,6 +61,10 @@ set(mexname iDynTreeMEX) # Directory in which the bindings are generated set(MEX_BINDINGS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/autogenerated) +# Directory containing the high-level-matlab-wrappers and its parent directory +set(HIGH_LEVEL_WRAPPERS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/+iDynTreeWrappers) +set(MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}) + # Generate SWIG wrapper if(IDYNTREE_GENERATE_MATLAB) # generate the wrapper @@ -78,7 +82,7 @@ endif() set(swig_generated_sources ${MEX_BINDINGS_SOURCE_DIR}/${sourcename}.cxx) set(swig_other_sources) -# Set the generated mex name to be iDynTreeMEX, as it the defaul one used by SWIG while generating bindings +# Set the generated mex name to be iDynTreeMEX, as it is the default one used by SWIG while generating bindings if(IDYNTREE_USES_MATLAB) find_package(Matlab REQUIRED @@ -104,6 +108,9 @@ if(IDYNTREE_USES_MATLAB) install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigMem.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR}) install(TARGETS ${mexname} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_LIBDIR}) + # Install the high-level-matlab-wrappers + install(DIRECTORY ${HIGH_LEVEL_WRAPPERS_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR}) + #On new versions of Clang, MATLAB requires C++11. #I enable it on all Clangs if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") @@ -143,6 +150,9 @@ if(IDYNTREE_USES_OCTAVE) install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigGet.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR}) install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigRef.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR}) install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigMem.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR}) + + # Install the high-level-matlab-wrappers + install(DIRECTORY ${HIGH_LEVEL_WRAPPERS_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR}) endif() # if compile tests compile also matlab/octave tests diff --git a/bindings/matlab/tests/CMakeLists.txt b/bindings/matlab/tests/CMakeLists.txt index 38c10e2d6e0..dcb44266b41 100644 --- a/bindings/matlab/tests/CMakeLists.txt +++ b/bindings/matlab/tests/CMakeLists.txt @@ -8,10 +8,10 @@ add_custom_command(TARGET copy_matlab_model_in_build PRE_BUILD if (IDYNTREE_USES_MATLAB) add_test(NAME matlab_idyntree_tests - COMMAND ${Matlab_MAIN_PROGRAM} -nodisplay -nodesktop -nojvm -r "addpath('$');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);") + COMMAND ${Matlab_MAIN_PROGRAM} -nodisplay -nodesktop -nojvm -r "addpath('$');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);") endif() if (IDYNTREE_USES_OCTAVE) add_test(NAME octave_idyntree_tests - COMMAND ${OCTAVE_EXECUTABLE} --no-gui --quiet --eval "addpath('$');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);") -endif() \ No newline at end of file + COMMAND ${OCTAVE_EXECUTABLE} --no-gui --quiet --eval "addpath('$');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);") +endif() diff --git a/bindings/matlab/tests/highLevelWrappersSmokeTest.m b/bindings/matlab/tests/highLevelWrappersSmokeTest.m new file mode 100644 index 00000000000..293bfd37315 --- /dev/null +++ b/bindings/matlab/tests/highLevelWrappersSmokeTest.m @@ -0,0 +1,155 @@ +function test_suite = highLevelWrappersSmokeTest + initTestSuite + +function test__high_level_wrappers + + % specify the list of joints that are going to be considered in the reduced model + jointList = {'torso_pitch','torso_roll','torso_yaw',... + 'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow','l_wrist_prosup', ... + 'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow','r_wrist_prosup', ... + 'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll', ... + 'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'}; + + % select the link that will be used as base link + baseLinkName = 'root_link'; + + % urdf model name (expected to be in the same folder of this test) + modelName = 'model.urdf'; + + % set the initial robot position and velocity [deg] + torso_Position = [0 0 0]; + left_arm_Position = [10 45 0 15 0]; + right_arm_Position = [10 45 0 15 0]; + right_leg_Position = [0 0 0 0 0 0]; + left_leg_Position = [0 0 0 0 0 0]; + + jointPos_init = [torso_Position';left_arm_Position';right_arm_Position';left_leg_Position';right_leg_Position']*pi/180; + jointVel_init = zeros(length(jointPos_init),1); + + % other configuration parameters + gravityAcc = [0;0;-9.81]; + frameVelRepr = 'mixed'; + frameName = 'l_sole'; + frame2Name = 'r_sole'; + frameID = 1; + frame2ID = 2; + + %% TESTS LIST + try + % test 1 + disp('1/28: testing loadReducedModel...') + KinDynModel = iDynTreeWrappers.loadReducedModel(jointList, baseLinkName, './', modelName, true); + + % test 2 + disp('2/28: testing setRobotState...') + iDynTreeWrappers.setRobotState(KinDynModel, jointPos_init, jointVel_init, gravityAcc) + + % test 3 + disp('3/28: testing setJointPos...') + iDynTreeWrappers.setJointPos(KinDynModel, jointPos_init) + + % test 4 + disp('4/28: testing setFrameVelocityRepresentation...') + iDynTreeWrappers.setFrameVelocityRepresentation(KinDynModel, frameVelRepr) + + % test 5 + disp('5/28: testing setFloatingBase...') + iDynTreeWrappers.setFloatingBase(KinDynModel, baseLinkName) + + % test 6 + disp('6/28: testing getJointPos...') + iDynTreeWrappers.getJointPos(KinDynModel); + + % test 7 + disp('7/28: testing getJointVel...') + iDynTreeWrappers.getJointVel(KinDynModel); + + % test 8 + disp('8/28: testing getCentroidalTotalMomentum...') + iDynTreeWrappers.getCentroidalTotalMomentum(KinDynModel); + + % test 9 + disp('9/28: testing getNrOfDegreesOfFreedom...') + iDynTreeWrappers.getNrOfDegreesOfFreedom(KinDynModel); + + % test 10 + disp('10/28: testing getCenterOfMassPosition...') + iDynTreeWrappers.getCenterOfMassPosition(KinDynModel); + + % test 11 + disp('11/28: testing getBaseTwist...') + iDynTreeWrappers.getBaseTwist(KinDynModel); + + % test 12 + disp('12/28: testing generalizedBiasForces...') + iDynTreeWrappers.generalizedBiasForces(KinDynModel); + + % test 13 + disp('13/28: testing generalizedGravityForces...') + iDynTreeWrappers.generalizedGravityForces(KinDynModel); + + % test 14 + disp('14/28: testing getWorldBaseTransform...') + iDynTreeWrappers.getWorldBaseTransform(KinDynModel); + + % test 15 + disp('15/28: testing getModelVel...') + iDynTreeWrappers.getModelVel(KinDynModel); + + % test 16 + disp('16/28: testing getFrameVelocityRepresentation...') + iDynTreeWrappers.getFrameVelocityRepresentation(KinDynModel); + + % test 17 + disp('17/28: testing getFloatingBase...') + iDynTreeWrappers.getFloatingBase(KinDynModel); + + % test 18 + disp('18/28: testing getFrameIndex...') + iDynTreeWrappers.getFrameIndex(KinDynModel, frameName); + + % test 19 + disp('19/28: testing getFrameName...') + iDynTreeWrappers.getFrameName(KinDynModel, frameID); + + % test 20 + disp('20/28: testing getWorldTransform...') + iDynTreeWrappers.getWorldTransform(KinDynModel, frameName); + + % test 21 + disp('21/28: testing getRelativeTransform...') + iDynTreeWrappers.getRelativeTransform(KinDynModel, frameName, frame2Name); + + % test 22 + disp('22/28: testing getRelativeJacobian...') + iDynTreeWrappers.getRelativeJacobian(KinDynModel, frameID, frame2ID); + + % test 23 + disp('23/28: testing getFreeFloatingMassMatrix...') + iDynTreeWrappers.getFreeFloatingMassMatrix(KinDynModel); + + % test 24 + disp('24/28: testing getRobotState...') + iDynTreeWrappers.getRobotState(KinDynModel); + + % test 25 + disp('25/28: testing getFrameBiasAcc...') + iDynTreeWrappers.getFrameBiasAcc(KinDynModel, frameName); + + % test 26 + disp('26/28: testing getCenterOfMassJacobian...') + iDynTreeWrappers.getCenterOfMassJacobian(KinDynModel); + + % test 27 + disp('27/28: testing getCenterOfMassVelocity...') + iDynTreeWrappers.getCenterOfMassVelocity(KinDynModel); + + % test 28 + disp('28/28: testing getFrameFreeFloatingJacobian...') + iDynTreeWrappers.getFrameFreeFloatingJacobian(KinDynModel, frameName); + + catch ME + disp('[High Level Wappers]: test failed. Message: ') + disp(ME) + assertTrue(false); + end From 4f69b3c808bc8ffcb92a161393c1e0eb3bf17224 Mon Sep 17 00:00:00 2001 From: Gabriele Date: Mon, 15 Apr 2019 14:04:16 +0200 Subject: [PATCH 2/6] added documentation on the high level wrappers --- README.md | 24 +++++++++++++++++++++ bindings/matlab/+iDynTreeWrappers/README.md | 6 +++--- 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index a2edcb8b449..d14c6783f88 100644 --- a/README.md +++ b/README.md @@ -167,6 +167,30 @@ 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. In fact for the 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` method: + +``` + 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 high-level wrappers is therefore 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). **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 | diff --git a/bindings/matlab/+iDynTreeWrappers/README.md b/bindings/matlab/+iDynTreeWrappers/README.md index c2b50532dff..ea4aff67a16 100644 --- a/bindings/matlab/+iDynTreeWrappers/README.md +++ b/bindings/matlab/+iDynTreeWrappers/README.md @@ -1,10 +1,10 @@ # idyntree high-level-wrappers -A collection of Matlab 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. +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 user, it may be sometimes counterintuitive to use C++ based formalism inside Matlab. Furthermore, there are common iDyntree functions that require several lines of code in order to be used correctly. E.g. see the `getRobotState` function: +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(); @@ -27,7 +27,7 @@ For a Matlab user, it may be sometimes counterintuitive to use C++ based formali 0, 0, 0, 1]; ``` -The purpose of the wrapper is therefore to provide a simpler and easy-to-use interface for Matlab users who wants to use iDyntree inside Matlab, 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). +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 From 7e133f38143eaebbfe12d3909b9e686c86b71038 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Mon, 15 Apr 2019 14:17:16 +0200 Subject: [PATCH 3/6] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index d14c6783f88..286e19448c7 100644 --- a/README.md +++ b/README.md @@ -190,7 +190,7 @@ They are a collection of Matlab/Octave functions that wraps the functionalities 0, 0, 0, 1]; ``` -The purpose of the high-level wrappers is therefore 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). **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`. +The purpose of the high-level wrappers is therefore 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 | From bdb902af1123cfbc473bfe768e6dcbf5ee9615b6 Mon Sep 17 00:00:00 2001 From: Gabriele Date: Tue, 16 Apr 2019 15:56:01 +0200 Subject: [PATCH 4/6] README and documentation cleanup --- README.md | 24 ++----------------- .../+iDynTreeWrappers/generalizedBiasForces.m | 9 ++++--- .../generalizedGravityForces.m | 9 ++++--- .../matlab/+iDynTreeWrappers/getBaseTwist.m | 7 ++++-- .../getCenterOfMassJacobian.m | 9 ++++--- .../getCenterOfMassPosition.m | 7 ++++-- .../getCenterOfMassVelocity.m | 7 ++++-- .../getCentroidalTotalMomentum.m | 7 ++++-- .../+iDynTreeWrappers/getFloatingBase.m | 7 ++++-- .../+iDynTreeWrappers/getFrameBiasAcc.m | 9 ++++--- .../getFrameFreeFloatingJacobian.m | 9 ++++--- .../matlab/+iDynTreeWrappers/getFrameIndex.m | 7 ++++-- .../matlab/+iDynTreeWrappers/getFrameName.m | 7 ++++-- .../getFrameVelocityRepresentation.m | 7 ++++-- .../getFreeFloatingMassMatrix.m | 9 ++++--- .../matlab/+iDynTreeWrappers/getJointPos.m | 7 ++++-- .../matlab/+iDynTreeWrappers/getJointVel.m | 7 ++++-- .../matlab/+iDynTreeWrappers/getModelVel.m | 9 ++++--- .../getNrOfDegreesOfFreedom.m | 7 ++++-- .../+iDynTreeWrappers/getRelativeJacobian.m | 7 ++++-- .../+iDynTreeWrappers/getRelativeTransform.m | 7 ++++-- .../matlab/+iDynTreeWrappers/getRobotState.m | 7 ++++-- .../+iDynTreeWrappers/getWorldBaseTransform.m | 7 ++++-- .../+iDynTreeWrappers/getWorldTransform.m | 7 ++++-- .../+iDynTreeWrappers/initializeVisualizer.m | 7 ++++-- .../+iDynTreeWrappers/loadReducedModel.m | 9 ++++--- .../+iDynTreeWrappers/setFloatingBase.m | 7 ++++-- .../setFrameVelocityRepresentation.m | 7 ++++-- .../matlab/+iDynTreeWrappers/setJointPos.m | 7 ++++-- .../matlab/+iDynTreeWrappers/setRobotState.m | 7 ++++-- .../+iDynTreeWrappers/updateVisualizer.m | 7 ++++-- .../+iDynTreeWrappers/visualizerSetup.m | 7 ++++-- 32 files changed, 165 insertions(+), 92 deletions(-) diff --git a/README.md b/README.md index 286e19448c7..58d4526ef6e 100644 --- a/README.md +++ b/README.md @@ -168,29 +168,9 @@ version of Swig with Matlab support from https://github.com/robotology-dependenc 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. In fact for the 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` method: +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). -``` - 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 high-level wrappers is therefore 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`. +**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 | diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m index e68da93cdeb..f19d619c305 100644 --- a/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m +++ b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m @@ -4,16 +4,19 @@ % the reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % 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: [ndof+6 x 1] generalized bias accelerations. + % OUTPUTS: - h: [6+ndof x 1] generalized bias accelerations. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m index 25a23e21dbb..eb0fadc58ef 100644 --- a/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m +++ b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m @@ -4,16 +4,19 @@ % given the reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % 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: [nDof+6 x 1] generalized gravity forces. + % OUTPUTS: - g: [6+ndof x 1] generalized gravity forces. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m index 34836d5a3fe..cd6e4a66d38 100644 --- a/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m +++ b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m @@ -3,7 +3,7 @@ % 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: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: baseVel = getBaseTwist(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - baseVel: [6 x 1] vector of base linear and angular velocity; % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m index e046320e2a6..16a1c5df24d 100644 --- a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m @@ -3,16 +3,19 @@ % GETCENTEROFMASSJACOBIAN retrieves the CoM jacobian. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % 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 ndof+6] CoM free floating Jacobian. + % OUTPUTS: - J_CoM: [3 x 6+ndof] CoM free floating Jacobian. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m index b82dce36760..2268fedfe8e 100644 --- a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m @@ -3,7 +3,7 @@ % GETCENTEROFMASSPOSITION retrieves the CoM position in world coordinates. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: posCoM = getCenterOfMassPosition(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - posCoM: [3 x 1] CoM position w.r.t. world frame. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m index 8e9bfc808b7..f7fe44a3956 100644 --- a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m +++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m @@ -3,7 +3,7 @@ % GETCENTEROFMASSVELOCITY retrieves the CoM velocity in world coordinates. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: velCoM = getCenterOfMassVelocity(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - velCoM: [3 x 1] CoM velocity w.r.t. world frame. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m index 2c1e1b35925..f0a0e73783a 100644 --- a/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m +++ b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m @@ -5,7 +5,7 @@ % on the 'FrameVelocityReperesentation' settings. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: totalMomentum = getCentroidalTotalMomentum(KinDynModel) % @@ -14,7 +14,10 @@ % OUTPUTS: - totalMomentum: [6 x 1] vector of linear and angular momentum. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m index a2107ecb75c..f67a974fd02 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m +++ b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m @@ -4,7 +4,7 @@ % reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: baseLinkName = getFloatingBase(KinDynModel) % @@ -13,7 +13,10 @@ % OUTPUTS: - baseLinkName: name of the base link. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m index 60c1de7340a..ef2c18dd694 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m +++ b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m @@ -3,7 +3,7 @@ % GETFRAMEBIASACC gets the bias accelerations of a specified frame. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: JDot_nu_frame = getFrameBiasAcc(KinDynModel,frameName) % @@ -11,10 +11,13 @@ % - frameName: a string that specifies the frame w.r.t. compute the % bias accelerations, or the associated ID; % - % OUTPUTS: - JDot_nu_frame: [6 x ndof+6] frame bias accelerations. + % OUTPUTS: - JDot_nu_frame: [6 x 6+ndof] frame bias accelerations. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m index 5601dca420a..4f6ce3636c5 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m +++ b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m @@ -4,7 +4,7 @@ % specified frame. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: J_frame = getFrameFreeFloatingJacobian(KinDynModel,frameName) % @@ -12,10 +12,13 @@ % - frameName: a string that specifies the frame w.r.t. compute the % jacobian matrix, or the associated ID; % - % OUTPUTS: - J_frame: [6 x ndof+6] frame free floating Jacobian. + % OUTPUTS: - J_frame: [6 x 6+ndof] frame free floating Jacobian. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m index 91043b9042a..3eb11a769a0 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m +++ b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m @@ -3,7 +3,7 @@ % GETFRAMEINDEX gets the index corresponding to a given frame name. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: frameID = getFrameIndex(KinDynModel,frameName) % @@ -13,7 +13,10 @@ % OUTPUTS: - frameID: the ID associated to the given frame name. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameName.m b/bindings/matlab/+iDynTreeWrappers/getFrameName.m index c0640dec18b..74aa02d0245 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFrameName.m +++ b/bindings/matlab/+iDynTreeWrappers/getFrameName.m @@ -3,7 +3,7 @@ % GETFRAMENAME gets the name corresponding to a given frame index. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: frameName = getFrameName(KinDynModel,frameID) % @@ -13,7 +13,10 @@ % OUTPUTS: - frameName: a string specifying a valid frame name. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m index 9b5c780ce9a..918b167cf16 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m +++ b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m @@ -4,7 +4,7 @@ % representation. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: frameVelRepr = getFrameVelocityRepresentation(KinDynModel) % @@ -22,7 +22,10 @@ % 2 = MIXED_REPRESENTATION % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m index fb262324f38..3eb975d359d 100644 --- a/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m +++ b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m @@ -3,16 +3,19 @@ % GETFREEFLOATINGMASSMATRIX retrieves the free floating mass matrix. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: M = getFreeFloatingMassMatrix(KinDynModel) % % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. % - % OUTPUTS: - M: [ndof+6 x ndof+6] free floating mass matrix. + % OUTPUTS: - M: [6+ndof x 6+ndof] free floating mass matrix. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getJointPos.m b/bindings/matlab/+iDynTreeWrappers/getJointPos.m index c42fa3b825b..d4139be3caf 100644 --- a/bindings/matlab/+iDynTreeWrappers/getJointPos.m +++ b/bindings/matlab/+iDynTreeWrappers/getJointPos.m @@ -3,7 +3,7 @@ % GETJOINTPOS retrieves the joints configuration from the reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: jointPos = getJointPos(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - jointPos: [ndof x 1] vector of joint positions. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getJointVel.m b/bindings/matlab/+iDynTreeWrappers/getJointVel.m index 48b8c01fd53..3dc0243315a 100644 --- a/bindings/matlab/+iDynTreeWrappers/getJointVel.m +++ b/bindings/matlab/+iDynTreeWrappers/getJointVel.m @@ -3,7 +3,7 @@ % GETJOINTVEL retrieves joint velocities from the reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: jointVel = getJointVel(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - jointVel: [ndof x 1] vector of joint velocities. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getModelVel.m b/bindings/matlab/+iDynTreeWrappers/getModelVel.m index ba2b1aafbff..487a16a79fb 100644 --- a/bindings/matlab/+iDynTreeWrappers/getModelVel.m +++ b/bindings/matlab/+iDynTreeWrappers/getModelVel.m @@ -4,16 +4,19 @@ % reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: stateVel = getModelVel(KinDynModel) % % INPUTS: - KinDynModel: a structure containing the loaded model and additional info. % - % OUTPUTS: - stateVel: [ndof+6 x 1] vector of joints and base velocities. + % OUTPUTS: - stateVel: [6+ndof x 1] vector of joints and base velocities. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m index 8c62c6231bd..a285b59bd93 100644 --- a/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m +++ b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m @@ -3,7 +3,7 @@ % GETNROFDEGREESOFFREEDOM gets the dimension of the joint space. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: nDof = getNrOfDegreesOfFreedom(KinDynModel) % @@ -12,7 +12,10 @@ % OUTPUTS: - nDof: number of DoFs of the system. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m index bd1e3058d2a..0d798adc74e 100644 --- a/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m +++ b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m @@ -5,7 +5,7 @@ % frameRef, to the joint velocity. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: J_frameVel = getRelativeJacobian(KinDynModel,frameVelID,frameRefID) % @@ -18,7 +18,10 @@ % OUTPUTS: - J_frameVel: [6 x ndof] frameVel Jacobian. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m index e4d1dc217f6..63e847ab88f 100644 --- a/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m +++ b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m @@ -4,7 +4,7 @@ % frames. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: frame1_H_frame2 = getRelativeTransform(KinDynModel,frame1Name,frame2Name) % @@ -17,7 +17,10 @@ % OUTPUTS: - frame1_H_frame2: [4 x 4] from frame2 to frame1 transformation matrix. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getRobotState.m b/bindings/matlab/+iDynTreeWrappers/getRobotState.m index 712380658c2..23f6b1d28a6 100644 --- a/bindings/matlab/+iDynTreeWrappers/getRobotState.m +++ b/bindings/matlab/+iDynTreeWrappers/getRobotState.m @@ -3,7 +3,7 @@ % GETROBOTSTATE gets the floating base system state from the reduced model. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: [basePose,jointPos,baseVel,jointVel] = getRobotState(KinDynModel) % @@ -15,7 +15,10 @@ % - jointVel: [ndof x 1] vector of joint velocities. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m index 17611538471..c762f168a83 100644 --- a/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m +++ b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m @@ -4,7 +4,7 @@ % and the inertial reference frame. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: basePose = getWorldBaseTransform(KinDynModel) % @@ -13,7 +13,10 @@ % OUTPUTS: - basePose: [4 x 4] from base to world transformation matrix. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m index 48002e3e9b7..c3a1a745929 100644 --- a/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m +++ b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m @@ -4,7 +4,7 @@ % frame and the inertial reference frame. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: w_H_frame = getWorldTransform(KinDynModel,frameName) % @@ -15,7 +15,10 @@ % OUTPUTS: - w_H_frame: [4 x 4] from frame to world transformation matrix. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m index e69564dacbb..08c36a533b2 100644 --- a/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m +++ b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m @@ -4,7 +4,7 @@ % model into the visualizer. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). % @@ -16,7 +16,10 @@ % OUTPUTS: - Visualizer: a structure containing the visualizer and its options. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m index ad88ca7b549..c21b3c8622d 100644 --- a/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m +++ b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m @@ -3,7 +3,7 @@ % LOADREDUCEDMODEL loads the urdf model of the rigid multi-body system. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: KinDynModel = loadReducedModel(jointList,baseLinkName,modelPath,modelName,debugMode) % @@ -18,8 +18,11 @@ % OUTPUTS: - KinDynModel: a structure containing the loaded model and additional info. % % Author: Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 - + % + % 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---------------- disp(['[loadReducedModel]: loading the following model: ',[modelPath,modelName]]); diff --git a/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m index d75408457fa..783f44e0b73 100644 --- a/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m +++ b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m @@ -3,7 +3,7 @@ % SETFLOATINGBASE sets the link that is used as floating base. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: [] = setFloatingBase(KinDynModel,floatBaseLinkName) % @@ -12,7 +12,10 @@ % - KinDynModel: a structure containing the loaded model and additional info. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m index 93e6f0df2b9..b1c24bf7e94 100644 --- a/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m +++ b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m @@ -3,7 +3,7 @@ % SETFRAMEVELOCITYREPRESENTATION sets the frame velocity representation. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: [] = setFrameVelocityRepresentation(KinDynModel,frameVelRepr) % @@ -20,7 +20,10 @@ % 2 = MIXED_REPRESENTATION % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/setJointPos.m b/bindings/matlab/+iDynTreeWrappers/setJointPos.m index 0988e39ed52..1afa351ac68 100644 --- a/bindings/matlab/+iDynTreeWrappers/setJointPos.m +++ b/bindings/matlab/+iDynTreeWrappers/setJointPos.m @@ -4,7 +4,7 @@ % computations. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: [] = setJointPos(KinDynModel,jointPos) % @@ -13,7 +13,10 @@ % - KinDynModel: a structure containing the loaded model and additional info. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/setRobotState.m b/bindings/matlab/+iDynTreeWrappers/setRobotState.m index 0d635ce5575..4e15dd90873 100644 --- a/bindings/matlab/+iDynTreeWrappers/setRobotState.m +++ b/bindings/matlab/+iDynTreeWrappers/setRobotState.m @@ -11,7 +11,7 @@ % in the inertial frame. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % FORMAT: Floating base system: % @@ -31,7 +31,10 @@ % inertial frame. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m index 23d1a3c58e6..fd6af5478e7 100644 --- a/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m +++ b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m @@ -4,7 +4,7 @@ % base pose and joints position. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). % @@ -17,7 +17,10 @@ % - basePose: [4 x 4] from base frame to world frame transform. % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- diff --git a/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m index dab11e163c3..12633657af0 100644 --- a/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m +++ b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m @@ -4,7 +4,7 @@ % user specifications. % % This matlab function wraps a functionality of the iDyntree library. - % For further info see also: http://wiki.icub.org/codyco/dox/html/idyntree/html/ + % For further info see also: https://github.com/robotology/idyntree % % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON). % @@ -18,7 +18,10 @@ % - cameraTarget: [3 x 1] vector describing the camera target; % % Author : Gabriele Nava (gabriele.nava@iit.it) - % Genova, Nov 2018 + % + % 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---------------- From c903705253461f354733449c2ab85de4389cc9f0 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 16 Apr 2019 16:20:47 +0200 Subject: [PATCH 5/6] Update v0_12.md --- doc/releases/v0_12.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md index 8c1fd314669..0b081546a6a 100644 --- a/doc/releases/v0_12.md +++ b/doc/releases/v0_12.md @@ -42,6 +42,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput * Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522) * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522) +#### `bindings` +* Added high-level Matlab/Octave wrappers of the iDyntree bindings. Related PR: (https://github.com/robotology/idyntree/pull/530) + ### `core` * Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522) * Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522) From 3a34f3d6943981730826549cff2e21e7c903b841 Mon Sep 17 00:00:00 2001 From: Gabriele Nava Date: Tue, 16 Apr 2019 16:45:43 +0200 Subject: [PATCH 6/6] Update v0_12.md --- doc/releases/v0_12.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md index 0b081546a6a..e6183cda32d 100644 --- a/doc/releases/v0_12.md +++ b/doc/releases/v0_12.md @@ -43,7 +43,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522) #### `bindings` -* Added high-level Matlab/Octave wrappers of the iDyntree bindings. Related PR: (https://github.com/robotology/idyntree/pull/530) +* Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530) ### `core` * Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)