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

Compare commits

base: Robotics:6561c775dc8f16cc8cf361229924486d415f47e0
Branches Tags
Robotics:main
Robotics:feature/calulated-col-force
Robotics:test/grab-to-unstow
Robotics:feature/Make-GC-Compilable
Robotics:feature/R1000-identification
Robotics:test/R1000-test
Robotics:feature/add-regressor
Robotics:feature/add-kinematics
...
compare: Robotics:3833315adb109e2d8742fa142cd7a8c9e4f3e02b
Branches Tags
Robotics:feature/calulated-col-force
Robotics:main
Robotics:test/grab-to-unstow
Robotics:feature/Make-GC-Compilable
Robotics:feature/R1000-identification
Robotics:test/R1000-test
Robotics:feature/add-regressor
Robotics:feature/add-kinematics

2 Commits
6561c775dc ... 3833315adb

Author SHA1 Message Date
cosmic_power 3833315adb still bugs 2024-10-17 08:14:52 +08:00
cosmic_power f4f045a161 gravity force match, expect the MPK 2024-10-17 01:03:10 +08:00
8 changed files with 3539 additions and 3328 deletions
Show Stats Expand all files Collapse all files

6
Identification_main.m
View File

@ -12,11 +12,11 @@ opt.Isreal = true;
robot = get_robot_R1000(file,opt); robot = get_robot_R1000(file,opt);
robot = get_Kinematics(robot, opt); 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);

3
R1000_Dynamics_num.m
View File

@ -7,12 +7,13 @@ q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time); qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J)); zero_ = zeros(1,length(q_J));
q_J = ones(1,length(q_J));
% Dynamics parameters % Dynamics parameters
link_mass = robot.m; 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_;zero_;zero_;q_J;zero_;zero_;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_]';

6775
autogen/standard_regressor_R1000.m
View File

File diff suppressed because it is too large Load Diff

4
get_Kinematics.m
View File

@ -128,8 +128,8 @@ else
else else
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i); robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
end end
robot.R(:,:,i) = robot.T(1:3,1:3,i); robot.kine.R(:,:,i) = robot.T(1:3,1:3,i);
robot.t(:,:,i) = robot.T(1:3,4,i); robot.kine.t(:,:,i) = robot.T(1:3,4,i);
end end
robot.Fkine = robot.TW(:,:,end); robot.Fkine = robot.TW(:,:,end);

8
get_regressor.m
View File

@ -43,6 +43,8 @@ switch opt.LD_method
% 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)];
%? match screw method
robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
end end
% construct matrix U, size: [6*n,10*n] % construct matrix U, size: [6*n,10*n]
% U_ = sym(zeros([6*ndof,10*ndof])); % U_ = sym(zeros([6*ndof,10*ndof]));
@ -64,6 +66,11 @@ switch opt.LD_method
delta_ = zeros([ndof,6*ndof]); delta_ = zeros([ndof,6*ndof]);
for i = 1:ndof for i = 1:ndof
delta_(i,6*i) = 1; delta_(i,6*i) = 1;
% %FIXME: use link type
% if(i==ndof)
% delta_(i,6*i) = 0;
% end
end end
robot.regressor.K = delta_*robot.regressor.U; robot.regressor.K = delta_*robot.regressor.U;
if(opt.debug) if(opt.debug)
@ -75,6 +82,7 @@ switch opt.LD_method
% 'Vars',{q_sym,qd_sym,q2d_sym}); % 'Vars',{q_sym,qd_sym,q2d_sym});
if(opt.reGenerate) if(opt.reGenerate)
tic tic
disp('compiling robot.regressor.K');
matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),... matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
'Vars',{q_sym}); 'Vars',{q_sym});
compileTime = toc; compileTime = toc;

9
get_robot_R1000.m
View File

