objective_funct.m

本程序用于对加工夹具进行受力分析 预测被动力

% External Wrench Exerted on the Workpiece
%----------------------------------------------------------------------------------------------------------------
% external wrench described in the tool frame as follows (Dimension-force: newton; torsion: newton-meter):
  f_tool(1) = 30;                 % x-directional force
  f_tool(2) = 25*sin(pi*t/4);     % y-directional force  
  f_tool(3) = -20;                % z-directional force  
  
  m_tool(1) = 0;                  % x-directional torsion
  m_tool(2) = 0;                  % y-directional torsion
  m_tool(3) = 800;                % z-directional torsion
% R_g_tool: means the orientation of the tool frame with respect to the global frame 
  R_g_tool = [-sqrt(2)/2 -sqrt(2)/2 0
               sqrt(2)/2 -sqrt(2)/2 0
                       0          0 1];
%----------------------------------------------------------------------------------------------------------------
% P_tool: means the position vector of the wrench exerted on the workpiece (described in the global frame)
%         UNIT: micrometer
%----------------------------------------------------------------------------------------------------------------
  P_tool(1) = Side_L-10-5*t;
  P_tool(2) = 10+5*t;
  P_tool(3) = Side_L-10;
  
  for i=1:3
      P_toolX(i,i) = 0;
  end
                                     P_toolX(1,2) = -P_tool(3);     P_toolX(1,3) = P_tool(2);       
  P_toolX(2,1) = -P_toolX(1,2);                                     P_toolX(2,3) = -P_tool(1);
  P_toolX(3,1) = -P_toolX(1,3);      P_toolX(3,2) = -P_toolX(2,3);
 
  F_cutting_f = R_g_tool*f_tool';
  F_cutting_m = R_g_tool*m_tool'+P_toolX*F_cutting_f;
%----------------------------------------------------------------------------------------------------------------
% P_weight: means the position vector of the weight center of the workpiece (described in the global frame)
%           UNIT: micrometer
%----------------------------------------------------------------------------------------------------------------
  P_weight(1) = 5*Side_L/12;
  P_weight(2) = 5*Side_L/12;
  P_weight(3) = 5*Side_L/12;
  
  F_weight = [0 0 Fg P_weight(2)*Fg -P_weight(1)*Fg 0]';
  
%----------------------------------------------------------------------------------------------------------------
% Fe: means external wrench (including the weight of the workpiece)
%----------------------------------------------------------------------------------------------------------------
  Fe(1) = F_cutting_f(1)+F_weight(1);
  Fe(2) = F_cutting_f(2)+F_weight(2);
  Fe(3) = F_cutting_f(3)+F_weight(3);
  Fe(4) = F_cutting_m(1)+F_weight(4);
  Fe(5) = F_cutting_m(2)+F_weight(5);
  Fe(6) = F_cutting_m(3)+F_weight(6);
