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);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J));
q_J = ones(1,length(q_J));
q_J = pi/4*ones(1,length(q_J));
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
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_]';
% Get general mass matrix
Glist=[];
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])];
Glist = cat(3, Glist, Gb);
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);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
zero_ = zeros(1,length(q_J));
q_J = ones(1,length(q_J));
q_J = pi/4*ones(1,length(q_J));
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
thetalist = [zero_;zero_;q_J;q_J;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_]';
% Get general mass matrix

4
get_regressor.m
View File

@ -44,8 +44,10 @@ switch opt.LD_method
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
% size of matrix A is 6*10, need to -1
% 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); ...
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
% robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
end

12
get_robot_R1000.m
View File

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

14
get_velocity.asv
View File

@ -14,6 +14,15 @@ switch opt.KM_method
% ?
% mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
% 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
if opt.Isreal
% init q
@ -33,9 +42,10 @@ switch opt.KM_method
% FIXME: twist is not equal to velocity
% FIXME: Need to get the velocity represent at base frame
robot.vel.w = V(1:3,2:end);
for i = i:robot.ndof
robot.vel.v = V(4:6,i+1)+robot.vel.w*robot.kine.t();
for i = 1:robot.ndof
robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*;
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)];

9
get_velocity.m
View File

@ -19,6 +19,7 @@ switch opt.KM_method
% 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
% get joint inertial
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));
@ -42,12 +43,14 @@ switch opt.KM_method
% FIXME: twist is not equal to velocity
% FIXME: Need to get the velocity represent at base frame
robot.vel.w = V(1:3,2:end);
robot.vel.dw = Vd(1:3,2:end);
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
% 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)];
case 'SDH'
case 'MDH'