SpaceDYN 空间环境仿真matlab工具包

这是日本人开发的一个空间环境仿真工具包，可用于仿真空间机器人，卫星等的运动操控等。

%
% SAMPLE	A Sample Program for Spacedyn Toolbox.
%
%
% 1998.9.17  : originally written by K.FUJISHIMA
% 1998.10.23 : change commands to use “f_dyn_rk.m“
% 1999.10.7  : modified to meet the version 1
% 2006.7.20  : modified to meet the version 1.2
%
% 1998 （C） Space Robotics Lab Tohoku Univ Japan.
%

global Qi J_type BB SS SE S0
global cc c0 ce
global m0 inertia0 m inertia mass
global d_time
global Qe
global Gravity
global num_q Ez

% Unit vector in z-axis
Ez = [0;0;1];

% link Parameters

BB = [ 0 1 2 0 4 5 ];

SS = [ -1  1  0  0  0  0;
        0 -1  1  0  0  0;
        0  0 -1  0  0  0;
        0  0  0 -1  1  0;
        0  0  0  0 -1  1;
        0  0  0  0  0 -1 ];

S0 = [ 1 0 0 1 0 0 ];

SE = [ 0 0 1 0 0 1 ];

J_type = [ ‘R‘ ‘R‘ ‘P‘ ‘R‘ ‘R‘ ‘P‘ ];

c0 = [ 0 0 0  0 0 0;
       1 0 0 -1 0 0;
       0 0 0  0 0 0 ];

m0 = 100;

inertia0 = [ 10  0  0;
              0 10  0;
              0  0 10 ];

m = [ 10 10 10 10 10 10 ];

mass = sum（m） + m0;

Qi = [ -pi/2 pi/2 0 pi/2 pi/2 0;
           0    0 0    0    0 0;
           0    0 0    0    0 0 ];

inertia = [  1 0   0   1   0 0   1   0 0  1 0   0   1   0 0   1   0 0;
             0 1   0   0 0.1 0   0 0.1 0  0 1   0   0 0.1 0   0 0.1 0;
             0 0 0.1   0   0 1   0   0 1  0 0 0.1   0   0 1   0   0 1 ];

ce = [ 0 0   0 0 0   0;
       0 0 0.5 0 0 0.5;
       0 0   0 0 0   0 ];

Qe = [ 0 0    0 0 0    0;
       0 0    0 0 0    0;
       0 0 pi/2 0 0 pi/2];

cc = zeros（366）;

cc（:11） = [ 0 0 -0.5 ]‘;
cc（:22） = [ 0 0 -0.5 ]‘;
cc（:33） = [ 0 0 -0.5 ]‘;
cc（:44） = [ 0 0 -0.5 ]‘;
cc（:55） = [ 0 0 -0.5 ]‘;
cc（:66） = [ 0 0 -0.5 ]‘;
cc（:12） = [ 0 0 0.5 ]‘;
cc（:23） = [ 0 0 0.5 ]‘;
cc（:45） = [ 0 0 0.5 ]‘;
cc（:56） = [ 0 0 0.5 ]‘;

% Initialize variables

q   = zeros（61）;
qd  = zeros（61）;
qdd = zeros（61）;

v0  = [ 0 0 0 ]‘;
w0  = [ 0 0 0 ]‘;
vd0 = [ 0 0 0 ]‘;
wd0 = [ 0 0 0 ]‘;

vv = zeros（36）;
ww = zeros（36）;
vd = zeros（36）;
wd = zeros（36）;

R0 = [ 0 0 0 ]‘;
Q0 = [ 0 0 0 ]‘;
A0 = eye（33）;

Fe = zeros（36）;
Te = zeros（36）;
F0 = [ 0 0 0 ]‘;
T0 = [ 0 0 0 ]‘;

tau = zeros（61）;
d_time = 0.01;
Gravity = [ 0 0 0 ]‘;

desired_q = [ 0.3 0.2 0.1 0.6 0.5 0.4 ]‘;
gain_spring = 10;
gain_dumper = 10;

% Number of links
num_q = length（ q ）;


%%% Simulation Loop start

for time = 0:d_time:20

        time

        tau = zeros（61）;
        tau（1:61） = gain_spring.*（ desired_q - q ） - gain_dumper.*qd;
        
    [ R0 A0 v0 w0 q qd ] = f_dyn_rk2（ R0 A0 v0 w0 q qd F0 T0 Fe Te tau ）;
	figure（1）;
	hold on;
	plot（timeq（1）‘r-‘）;
	plot（timeq（2）‘b-‘）;
	plot（timeq（3）‘y-‘）;
	plot（timeq（4）‘m-‘）;
	plot（timeq（5）‘c-‘）;
	plot（timeq（6）‘g-‘）;


end



%%% EOF

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1358  2010-05-31 18:16  spacedyn\demo1.html

     文件        993  2010-05-31 18:17  spacedyn\demo2.html

     文件    3274739  2010-05-31 18:18  spacedyn\doc.pdf

     文件       2236  2010-05-31 18:17  spacedyn\func_v1r2.txt

     文件    1808341  2010-05-31 18:20  spacedyn\movx.mov

     文件       2821  2010-05-31 18:17  spacedyn\sample_v1r2.m

     文件       3771  2010-05-31 18:17  spacedyn\sdx-rm.txt

     文件      12627  2010-05-31 18:17  spacedyn\sdx.gz

     文件      81920  2010-05-31 18:17  spacedyn\sdx.tar

     文件      95232  2010-05-31 18:16  spacedyn\spacedyn-v1r2.tar

     目录          0  2010-06-10 07:54  spacedyn

----------- ---------  ---------- -----  ----

              5284038                    11