example matlab bindings

gistfile1.txt

clear variables
close all
clc

DEBUG = false;

% model name and path
Config.Model.modelName    = 'model.urdf';
Config.Model.baseLinkName = 'l_sole';
Config.Model.modelPath    = '/Users/lorenzorapetti/Software/robotology-superbuild/build/install/share/iCub/robots/iCubGazeboV2_5/';

% specify the list of joints that are going to be considered in the reduced model
Config.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'};

% robot configuration
torso_Position     = [-3  0  0];                 
left_arm_Position  = [-35.97 29.97 0.006  50 0];           
right_arm_Position = [-35.97 29.97 0.006  50 0];                
left_leg_Position  = [ 12 5 0 -10 -1.6 -5];
right_leg_Position = [ 12 5 0 -10 -1.6 -5]; 

Config.initJointPosOpt.jointPos_init = [torso_Position';left_arm_Position';right_arm_Position';left_leg_Position';right_leg_Position']*pi/180;
Config.initJointPosOpt.jointVel_init = zeros(length(Config.initJointPosOpt.jointPos_init),1);
Config.initJointPosOpt.gravityAcc    = [0;0;-9.81];

% initial base pose w.r.t. the world frame
Config.initJointPosOpt.w_R_b_init = [ 1  0  0;
                                      0  1  0;
                                      0  0  1];
Config.initJointPosOpt.w_H_b_init = [Config.initJointPosOpt.w_R_b_init , [0;0;0.0];
                                         0        0        0                 1   ];
                                     
                                     
%%%%%%%%%% iDynTree %%%%%%%%%%%%%
% load the reduced model
KinDynModel  = iDynTreeWrappers.loadReducedModel(Config.Model.jointList, Config.Model.baseLinkName, Config.Model.modelPath, ...
                                                 Config.Model.modelName, DEBUG); 

% set the initial robot state 
iDynTreeWrappers.setRobotState(KinDynModel, Config.initJointPosOpt.w_H_b_init, Config.initJointPosOpt.jointPos_init, ...
                               zeros(6,1), Config.initJointPosOpt.jointVel_init, Config.initJointPosOpt.gravityAcc);
                 
                           
                           
                           
                           
% check the pose of the choosen baseLink  -> it is different from
% w_H_b_init
iDynTreeWrappers.getWorldTransform(KinDynModel,Config.Model.baseLinkName)

% check the pose of the root_link -> it is w_H_b_init
iDynTreeWrappers.getWorldTransform(KinDynModel,'root_link')