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

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +18 -10
5 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 9cc533fe17 - Show all commits

4
Identification_main.m
View File

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

2
R1000_Dynamics_num.m
View File

@ -13,7 +13,7 @@ link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
thetalist = [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_]';

15
get_regressor.m
View File

@ -40,11 +40,15 @@ switch opt.LD_method
dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
w_l(:,:,i) = vec2linearSymMat(w(:,i));
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
% hack: because we only know the com in the world frame
% the inv of rotation is beacause we want to the com in
% 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);
% 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); ...
zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
%? match screw method
robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
% robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
end
% construct matrix U, size: [6*n,10*n]
% U_ = sym(zeros([6*ndof,10*ndof]));
@ -67,10 +71,11 @@ switch opt.LD_method
for i = 1:ndof
delta_(i,6*i) = 1;
% %FIXME: use link type
% if(i==ndof)
% delta_(i,6*i) = 0;
% end
%FIXME: use link type, hack now
if(i==ndof)
delta_(i,6*i) = 0;
delta_(ndof,6*(ndof-1)+3) = 1;
end
end
robot.regressor.K = delta_*robot.regressor.U;
if(opt.debug)

3
get_robot_R1000.m
View File

@ -152,6 +152,9 @@ switch opt.robot_def
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
[0;0;0;1;0;0]];
robot.theta = zeros([ndof,1]);
% robot.theta(2) = 1;
% robot.theta(4) = 1;
% robot.theta(6) = 1;
otherwise
disp('Bad opt.KM_method!')
return;

4
get_velocity.m
View File

@ -35,8 +35,8 @@ switch opt.KM_method
robot.vel.w = V(1:3,:);
robot.vel.v = V(4:6,:);
robot.vel.dw = Vd(1:3,:);
% robot.vel.dv = Vd(4:6,:);
robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
robot.vel.dv = Vd(4:6,2:end);
% robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
case 'SDH'
case 'MDH'
switch opt.Vel_method