%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% test mass calculation function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% dynamicBasicInfo = get_PSA_robot_arm_dynamic_basic_info();
dynamicBasicInfo = get_robot_arm_dynamic_basic_info();
% theta = zeros(9,1);
theta = [0;-pi/3;2*pi/3;-pi/3;pi/2;-pi/2;0;0;0];
rotInfo = calculateArmRot(dynamicBasicInfo,theta);
jaco = calculateJacobian(dynamicBasicInfo,...
                         rotInfo,...
                         theta);
[massMatrix,~,gravityForce] = calculateMassMatrix(dynamicBasicInfo,...
                                                  0,...
                                                  jaco);