資源簡(jiǎn)介
C語(yǔ)言實(shí)現(xiàn)三自由度機(jī)械臂軌跡規(guī)劃源程序,輸入為空間三維坐標(biāo),輸出為相應(yīng)電機(jī)需要旋轉(zhuǎn)的角度

代碼片段和文件信息
#include
#include
#include
#include
#include
#include?
#include?
const?double?p?=3.1415926;
using?namespace?std;
double?l1=100.0l2=100.0;
inline?void?swap(double?&adouble?&b)
{
double?c=a;a=b;b=c;
}
//求逆函數(shù)
void?MatrixInV(double?M[20][20]int?ndouble*?Q)???
{
int?ijk;
double?d;
int?JS[20]IS[20];
for?(k=0;k d=0;
for?(i=k;i for?(j=k;j if?(fabs(M[i][j])>d){
d=fabs(M[i][j]);
IS[k]=i;
JS[k]=j;
????????????????}
????????????}
}
//出錯(cuò)處理?
if?(d+1.0==1.0)?
return;
if?(IS[k]!=k)
for?(j=0;j swap(M[k][j]M[IS[k]][j]);
if?(JS[k]!=k)
for?(i=0;i swap(M[i][k]M[i][JS[k]]);
M[k][k]=1/M[k][k];
for?(j=0;j if?(j!=k)?
M[k][j]=M[k][j]*M[k][k];
for?(i=0;i if?(i!=k)
for?(j=0;j if?(j!=k)?
M[i][j]=M[i][j]-M[i][k]*M[k][j];
for?(i=0;i if?(i!=k)?
M[i][k]=-M[i][k]*M[k][k];
}
for?(k=n-1;k>=0;k--){
for?(j=0;j if?(JS[k]!=k)?
swap(M[k][j]M[JS[k]][j]);
for?(i=0;i if?(IS[k]!=k)?
swap(M[i][k]M[i][IS[k]]);
????}
int?t=0;
for?(int?c=0;c<20;c++){
for?(int?d=0;d<20;d++){?
Q[t]=M[c][d];
t++;
????}
????}
}
//求系數(shù)函數(shù)
void?coefficient(double?A[6]double*?Z)????????????
{
double?N[20]={A[0]0A[1]A[1]00A[2]A[2]00A[3]A[3]00A[4]A[4]00A[5]0};
//求系數(shù)矩陣
double?M[20][20]={{10000000000000000000}
??????????????????{01000000000000000000}
??????????????????{11110000000000000000}
??????????????????{00001000000000000000}
??????????????????{01230-100000000000000}
??????????????????{002600-20000000000000}
??????????????????{00001111000000000000}
??????????????????{00000000100000000000}
??????????????????{000001230-10000000000}
??????????????????{0000002600-2000000000}
??????????????????{00000000111100000000}
??????????????????{00000000000010000000}
??????????????????{0000000001230-1000000}
??????????????????{00000000002600-200000}
??????????????????{00000000000011110000}
??????????????????{00000000000000001000}
??????????????????{00000000000001230-100}
??????????????????{000000000000002600-20}
??????????????????{00000000000000001111}
??????????????????{00000000000000000123}};???
double?Q[20*20]={0};
double?B[20]={0};
double?G[20]={0};
int?t=0;
int?k=0;
MatrixInV(M20Q);
for?(int?i=0;i<20;i++){???????????
for?(int?j=0;j<20;j++){??????
B[j]=Q[t]*N[j];?????
G[i]+=B[j];
t++;
}
}
?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件???????8328??2011-09-25?16:13??31\31\31.cpp
?????文件???????3906??2011-09-21?10:20??31\31\31.vcproj
?????文件???????1407??2011-09-28?18:53??31\31\31.vcproj.THINK.IBM.user
?????文件????????663??2011-09-21?10:20??31\31\Debug\31.exe.em
?????文件????????728??2011-09-21?10:20??31\31\Debug\31.exe.em
?????文件????????621??2011-09-25?16:13??31\31\Debug\31.exe.intermediate.manifest
?????文件??????80299??2011-09-25?16:13??31\31\Debug\31.obj
?????文件???????5828??2011-09-25?16:13??31\31\Debug\BuildLog.htm
?????文件?????????65??2011-09-25?16:13??31\31\Debug\mt.dep
?????文件?????183296??2011-09-25?16:13??31\31\Debug\vc90.idb
?????文件?????217088??2011-09-25?16:13??31\31\Debug\vc90.pdb
?????文件????1780736??2011-09-28?18:53??31\31.ncb
?????文件????????872??2011-09-21?10:10??31\31.sln
????..A..H.?????13824??2011-09-28?18:53??31\31.suo
?????文件??????56320??2011-09-25?16:13??31\Debug\31.exe
?????文件?????440784??2011-09-25?16:13??31\Debug\31.ilk
?????文件?????625664??2011-09-25?16:13??31\Debug\31.pdb
?????目錄??????????0??2011-09-25?16:13??31\31\Debug
?????目錄??????????0??2011-09-25?16:13??31\31
?????目錄??????????0??2011-09-25?16:07??31\Debug
?????目錄??????????0??2011-09-21?10:20??31
-----------?---------??----------?-----??----
??????????????3420429????????????????????21
評(píng)論
共有 條評(píng)論