空间机械臂动力学模型

%
% 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（:34） = [ 0  0 0.5]‘;
cc（:45） = [ 0 0 0.5 ]‘;
cc（:56） = [ 0 0 0.5 ]‘;

% Initialize variables

q   = zeros（61）;
q