Compare commits
No commits in common. "3833315adb109e2d8742fa142cd7a8c9e4f3e02b" and "6561c775dc8f16cc8cf361229924486d415f47e0" have entirely different histories.
3833315adb
...
6561c775dc
|
|
@ -12,11 +12,11 @@ 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);
|
||||
% 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,13 +7,12 @@ 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_;q_J;zero_;zero_;zero_;zero_;zero_]';
|
||||
thetalist = [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_]';
|
||||
|
||||
|
|
|
|||
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.kine.R(:,:,i) = robot.T(1:3,1:3,i);
|
||||
robot.kine.t(:,:,i) = robot.T(1:3,4,i);
|
||||
robot.R(:,:,i) = robot.T(1:3,1:3,i);
|
||||
robot.t(:,:,i) = robot.T(1:3,4,i);
|
||||
end
|
||||
robot.Fkine = robot.TW(:,:,end);
|
||||
|
||||
|
|
|
|||
|
|
@ -43,8 +43,6 @@ 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]));
|
||||
|
|
@ -66,11 +64,6 @@ 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)
|
||||
|
|
@ -82,7 +75,6 @@ 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,9 +118,8 @@ switch opt.robot_def
|
|||
% ------------------------------------------------------------------------
|
||||
if(opt.Isreal)
|
||||
%FIXME: only consider the theta temply
|
||||
robot.dtheta = zeros([ndof,1]);
|
||||
robot.ddtheta = zeros([ndof,1]);
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
robot.dtheta = zeros([1,ndof]);
|
||||
robot.ddtheta = zeros([1,ndof]);
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
|
|
@ -128,6 +127,7 @@ 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([ndof,1]);
|
||||
robot.theta = zeros(1,9);
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
|
|
@ -196,7 +196,6 @@ 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];
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -31,37 +31,36 @@ 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
|
||||
case 'Direct'
|
||||
Z = [0,0,1]';
|
||||
w0 = zeros(3,1); dw0 = zeros(3,1);
|
||||
dv0 = robot.gravity;
|
||||
v0 = 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.kine.R;
|
||||
P = robot.kine.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.R;
|
||||
P = robot.t;
|
||||
% 1-n外推公式 参考robotics toolbox
|
||||
%第一关节
|
||||
switch link_type(1)
|
||||
|
|
@ -78,20 +77,20 @@ switch opt.KM_method
|
|||
2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
|
||||
end
|
||||
%后面n-1关节
|
||||
for i = 1:robot.ndof-1
|
||||
% switch link_type(i)
|
||||
% case 'R' %revolute
|
||||
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
|
||||
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,8 +25,7 @@ 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));
|
||||
thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
|
||||
thetalist = [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_]';
|
||||
tau = zeros([ndof,length(q_J)]);
|
||||
|
|
@ -39,7 +38,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