Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix bug when match regressor and dynamics

This commit is contained in:
cosmic_power 2024-10-20 01:39:18 +08:00
parent 9cc533fe17
commit e2e7406f4a
5 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Identification_main.m
View File

@ -15,8 +15,8 @@ robot = get_Kinematics(robot, opt);
R1000_Dynamics_num; R1000_Dynamics_num;
% R1000_Dynamics; % R1000_Dynamics;
% getGravityForce; % getGravityForce;
robot = get_velocity(robot, opt); % robot = get_velocity(robot, opt);
robot = get_regressor(robot,opt); % robot = get_regressor(robot,opt);
% symbol matched % symbol matched
% verify_regressor_R1000; % verify_regressor_R1000;
% robot = get_baseParams(robot, opt); % robot = get_baseParams(robot, opt);

2
R1000_Dynamics_num.m
View File

@ -13,7 +13,7 @@ link_mass = robot.m;
com_pos = robot.com; com_pos = robot.com;
link_inertia = robot.I; link_inertia = robot.I;
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; thetalist = [zero_;q_J;zero_;q_J;zero_;q_J;zero_;zero_;zero_]';
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';

2
get_regressor.m
View File

@ -43,7 +43,7 @@ switch opt.LD_method
% hack: because we only know the com in the world frame % hack: because we only know the com in the world frame
% the inv of rotation is beacause we want to the com in % the inv of rotation is beacause we want to the com in
% our defined frame. R*com is the wrong result % our defined frame. R*com is the wrong result
robot.pi(2:4,i) = robot.TW(1:3,1:3,i)'*robot.pi(2:4,i); robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i);
% size of matrix A is 6*10, need to -1 % size of matrix A is 6*10, need to -1
robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ... robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)]; zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];

2
get_robot_R1000.m
View File

@ -152,7 +152,7 @@ switch opt.robot_def
[0;-1;0;cross(-[0;-1;0],co(:,8))]... [0;-1;0;cross(-[0;-1;0],co(:,8))]...
[0;0;0;1;0;0]]; [0;0;0;1;0;0]];
robot.theta = zeros([ndof,1]); robot.theta = zeros([ndof,1]);
% robot.theta(2) = 1; % robot.theta(2) = pi/4;
% robot.theta(4) = 1; % robot.theta(4) = 1;
% robot.theta(6) = 1; % robot.theta(6) = 1;
otherwise otherwise

18
untitled3.m Normal file
View File

@ -0,0 +1,18 @@
R = robot.kine.R;
P = robot.kine.t;
F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1
FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
FF2 = F_Simpack(end-1,:,1)'+FF1
%%
F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
%%
F_Simpack(end,:,1)
F_Simpack(end-2,:,1)
%%
robot_pi_vecoter = reshape(robot.pi,[90,1]);
F = robot.regressor.U*robot_pi_vecoter;
FF = reshape(F,[6,9])
F_Simpack(:,:,1)