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

pos don't match

This commit is contained in:
cosmic_power 2024-10-20 19:14:23 +08:00
parent c88067a4b1
commit 27280103ff
6 changed files with 37 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
R1000_Dynamics_num.asv
View File

@ -7,20 +7,20 @@ 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)); q_J = pi/4*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_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; dthetalist = [zero_;zero_;zero_;q_J;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_]';
% Get general mass matrix % Get general mass matrix
Glist=[]; Glist=[];
for i = 1:N for i = 1:N
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i) link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])]; Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
Glist = cat(3, Glist, Gb); Glist = cat(3, Glist, Gb);
end end

6
R1000_Dynamics_num.m
View File

@ -7,14 +7,14 @@ 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)); q_J = pi/4*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_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;q_J;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_]';
% Get general mass matrix % Get general mass matrix

4
get_regressor.m
View File

@ -44,8 +44,10 @@ switch opt.LD_method
dw_l(:,:,i) = vec2linearSymMat(dw(:,i)); dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
% size of matrix A is 6*10, need to -1 % size of matrix A is 6*10, need to -1
% Traversaro el 2015 % Traversaro el 2015
% robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
% zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_skew(:,:,i)),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ... robot.regressor.A(:,:,i) = [dv(:,i)+w_skew(:,:,i)*v(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
zeros(3,1),-(dv_skew(:,:,i)+w_skew(:,:,i)*v_skew(:,:,i)-v_skew(:,:,i)*w_skew(:,:,i)),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)]; zeros(3,1),-vec2skewSymMat((dv(:,i)+w_skew(:,:,i)*v(:,i))),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
%? match screw method %? 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 end

12
get_robot_R1000.m
View File

@ -120,7 +120,11 @@ switch opt.robot_def
%FIXME: only consider the theta temply %FIXME: only consider the theta temply
robot.dtheta = zeros([ndof,1]); robot.dtheta = zeros([ndof,1]);
robot.ddtheta = zeros([ndof,1]); robot.ddtheta = zeros([ndof,1]);
robot.dtheta(2) = 1; % robot.theta(end-1) = pi/4;
% robot.dtheta(end-2) = pi/4;
% robot.ddtheta(end-1) = 100*pi/4;
% robot.ddtheta(end-2) = 100*pi/4;
% robot.ddtheta(end-3) = 100*pi/4;
robot.link_type = ['R','R','R','R','R','R','R','R','P']; robot.link_type = ['R','R','R','R','R','R','R','R','P'];
switch opt.KM_method switch opt.KM_method
case 'SDH' case 'SDH'
@ -153,9 +157,9 @@ 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) = pi/4; % robot.theta(end-1) = pi/4;
% robot.theta(4) = 1; robot.theta(3) = pi/4;
% robot.theta(6) = 1; robot.theta(4) = pi/4;
otherwise otherwise
disp('Bad opt.KM_method!') disp('Bad opt.KM_method!')
return; return;

18
get_velocity.asv
View File

@ -12,8 +12,17 @@ switch opt.KM_method
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])]; Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
Glist = cat(3, Glist, Gb); Glist = cat(3, Glist, Gb);
% ? % ?
% mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i); % mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
% robot.pi(2:4,i) = mc; % robot.pi(2:4,i) = mc;
% 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.Home.R(:,:,i)'*robot.pi(2:4,i);
com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
robot.pi(5:end,i) = robot.I_vec(:,i);
end end
if opt.Isreal if opt.Isreal
% init q % init q
@ -33,9 +42,10 @@ switch opt.KM_method
% FIXME: twist is not equal to velocity % FIXME: twist is not equal to velocity
% FIXME: Need to get the velocity represent at base frame % FIXME: Need to get the velocity represent at base frame
robot.vel.w = V(1:3,2:end); robot.vel.w = V(1:3,2:end);
for i = i:robot.ndof for i = 1:robot.ndof
robot.vel.v = V(4:6,i+1)+robot.vel.w*robot.kine.t(); robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*;
end end
% robot.vel.v = V(4:6,2:end);
robot.vel.dw = Vd(1:3,2:end); robot.vel.dw = Vd(1:3,2:end);
robot.vel.dv = Vd(4:6,2:end); robot.vel.dv = Vd(4:6,2:end);
% 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)];

9
get_velocity.m
View File

@ -19,6 +19,7 @@ switch opt.KM_method
% our defined frame. R*com is the wrong result % our defined frame. R*com is the wrong result
robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i); robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i);
com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
% get joint inertial
robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat; robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i); robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i)); robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
@ -42,12 +43,14 @@ switch opt.KM_method
% FIXME: twist is not equal to velocity % FIXME: twist is not equal to velocity
% FIXME: Need to get the velocity represent at base frame % FIXME: Need to get the velocity represent at base frame
robot.vel.w = V(1:3,2:end); robot.vel.w = V(1:3,2:end);
robot.vel.dw = Vd(1:3,2:end);
for i = 1:robot.ndof for i = 1:robot.ndof
robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.pi(2:4,i)/robot.m(i); robot.vel.v(:,i) = V(4:6,i+1) - vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i);
robot.vel.dv(:,i) = Vd(4:6,i+1)- vec2skewSymMat(robot.vel.dw(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i)...
- vec2skewSymMat(robot.vel.w(:,i))*(vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i));
end end
% robot.vel.v = V(4:6,2:end); % robot.vel.v = V(4:6,2:end);
robot.vel.dw = Vd(1:3,2:end);
robot.vel.dv = Vd(4:6,2:end);
% 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'