資源簡介
源碼展現了6軸機械手的想寫正解和反解過程，算法詳細，有對話框界面演示功能，界面做的相對比較簡單，為了實際測試用，算法是完整的。
代碼片段和文件信息
#include?“StdAfx.h“
#include?“C6AxisData.h“
#include?“math.h“
C6AxisData::C6AxisData（void）
{
m_kValue?=?0;
//double?a2=175a3?=?900?a4?=?175?d1?=?495?d3?=?0?d4?=?960?d6?=?135;
m_aValue[0]?=?0;//30;//175;
m_aValue[1]?=?300;//900;
m_aValue[2]?=?30;
m_aValue[3]?=?0;
m_aValue[4]?=?0;
m_aValue[5]?=?0;
m_dValue[0]?=?0;//495;
m_dValue[1]?=?0;
m_dValue[2]?=?60;
m_dValue[3]?=?260;//960;
m_dValue[4]?=?0;
m_dValue[5]?=?0;//135;
//1610.5314?986.9349?-137.7560?這時候的xyz
m_fValue[0]?=?330;
m_fValue[1]?=?60;
m_fValue[2]?=?-260;
m_fValue[3]?=?0;
m_fValue[4]?=?0;
m_fValue[5]?=?0;
m_thtaValue[0][0]?=?0;
m_thtaValue[1][0]?=?0;
m_thtaValue[2][0]?=?0;
m_thtaValue[3][0]?=?0;
m_thtaValue[4][0]?=?0;
m_thtaValue[5][0]?=?0;
}
C6AxisData::~C6AxisData（void）
{
}
void?C6AxisData::CaculateThta（?）
{
double?kValue[4];
double?thtaValue[6][8];
//thta1?//Atan2（py/px）?-?Atan2（d3/+SQRT（?px*px?+?py*py?-?d3*d3）?）
thtaValue[0][0]??=?atan2（m_fValue[1]m_fValue[0]）?-?atan2（m_dValue[2]sqrt（m_fValue[0]*m_fValue[0]?+?m_fValue[1]*m_fValue[1]?-?m_dValue[2]*m_dValue[2]）?）;
thtaValue[0][1]??=?atan2（m_fValue[1]m_fValue[0]）?-?atan2（m_dValue[2]-sqrt（m_fValue[0]*m_fValue[0]?+?m_fValue[1]*m_fValue[1]?-?m_dValue[2]*m_dValue[2]）?）;
//thta3?Kvalue
//K?=?（Px*Px?+?Py*Py?+?Pz*Pz?+?a1*a1?-?2a1c1Px?-?2a1s1Py?-?a2*a2?-?a3*a3?-?d3*d3?-?d4*d4）/2a2
kValue[0]?=?（?m_fValue[0]*m_fValue[0]?+?m_fValue[1]*m_fValue[1]?+?m_fValue[2]*m_fValue[2]?+?m_aValue[0]*m_aValue[0]
?????????-?2*m_aValue[0]*cos（m_thtaValue[0][0]）*m_fValue[0]?-?2*m_aValue[0]*sin（m_thtaValue[0][0]）*m_fValue[1]
?-?m_aValue[1]*m_aValue[1]?-?m_aValue[2]*m_aValue[2]?-?m_dValue[2]*m_dValue[2]?-?m_dValue[3]*m_dValue[3]?）
/?（2*m_aValue[1]）;
kValue[1]?=?（?m_fValue[0]*m_fValue[0]?+?m_fValue[1]*m_fValue[1]?+?m_fValue[2]*m_fValue[2]?+?m_aValue[0]*m_aValue[0]
????????-?2*m_aValue[0]*cos（m_thtaValue[0][1]）*m_fValue[0]?-?2*m_aValue[0]*sin（m_thtaValue[0][1]）*m_fValue[1]
?-?m_aValue[1]*m_aValue[1]?-?m_aValue[2]*m_aValue[2]?-?m_dValue[2]*m_dValue[2]?-?m_dValue[3]*m_dValue[3]?）
/?（2*m_aValue[1]）;
???//θ3?=?atan2（a3/d4）?-?atan2（K/+_sqrt（a3*a3?+?d4*d4?-K*K））;
double?angle1angle2;
angle1?=?sqrt（m_aValue[2]*m_aValue[2]?+?m_dValue[3]*m_dValue[3]?-?kValue[0]*kValue[0]?）;
angle2?=?m_dValue[3];
if（?abs（angle1）? {
angle1?=?PI/2;
}else
{
angle1?=?atan2（?m_kValue??sqrt（m_aValue[2]*m_aValue[2]?+?m_dValue[3]*m_dValue[3]?-?m_kValue*m_kValue?）?）;
}
if（?abs（angle2）? {
if（?m_aValue[2]*m_dValue[3]?>=?0?）
{
angle2?=?PI/2;
}else
{
angle2?=?-PI/2;
}
}else
{
angle2?=?atan2（?m_aValue[2]m_dValue[3]）;
}
thtaValue[2][0]?=?angle2?-?angle1;
thtaValue[2][1]?=?angle2?+?angle1;
angle1?=?sqrt（m_aValue[2]*m_aValue[2]?+?m_dValue[3]*m_dValue[3]?-?kValue[1]*kValue[1]?）;
if（?abs（angle1）? {
angle1?=?PI/2;
}else
{
angle1?=?atan2（?m_kValue??sqrt（m?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????6375936??2018-10-10?01:16??Test6Axis\Debug\Test6Axis.exe
?????文件???14892276??2018-10-10?01:16??Test6Axis\Debug\Test6Axis.ilk
?????文件???25897984??2018-10-10?01:16??Test6Axis\Debug\Test6Axis.pdb
?????文件???88145920??2018-10-10?01:07??Test6Axis\ipch\test6axis-6deae2de\test6axis-fabb38f7.ipch
?????文件???????8538??2018-10-10?01:15??Test6Axis\Test6Axis\C6AxisData.cpp
?????文件????????406??2018-10-10?01:07??Test6Axis\Test6Axis\C6AxisData.h
?????文件??????23826??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\C6AxisData.obj
?????文件???????3150??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\cl.command.1.tlog
?????文件??????72340??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\CL.read.1.tlog
?????文件???????3068??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\CL.write.1.tlog
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
?????文件??????????2??2018-10-10?01:16??Test6Axis\Test6Axis\Debug\li
............此處省略72個文件信息
評論
共有 條評論
川公網安備 51152502000135號