%
%?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