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 +111 -2
4 changed files with 111 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit f4f045a161 - Show all commits

2
Identification_main.m
View File

@ -12,7 +12,7 @@ opt.Isreal = true;
robot = get_robot_R1000(file,opt);
robot = get_Kinematics(robot, opt);
% R1000_Dynamics_num;
R1000_Dynamics_num;
% R1000_Dynamics;
% getGravityForce;
robot = get_velocity(robot, opt);

2
get_regressor.m
View File

@ -43,6 +43,8 @@ switch opt.LD_method
% 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);
end
% construct matrix U, size: [6*n,10*n]
% U_ = sym(zeros([6*ndof,10*ndof]));

106
get_velocity.asv Normal file
View File

@ -0,0 +1,106 @@
function robot = get_velocity(robot, opt)
switch opt.KM_method
case 'SCREW'
Mlist_CG = robot.kine.Mlist_CG;
% Get general mass matrix
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
Slist=robot.slist;
Glist=[];
for i = 1:robot.ndof
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
Glist = cat(3, Glist, Gb);
% ?
mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
robot.pi(2:4,i) = mc;
end
if opt.Isreal
% init q
q = robot.theta';
qd = robot.dtheta';
qdd = robot.ddtheta';
[V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
else
% init q
q = robot.theta;
qd = robot.dtheta;
qdd = robot.ddtheta;
[V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
end
% FIXME: twist is not equal to velocity
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)];
case 'SDH'
case 'MDH'
switch opt.Vel_method
case 'Direct'
Z = [0,0,1]';
w0 = zeros(3,1); dw0 = zeros(3,1);
% dv0 = robot.gravity;
v0 = zeros(3,1);dv0 = zeros(3,1);
link_type = robot.link_type;
% init q
q = robot.theta;
qd = robot.dtheta;
qdd = robot.ddtheta;
for i = 1:robot.ndof
switch link_type(i)
case 'R'
%Do nothing
case 'P'
q(i) = robot.d(i);
qd(i) = robot.vd(i);
qdd(i) = robot.accd(i);
end
end
R = robot.R;
P = robot.t;
% 1-n外推公式 参考robotics toolbox
%第一关节
switch link_type(1)
case 'R' %revolute
w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
case 'P' %prismatic
w(:,1) = R(:,:,1)' * w0;
v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
dw(:,1) = R(:,:,1)' * dw0;
dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
end
%后面n-1关节
for i = 1:robot.ndof
switch link_type(i)
case 'R' %revolute
w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(i+1) * Z ;
v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), qd(i+1) * Z)+ qdd(i+1) * Z;
dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
case 'P' %prismatic
w(:,i+1) = R(:,:,i+1)' * w(:,i);
v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
end
end
robot.vel.w = w;
robot.vel.v = v;
robot.vel.dw = dw;
robot.vel.dv = dv;
otherwise
disp('Bad opt.Vel_method!')
return;
end
otherwise
disp('Bad opt.KM_method!')
return;
end

3
get_velocity.m
View File

@ -31,11 +31,12 @@ switch opt.KM_method
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
end
% FIXME: twist is not equal to velocity
% FIXME: Need to get the velocity represent at base frame
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=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
case 'SDH'
case 'MDH'
switch opt.Vel_method