still bugs
This commit is contained in:
parent
f4f045a161
commit
3833315adb
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -7,12 +7,13 @@ 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));
|
||||
% Dynamics parameters
|
||||
link_mass = robot.m;
|
||||
com_pos = robot.com;
|
||||
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_]';
|
||||
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -128,8 +128,8 @@ else
|
|||
else
|
||||
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
|
||||
end
|
||||
robot.R(:,:,i) = robot.T(1:3,1:3,i);
|
||||
robot.t(:,:,i) = robot.T(1:3,4,i);
|
||||
robot.kine.R(:,:,i) = robot.T(1:3,1:3,i);
|
||||
robot.kine.t(:,:,i) = robot.T(1:3,4,i);
|
||||
end
|
||||
robot.Fkine = robot.TW(:,:,end);
|
||||
|
||||
|
|
|
|||
|
|
@ -66,6 +66,11 @@ switch opt.LD_method
|
|||
delta_ = zeros([ndof,6*ndof]);
|
||||
for i = 1:ndof
|
||||
delta_(i,6*i) = 1;
|
||||
|
||||
% %FIXME: use link type
|
||||
% if(i==ndof)
|
||||
% delta_(i,6*i) = 0;
|
||||
% end
|
||||
end
|
||||
robot.regressor.K = delta_*robot.regressor.U;
|
||||
if(opt.debug)
|
||||
|
|
@ -77,6 +82,7 @@ switch opt.LD_method
|
|||
% 'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
if(opt.reGenerate)
|
||||
tic
|
||||
disp('compiling robot.regressor.K');
|
||||
matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
|
||||
'Vars',{q_sym});
|
||||
compileTime = toc;
|
||||
|
|
|
|||
|
|
@ -118,8 +118,9 @@ switch opt.robot_def
|
|||
% ------------------------------------------------------------------------
|
||||
if(opt.Isreal)
|
||||
%FIXME: only consider the theta temply
|
||||
robot.dtheta = zeros([1,ndof]);
|
||||
robot.ddtheta = zeros([1,ndof]);
|
||||
robot.dtheta = zeros([ndof,1]);
|
||||
robot.ddtheta = zeros([ndof,1]);
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
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.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.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
case 'SCREW'
|
||||
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]];
|
||||
|
|
@ -151,7 +151,7 @@ switch opt.robot_def
|
|||
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||
[0;0;0;1;0;0]];
|
||||
robot.theta = zeros(1,9);
|
||||
robot.theta = zeros([ndof,1]);
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
|
|
@ -196,6 +196,7 @@ switch opt.robot_def
|
|||
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||
[0;0;0;1;0;0]];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
||||
|
|
|
|||
106
get_velocity.asv
106
get_velocity.asv
|
|
@ -1,106 +0,0 @@
|
|||
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
|
||||
|
|
@ -17,9 +17,9 @@ switch opt.KM_method
|
|||
end
|
||||
if opt.Isreal
|
||||
% init q
|
||||
q = robot.theta';
|
||||
qd = robot.dtheta';
|
||||
qdd = robot.ddtheta';
|
||||
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
|
||||
|
|
@ -43,25 +43,25 @@ switch opt.KM_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);
|
||||
dv0 = robot.gravity;
|
||||
v0 = 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;
|
||||
% 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.kine.R;
|
||||
P = robot.kine.t;
|
||||
% 1-n外推公式 参考robotics toolbox
|
||||
%第一关节
|
||||
switch link_type(1)
|
||||
|
|
@ -78,20 +78,20 @@ switch opt.KM_method
|
|||
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
|
||||
for i = 1:robot.ndof-1
|
||||
% 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
|
||||
% 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;
|
||||
|
|
|
|||
|
|
@ -25,7 +25,8 @@ 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));
|
||||
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_]';
|
||||
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
tau = zeros([ndof,length(q_J)]);
|
||||
|
|
@ -38,7 +39,7 @@ link_inertia = robot.I;
|
|||
% end
|
||||
robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
|
||||
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_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)');
|
||||
end
|
||||
|
|
|
|||
Loading…
Reference in New Issue