29 lines
1020 B
Matlab
29 lines
1020 B
Matlab
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; |