forked from robotology/idyntree
-
Notifications
You must be signed in to change notification settings - Fork 0
/
getCenterOfMassPosition.m
26 lines (22 loc) · 954 Bytes
/
getCenterOfMassPosition.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
function posCoM = getCenterOfMassPosition(KinDynModel)
% GETCENTEROFMASSPOSITION retrieves the CoM position in world coordinates.
%
% This matlab function wraps a functionality of the iDyntree library.
% For further info see also: https://github.com/robotology/idyntree
%
% FORMAT: posCoM = getCenterOfMassPosition(KinDynModel)
%
% INPUTS: - KinDynModel: a structure containing the loaded model and additional info.
%
% OUTPUTS: - posCoM: [3 x 1] CoM position w.r.t. world frame.
%
% Author : Gabriele Nava (gabriele.nava@iit.it)
%
% SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
% SPDX-License-Identifier: BSD-3-Clause
%% ------------Initialization----------------
% get the CoM position
posCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassPosition();
% covert to matlab
posCoM = posCoM_iDyntree.toMatlab;
end