%----------------------------------------------------------------------------------------------------------------
% 6 force equilibrium equations: G_lctF_lct +G_cnFcn+Fe = 0
%----------------------------------------------------------------------------------------------------------------
  F_lct(1) = force(1);
  F_lct(2) = force(8);
  F_lct(3) = force(15);
  F_lct(4) = force(2);
  F_lct(5) = force(9);
  F_lct(6) = force(16);
  F_lct(7) = force(3);
  F_lct(8) = force(10);
  F_lct(9) = force(17);
  F_lct(10) = force(4);
  F_lct(11) = force(11);
  F_lct(12) = force(18);
  F_lct(13) = force(5);
  F_lct(14) = force(12);
  F_lct(15) = force(19);
  F_lct(16) = force(6);
  F_lct(17) = force(13);
  F_lct(18) = force(20);
  F_lct(19) = force(14);
  F_lct(20) = force(21);
  
  fn = Set_Clamping_Force;
  FF1 = (G_lct*F_lct'+fn*G_cn'+Fe')*1e+0;
    
%---------------------------------------------------------------------------------------------------------------
% Representation of 6 force equilibrium equations' constraints (END)
%===============================================================================================================




%===============================================================================================================
% Elastic Deformation Compatible Equation Constraints (START)
% Nonlinear Equality Constraints: Ceq(x) = 0
%---------------------------------------------------------------------------------------------------------------
% Gt*Dlt_X-Dlt_c = 0
% where Gt is spanned by r_L1,r_L2 and r_L3.
% Dlt_c=[(Rg1*Dlt_c1_local)T (Rg2*Dlt_c2_local)T (Rg3*Dlt_c3_local)T]T
% where Rg? is the orientation matrix of the local frame ? with respect to the global frame
%---------------------------------------------------------------------------------------------------------------
  G = [eye(3) eye(3) eye(3); r_L1x r_L2x r_L3x];
%---------------------------------------------------------------------------------------------------------------
% Matrix representation: Rg1, Rg2, Rg3
%---------------------------------------------------------------------------------------------------------------
  Rg1 = [n_L1 t_L11 t_L21];
  Rg2 = [n_L2 t_L12 t_L22];
  Rg3 = [n_L3 t_L13 t_L23];
  
%---------------------------------------------------------------------------------------------------------------
% Elastic Deformation of the 1th, 2th, 3th contacts described in the global frame
%---------------------------------------------------------------------------------------------------------------
  Dlt_c1_local = [x(1) x(8) x(15)]';
  Dlt_c2_local = [x(2) x(9) x(16)]';
  Dlt_c3_local = [x(3) x(10) x(17)]';
  

  Dlt_c1 = Rg1*Dlt_c1_local;
  Dlt_c2 = Rg2*Dlt_c2_local;
  Dlt_c3 = Rg3*Dlt_c3_local;
  
  Dlt_c = [Dlt_c1; Dlt_c2; Dlt_c3];
%---------------------------------------------------------------------------------------------------------------
% Representation of the Matrix G_ spanned by the 3th, 4th, 5th, 6th, 7th contacts
%---------------------------------------------------------------------------------------------------------------
  G_ = [eye(3) eye(3) eye(3) eye(3) eye(3); r_L3x r_L4x r_L5x r_L6x r_cx];
%---------------------------------------------------------------------------------------------------------------
% Matrix representation: Rg4, Rg5, Rg6 and Rgc
%---------------------------------------------------------------------------------------------------------------
  Rg4 = [n_L4 t_L14 t_L24];
  Rg5 = [n_L5 t_L15 t_L25];
  Rg6 = [n_L6 t_L16 t_L26];
  Rgc = [n_c t_c1 t_c2];
%---------------------------------------------------------------------------------------------------------------
% Elastic Deformation of the 4th, 5th, 6th locator and the clamp contacts described in the global frame
%---------------------------------------------------------------------------------------------------------------
  Dlt_c4_local = [x(4) x(11) x(18)]';
  Dlt_c5_local = [x(5) x(12) x(19)]';
  Dlt_c6_local = [x(6) x(13) x(20)]';
  Dlt_cc_local = [x(7) x(14) x(21)]';
  
  Dlt_c4 = Rg4*Dlt_c4_local;
  Dlt_c5 = Rg5*Dlt_c5_local;
  Dlt_c6 = Rg6*Dlt_c6_local;
  Dlt_cc = Rgc*Dlt_cc_local;
  
  Dlt_c_ = [Dlt_c3; Dlt_c4; Dlt_c5; Dlt_c6; Dlt_cc];  
%--------------------------------------------------------------------------------------------------------------
  FF2 = ((G_')*(inv(G*G'))*G*Dlt_c-Dlt_c_)*1e+3;
%--------------------------------------------------------------------------------------------------------------
  FF12 = [FF1; FF2]
  temp = 0;
  for k=1:21
      temp = temp+FF12(k)^2;
  end
  F = 10000*temp;
%--------------------------------------------------------------------------------------------------------------
% Elastic Deformation Compatible Equation Constraints (END)
%==============================================================================================================