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