6轴机械手正解，反解算法；vs2017工程，vc编写的源码

源码展现了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） < 0.0001 ）
	{
		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） < 0.0001  ）
	{
		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） < 0.0001 ）
	{
		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\link-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link-cvtres.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.11060-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.11060-cvtres.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.11060.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.11060.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.15840-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.15840-cvtres.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.15840.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.15840.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.16756-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.16756-cvtres.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.16756.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.16756.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.2680-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.2680-cvtres.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.2680.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.2680.write.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.29392-cvtres.read.1.tlog

     文件          2  2018-10-10 01:16  Test6Axis\Test6Axis\Debug\link.29392-cvtres.write.1.tlog

............此处省略72个文件信息