@ -118,8 +118,9 @@ switch opt.robot_def
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
if(opt.Isreal) if(opt.Isreal)
%FIXME: only consider the theta temply %FIXME: only consider the theta temply
robot.dtheta = zeros([1,ndof]); robot.dtheta = zeros([ndof,1]);
robot.ddtheta = zeros([1,ndof]); robot.ddtheta = zeros([ndof,1]);
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
switch opt.KM_method switch opt.KM_method
case 'SDH' case 'SDH'
case 'MDH' case 'MDH'
@ -127,7 +128,6 @@ switch opt.robot_def
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896]; robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
robot.d = [0,0,0,0,0,0.28,0.40,0,0]; robot.d = [0,0,0,0,0,0.28,0.40,0,0];
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
case 'SCREW' case 'SCREW'
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]]; robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]]; robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
@ -151,7 +151,7 @@ switch opt.robot_def
[0;0;-1;cross(-[0;0;-1],co(:,7))]... [0;0;-1;cross(-[0;0;-1],co(:,7))]...
[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(1,9); robot.theta = zeros([ndof,1]);
otherwise otherwise
disp('Bad opt.KM_method!') disp('Bad opt.KM_method!')
return; return;
@ -196,6 +196,7 @@ switch opt.robot_def
[0;0;-1;cross(-[0;0;-1],co(:,7))]... [0;0;-1;cross(-[0;0;-1],co(:,7))]...
[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.link_type = ['R','R','R','R','R','R','R','R','P'];
case 'SDH' case 'SDH'
case 'MDH' case 'MDH'
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493]; robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];

57
get_velocity.m
View File

@ -17,9 +17,9 @@ switch opt.KM_method
end end
if opt.Isreal if opt.Isreal
% init q % init q
q = robot.theta'; q = robot.theta;
qd = robot.dtheta'; qd = robot.dtheta;
qdd = robot.ddtheta'; qdd = robot.ddtheta;
[V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ... [V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist); robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
else else
@ -31,36 +31,37 @@ switch opt.KM_method
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist); robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
end end
% FIXME: twist is not equal to velocity % 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.w = V(1:3,:);
robot.vel.v = V(4:6,:); robot.vel.v = V(4:6,:);
robot.vel.dw = Vd(1:3,:); robot.vel.dw = Vd(1:3,:);
% robot.vel.dv = Vd(4:6,:); % 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 'SDH'
case 'MDH' case 'MDH'
switch opt.Vel_method switch opt.Vel_method
case 'Direct' case 'Direct'
Z = [0,0,1]'; Z = [0,0,1]';
w0 = zeros(3,1); dw0 = zeros(3,1); w0 = zeros(3,1); dw0 = zeros(3,1);
% dv0 = robot.gravity; dv0 = robot.gravity;
v0 = zeros(3,1);dv0 = zeros(3,1); v0 = zeros(3,1);
link_type = robot.link_type; link_type = robot.link_type;
% init q % init q
q = robot.theta; q = robot.theta;
qd = robot.dtheta; qd = robot.dtheta;
qdd = robot.ddtheta; qdd = robot.ddtheta;
for i = 1:robot.ndof % for i = 1:robot.ndof
switch link_type(i) % switch link_type(i)
case 'R' % case 'R'
%Do nothing % %Do nothing
case 'P' % case 'P'
q(i) = robot.d(i); % q(i) = robot.d(i);
qd(i) = robot.vd(i); % qd(i) = robot.vd(i);
qdd(i) = robot.accd(i); % qdd(i) = robot.accd(i);
end % end
end % end
R = robot.R; R = robot.kine.R;
P = robot.t; P = robot.kine.t;
% 1-n外推公式 参考robotics toolbox % 1-n外推公式 参考robotics toolbox
%第一关节 %第一关节
switch link_type(1) switch link_type(1)
@ -77,20 +78,20 @@ switch opt.KM_method
2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1); 2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
end end
%后面n-1关节 %后面n-1关节
for i = 1:robot.ndof for i = 1:robot.ndof-1
switch link_type(i) % switch link_type(i)
case 'R' %revolute % case 'R' %revolute
w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(i+1) * Z ; 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)); 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; 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)); 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 % case 'P' %prismatic
w(:,i+1) = R(:,:,i+1)' * w(:,i); % 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)); % 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); % 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))+... % 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); % 2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
end % end
end end
robot.vel.w = w; robot.vel.w = w;
robot.vel.v = v; robot.vel.v = v;

5
verify_regressor_R1000.m
View File

@ -25,7 +25,8 @@ q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time); qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J)); zero_ = zeros(1,length(q_J));
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; q_J = ones(1,length(q_J));
thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;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_]';
tau = zeros([ndof,length(q_J)]); tau = zeros([ndof,length(q_J)]);
@ -38,7 +39,7 @@ link_inertia = robot.I;
% end % end
robot_pi_vector = reshape(robot.pi,[10*ndof,1]); robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
for i = 1:length(q_J) for i = 1:length(q_J)
regressor = standard_regressor_R1000(thetalist(i,:)'); % regressor = standard_regressor_R1000(thetalist(i,:)');
tau(:,i)=robot.regressor.K*robot_pi_vector; tau(:,i)=robot.regressor.K*robot_pi_vector;
tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)'); tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)');
end end