diff --git a/Identification_main.m b/Identification_main.m index 3db19f1..f95dfe1 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -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); diff --git a/R1000_Dynamics_num.asv b/R1000_Dynamics_num.asv new file mode 100644 index 0000000..0fd64f9 --- /dev/null +++ b/R1000_Dynamics_num.asv @@ -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); \ No newline at end of file diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m index be29c92..f58ad63 100644 --- a/R1000_Dynamics_num.m +++ b/R1000_Dynamics_num.m @@ -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 diff --git a/get_regressor.m b/get_regressor.m index 36de173..91d904e 100644 --- a/get_regressor.m +++ b/get_regressor.m @@ -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 diff --git a/get_robot_R1000.m b/get_robot_R1000.m index a892fcc..68a8901 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -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' diff --git a/get_velocity.asv b/get_velocity.asv new file mode 100644 index 0000000..1c9eee6 --- /dev/null +++ b/get_velocity.asv @@ -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 diff --git a/get_velocity.m b/get_velocity.m index beaddd4..13d6509 100644 --- a/get_velocity.m +++ b/get_velocity.m @@ -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])]; 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; + % 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' diff --git a/untitled3.m b/untitled3.m index 0feee9c..0a9b7e7 100644 --- a/untitled3.m +++ b/untitled3.m @@ -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) \ No newline at end of file