IRDYn/complie/R1000 DVT GravityModel V1/calculateGravityModel.m

function GravityTorque  = calculateGravityModel(thetaMea,permutationMatrix,pib_MLS)
% Example:
% thetalist = [0;0;0;0;0;pi/2;0;0;0];
% calculateGravityModel(thetalist,permutationMatrix,pib_MLS)

% code start
opt.robot_def = 'direct';
opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = false;
opt.robotName = 'R1000_DVT';
opt.reGenerate = false;
opt.Isreal = true;

% get_robot_func = sprintf('get_robot_%s',opt.robotName);
% robot = feval(get_robot_func,thetaMea,zeros(size(thetaMea)),zeros(size(thetaMea)),opt);
% get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
% robot = feval(get_Kinematics_func,robot,opt);
robot = get_robot_R1000_DVT(thetaMea,zeros(size(thetaMea)),zeros(size(thetaMea)),opt);
robot = get_Kinematics_R1000_DVT(robot,opt);
% FIXME
robot = get_velocity(robot, opt);
robot = get_regressor(robot,opt);
% get base params
% hack numberOfBaseParameters =15
Yb = robot.regressor.K*permutationMatrix(:,1:15);
% % get torque
GravityTorque = Yb*pib_MLS;