jiangzhihong

蒋志宏-机器人学基础/第12章 基于ADAMSMATLAB机器人动力学联合仿真/Controls_Plant_1.m

@@ -0,0 +1,88 @@
+% Adams / MATLAB Interface - Release 2015.0.0
+system('taskkill /IM scontrols.exe /F >NUL');clc;
+global ADAMS_sysdir; % used by setup_rtw_for_adams.m
+global ADAMS_host; % used by start_adams_daemon.m
+machine=computer;
+datestr(now)
+if strcmp(machine, 'SOL2')
+   arch = 'solaris32';
+elseif strcmp(machine, 'SOL64')
+   arch = 'solaris32';
+elseif strcmp(machine, 'GLNX86')
+   arch = 'linux32';
+elseif strcmp(machine, 'GLNXA64')
+   arch = 'linux64';
+elseif strcmp(machine, 'PCWIN')
+   arch = 'win32';
+elseif strcmp(machine, 'PCWIN64')
+   arch = 'win64';
+else
+   disp( '%%% Error : Platform unknown or unsupported by Adams/Controls.' ) ;
+   arch = 'unknown_or_unsupported';
+   return
+end
+if strcmp(arch,'win64')
+   [flag, topdir]=system('adams2015_x64 -top');
+else
+   [flag, topdir]=system('adams2015 -top');
+end
+if flag == 0
+  temp_str=strcat(topdir, '/controls/', arch);
+  addpath(temp_str)
+  temp_str=strcat(topdir, '/controls/', 'matlab');
+  addpath(temp_str)
+  temp_str=strcat(topdir, '/controls/', 'utils');
+  addpath(temp_str)
+  ADAMS_sysdir = strcat(topdir, '');
+else
+  addpath( 'C:\MSC~1.SOF\ADAMS_~1\2015\controls/win64' ) ;
+  addpath( 'C:\MSC~1.SOF\ADAMS_~1\2015\controls/win32' ) ;
+  addpath( 'C:\MSC~1.SOF\ADAMS_~1\2015\controls/matlab' ) ;
+  addpath( 'C:\MSC~1.SOF\ADAMS_~1\2015\controls/utils' ) ;
+  ADAMS_sysdir = 'C:\MSC~1.SOF\ADAMS_~1\2015\' ;
+end
+ADAMS_exec = '' ;
+ADAMS_host = 'Feng-PC' ;
+ADAMS_cwd ='C:\Users\Feng\Desktop\jiaocaichengxu'  ;
+ADAMS_prefix = 'Controls_Plant_1' ;
+ADAMS_static = 'no' ;
+ADAMS_solver_type = 'C++' ;
+if exist([ADAMS_prefix,'.adm']) == 0
+   disp( ' ' ) ;
+   disp( '%%% Warning : missing ADAMS plant model file(.adm) for Co-simulation or Function Evaluation.' ) ;
+   disp( '%%% If necessary, please re-export model files or copy the exported plant model files into the' ) ;
+   disp( '%%% working directory.  You may disregard this warning if the Co-simulation/Function Evaluation' ) ;
+   disp( '%%% is TCP/IP-based (running Adams on another machine), or if setting up MATLAB/Real-Time Workshop' ) ;
+   disp( '%%% for generation of an External System Library.' ) ;
+   disp( ' ' ) ;
+end
+ADAMS_init = '' ;
+ADAMS_inputs  = 'Tor_Jg1!Tor_Jg2!Tor_Jg3!Tor_Jg4!Tor_Jg5!Tor_Jg6' ;
+ADAMS_outputs = 'angle1!angle2!angle3!angle4!angle5!angle6' ;
+ADAMS_pinput = 'Controls_Plant_1.ctrl_pinput' ;
+ADAMS_poutput = 'Controls_Plant_1.ctrl_poutput' ;
+ADAMS_uy_ids  = [
+                   1
+                   2
+                   3
+                   4
+                   5
+                   6
+                   7
+                   8
+                   9
+                   10
+                   11
+                   12
+                ] ;
+ADAMS_mode   = 'non-linear' ;
+tmp_in  = decode( ADAMS_inputs  ) ;
+tmp_out = decode( ADAMS_outputs ) ;
+disp( ' ' ) ;
+disp( '%%% INFO : ADAMS plant actuators names :' ) ;
+disp( [int2str([1:size(tmp_in,1)]'),blanks(size(tmp_in,1))',tmp_in] ) ;
+disp( '%%% INFO : ADAMS plant sensors   names :' ) ;
+disp( [int2str([1:size(tmp_out,1)]'),blanks(size(tmp_out,1))',tmp_out] ) ;
+disp( ' ' ) ;
+clear tmp_in tmp_out ;
+% Adams / MATLAB Interface - Release 2015.0.0

蒋志宏-机器人学基础/第12章 基于ADAMSMATLAB机器人动力学联合仿真/dh.m

@@ -0,0 +1,9 @@
+ function out = dh( alpha,a,d,theta )
+%UNTITLED4 Summary of this function goes here
+%   Detailed explanation goes here
+out=[cos(theta) -sin(theta) 0 a
+    sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d
+    sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) cos(alpha)*d
+    0 0 0 1];
+end
+

蒋志宏-机器人学基础/第12章 基于ADAMSMATLAB机器人动力学联合仿真/dynamics.m

@@ -0,0 +1,139 @@
+function Torque = dynamics(angle, angluar_v, angluar_a)
+% 质量矩阵, unit: Kg
+mass = [65.0, 50.0, 20.0, 10.5, 3.5, 1.0];    
+
+% 杆件参数, unit: m
+P1=0.43;
+P2=0.09956;
+P4=0.65074;
+P5=0.00105;
+P6=0.70023;
+% P7=0.205;
+
+% 质心在杆件坐标系下的表示, unit: m
+PC = [ -0.027556004011,     -0.01390621464,		-0.0934367772;
+		0.281018326, 		-0.02378727752,		 0.1205031204; 
+	   -0.00026708720342,	-0.049327848994,	-0.013542622052;         
+		0.0012883588476, 	 0.0061959395757,	-0.2532128043;
+	   -0.0021609978802,	-0.047658510905,	 0.0051235574999; 
+	   -0.0019006821653,	 0.0040220390279, 	 0.129974036 ];
+
+% 下一个坐标系在上一个坐标系下的表示
+P = [0.0, 0.0,  P1; 
+	 -P2, 0.0, 0.0; 
+      P4, 0.0, 0.0; 
+	  P5, -P6, 0.0; 
+	 0.0, 0.0, 0.0; 
+	 0.0, 0.0, 0.0; ];	 
+
+% 杆件在质心坐标系下的惯性张量 unit: kg*m^2
+IC(:,:,1) = [1.3    0.0     0.0;  
+			 0.0   	0.9		0.0;   
+			 0.0 	0.0    	0.8]; 
+IC(:,:,2) = [2.9   	0.0     0.0;  
+			 0.0   	2.8		0.0;   
+			 0.0 	0.0    	0.2]; 
+IC(:,:,3) = [0.22	0.0     0.0;  
+			 0.0   	0.22	0.0;   
+			 0.0 	0.0    	0.17]; 
+IC(:,:,4) = [0.32	0.0     0.0;  
+			 0.0	0.32	0.0;   
+			 0.0 	0.0    	0.03]; 
+IC(:,:,5) = [0.002	0.0     0.0;  
+			 0.0	0.002	0.0;   
+			 0.0 	0.0		0.002]; 
+IC(:,:,6) = [0.002	0.0     0.0;  
+			 0.0	0.002	0.0;   
+			 0.0 	0.0		0.0004]; 
+
+% 获取杆件间的旋转矩阵(i+1在i下的表示) 
+R(:,:,1) = [cos(angle(1)), 	-sin(angle(1)),	0.0; 
+			sin(angle(1)), 	cos(angle(1)), 	0.0; 
+			0.0,		   	0.0,			1.0];
+
+R(:,:,2) = [sin(angle(2)),	cos(angle(2)),	0.0;  
+			0.0,		   	0.0,			1.0;
+			cos(angle(2)),	-sin(angle(2)), 0.0];
+
+R(:,:,3) = [cos(angle(3)),	-sin(angle(3)),	0.0;  
+			sin(angle(3)), 	cos(angle(3)),	0.0; 
+			0.0,		   	0.0,			1.0];
+
+R(:,:,4) = [cos(angle(4)),	-sin(angle(4)),	0.0; 
+			0.0,		   	0.0,			-1.0; 
+			sin(angle(4)), 	cos(angle(4)),	0.0];
+
+R(:,:,5) = [cos(angle(5)),	-sin(angle(5)),	0.0; 
+			0.0,		   	0.0,			1.0;
+			-sin(angle(5)),	-cos(angle(5)),	0.0];
+
+R(:,:,6) = [cos(angle(6)),	-sin(angle(6)),	0.0; 
+			0.0,		   	0.0,			-1.0;
+			sin(angle(6)), 	cos(angle(6)),	0.0];
+
+% 获取杆件间的旋转逆矩阵(i在i+1下的表示) 
+for i1 = 1:6
+    inR(:,:,i1) = inv(R(:,:,i1));
+end
+
+% 迭代初始化
+% 基座角速度为0
+omiga_v0 = [0; 0; 0];
+% 基座角加速度为0
+omiga_a0 = [0; 0; 0];
+% 基座线加速度为0
+acc0 = [0; 0; 0];
+
+% 外推，求杆件1-6的角速度，线加速度，角加速度
+for i = 1:6
+	if (i == 1)
+		% 求杆件1角速度
+		z = [0.0;0.0;angluar_v(i)];
+		omiga_v(:,i) = ones(3,3)*omiga_v0 + z;
+		% 求杆件1角加速度
+		za = [0.0; 0.0; angluar_a(i)];
+		omiga_a(:,i) = ones(3,3)*omiga_a0 + cross(ones(3,3)*omiga_v0, z) + za;
+		% 求杆件1线加速度   
+		acc(:,i) = ones(3,3)*(cross(omiga_a0, P(i,:)') + cross(omiga_v0, cross(omiga_v0, P(i,:)')) + acc0);
+	else
+		% 求杆件2-6角速度---ok
+		z = [0.0;0.0;angluar_v(i)];
+		omiga_v(:,i) = inR(:,:,i)*omiga_v(:,i-1) + z;
+		% 求杆件2-6角加速度--ok
+		za = [0.0; 0.0; angluar_a(i)];
+		omiga_a(:,i) = inR(:,:,i)*omiga_a(:,i-1) + cross(inR(:,:,i)*omiga_v(:,i-1), z) + za;
+		% 求杆件2-6线加速度--ok
+		acc(:,i) = inR(:,:,i)*(cross(omiga_a(:,i-1), P(i,:)') + cross(omiga_v(:,i-1), cross(omiga_v(:,i-1), P(i,:)')) + acc(:,i-1));
+    end
+
+	% 求杆件1-6质心线加速度--ok
+    accz(:,i) = cross(omiga_a(:,i), PC(i,:)') + cross(omiga_v(:,i), cross(omiga_v(:,i), PC(i,:)')) + acc(:,i);
+	% 求杆件1-6惯性力
+	force1(:,i) = mass(i)*accz(:,i);
+	% 求杆件1-6惯性力矩
+    torque1(:,i) = IC(:,:,i)*omiga_a(:,i) + cross(omiga_v(:,i), IC(:,:,i)*omiga_v(:,i));
+end
+
+% 末端关节受外力/力矩
+force2out = [0; 0; 0];
+torque2out = [0; 0; 0];
+
+% 内推，求关节6-1的力和力矩	
+for i=6:-1:1
+	if (i==6)
+		% 求杆件6受到的力--ok
+		force2(:,i) = ones(3,3)*force2out + force1(:,i);
+		% 求杆件6受到的力矩--ok
+		torque2(:,i) = torque1(:,i) + ones(3,3)*torque2out + cross(PC(i,:)', force1(:,i)) + cross(zeros(3,1), ones(3,3)*force2out);
+	else
+		% 求杆件5-1受到的力
+%         force1(:,i)
+%         force2(:,i+1)
+		force2(:,i) = R(:,:,i+1)*force2(:,i+1) + force1(:,i);
+		% 求杆件5-1受到的力矩
+		torque2(:,i) = torque1(:,i) + R(:,:,i+1)*torque2(:,i+1) + cross(PC(i,:)', force1(:,i)) + cross(P(i+1,:)', R(:,:,i+1)*force2(:,i+1));
+	end
+
+	% 关节i受力矩
+    Torque(i) = torque2(3,i);
+end

蒋志宏-机器人学基础/第12章 基于ADAMSMATLAB机器人动力学联合仿真/interpolation.m

@@ -0,0 +1,21 @@
+function y = interpolation(x0,x1,tf,t1)
+
+v0=0;
+v1=0;
+a0=0;
+a1=0;
+
+b0 = x0;
+b1 = v0;
+b2 = a0/2;
+b3 = (20.0*x1 - 20.0*x0 - (8.0*v1+12.0*v0)*tf - (3.0*a0-a1)*tf*tf)/(2.0*tf*tf*tf);
+b4 = (30.0*x0 - 30.0*x1 + (14.0*v1 + 16.0*v0)*tf + (3.0*a0-2.0*a1)*tf*tf)/(2.0*tf*tf*tf*tf);
+b5 = (12.0*x1 - 12.0*x0 - (6.0*v1+6.0*v0)*tf - (a0-a1)*tf*tf)/(2.0*tf*tf*tf*tf*tf);
+
+t=t1;
+y1 = b0 + b1*t + b2*t*t + b3*t*t*t + b4*t*t*t*t + b5*t*t*t*t*t;
+y2 = b1 + 2*b2*t + 3*b3*t*t + 4*b4*t*t*t + 5*b5*t*t*t*t;
+y3 = 2*b2 + 6*b3*t + 12*b4*t*t + 20*b5*t*t*t;
+y=[y1 y2 y3];
+
+end