match vel with standard
This commit is contained in:
parent
e2e7406f4a
commit
c88067a4b1
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,180 @@
|
|||
%% R1000
|
||||
N=9;
|
||||
% traj
|
||||
time = 0:0.01:1;
|
||||
f=1;
|
||||
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_]';
|
||||
dthetalist = [zero_;q_J;zero_;zero_;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)
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% Mlist_CG = [];
|
||||
% for i = 0:N-1
|
||||
% if i == 0
|
||||
% M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% else
|
||||
% M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% FIXME: BUG here
|
||||
% Mlist_CG=[];
|
||||
% for i = 0:N-1
|
||||
% if i == 0
|
||||
% M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% else
|
||||
% rotation_i = diag([1,1,1]);
|
||||
% for j = 1:i
|
||||
% rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
|
||||
% rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
|
||||
% end
|
||||
% M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% ct=[];
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% ct(:,i) = com_pos_R1(:,i);
|
||||
% elseif i< 9
|
||||
% ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
% else
|
||||
% ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
% end
|
||||
% robot.Home.com(:,i) = ct(:,i);
|
||||
% M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
|
||||
% elseif i<=8
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
|
||||
% elseif i==9
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% get the CG at the world base frame
|
||||
com_pos_R1 = robot.com_pos_R1;
|
||||
com_pos_R2 = robot.com_pos_R2;
|
||||
ct=[];
|
||||
Mlist_CG_Base=[];
|
||||
for i = 1:N
|
||||
if i == 1
|
||||
ct(:,i) = com_pos_R1(:,i);
|
||||
elseif i< 9
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
else
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
end
|
||||
robot.Home.com(:,i) = ct(:,i);
|
||||
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||
end
|
||||
% get the CG at the last GC frame
|
||||
Mlist_CG=[];
|
||||
for i = 1:N
|
||||
if i == 1
|
||||
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||
else
|
||||
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||
end
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
|
||||
% Get the end efforce transformation in each joint reference frame
|
||||
Mlist_ED = [];
|
||||
for i = 1:N
|
||||
M = robot.T(:,:,i);
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
|
||||
%TODO: Get Slist form DH table method
|
||||
% RRRRRRRRP
|
||||
Slist=robot.slist;
|
||||
|
||||
Vlinear=sym(zeros(3,3));
|
||||
J=sym(zeros(6,N));
|
||||
exf=[0;0;0;0;0;0];
|
||||
|
||||
for i = 1:length(q_J)
|
||||
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
|
||||
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
|
||||
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||||
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
|
||||
T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
|
||||
%Want to get the result from TC_delta, which means F at CG represent under frame at the last origin
|
||||
%why we need Mlist_ED
|
||||
%please explain this more
|
||||
F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
||||
end
|
||||
% plot Torque
|
||||
% above 2020b
|
||||
% F_Simpack = pagetranspose(F_Simpack);
|
||||
% below 2020b
|
||||
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,taumat(i+6,:))
|
||||
xlabel('time(s)')
|
||||
ylabel('Torque(Nm)')
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
F_Simpack = permute(F_Simpack,[2 1 3]);
|
||||
figure(2)
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
% Use Body Twist cal linear vel, but can't cal the end frame vel
|
||||
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||||
% [0;0;0], exf, Mlist, Glist, Slist);
|
||||
% j=1;
|
||||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
|
||||
% j=2;
|
||||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
|
||||
|
|
@ -13,13 +13,14 @@ link_mass = robot.m;
|
|||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
thetalist = [zero_;q_J;zero_;q_J;zero_;q_J;zero_;zero_;zero_]';
|
||||
dthetalist = [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_]';
|
||||
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)*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
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ R = robot.kine.R;
|
|||
P = robot.kine.t;
|
||||
w = robot.vel.w ;
|
||||
dw = robot.vel.dw ;
|
||||
v = robot.vel.v ;
|
||||
dv = robot.vel.dv ;
|
||||
switch opt.LD_method
|
||||
case 'Direct'
|
||||
|
|
@ -37,16 +38,14 @@ switch opt.LD_method
|
|||
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||||
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||||
dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
|
||||
v_skew(:,:,i) = vec2skewSymMat(v(:,i));
|
||||
dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
|
||||
w_l(:,:,i) = vec2linearSymMat(w(:,i));
|
||||
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
|
||||
% 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);
|
||||
% 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)];
|
||||
% 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)];
|
||||
%? match screw method
|
||||
% robot.regressor.A(:,:,i) = Adjoint(RpToTrans(robot.TW(1:3,1:3,i),[0;0;0]))'*robot.regressor.A(:,:,i);
|
||||
end
|
||||
|
|
|
|||
|
|
@ -120,6 +120,7 @@ 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.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
|
|
|
|||
|
|
@ -0,0 +1,109 @@
|
|||
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
|
||||
% 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();
|
||||
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'
|
||||
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);
|
||||
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;
|
||||
% 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-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
|
||||
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
|
||||
|
|
@ -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
|
||||
|
|
@ -32,9 +41,12 @@ switch opt.KM_method
|
|||
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.w = V(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);
|
||||
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'
|
||||
|
|
|
|||
|
|
@ -15,4 +15,5 @@ F_Simpack(end-2,:,1)
|
|||
robot_pi_vecoter = reshape(robot.pi,[90,1]);
|
||||
F = robot.regressor.U*robot_pi_vecoter;
|
||||
FF = reshape(F,[6,9])
|
||||
%%
|
||||
F_Simpack(:,:,1)
|
||||
Loading…
Reference in New Issue