xzl.cpp

实现4自由度scara机械手的点对点运动,用Jacobi实现坐标变换

#include <time.h>
#include "matrix.h"

#ifndef _NO_NAMESPACE
using namespace std;
using namespace math;
#define STD std
#else
#define STD
#endif

#ifndef _NO_TEMPLATE
typedef matrix<double> Matrix;
#else
typedef matrix Matrix;
#endif

#ifndef _NO_EXCEPTION
#  define TRYBEGIN()	try {
#  define CATCHERROR()	} catch (const STD::exception& e) { \
						cerr << "Error: " << e.what() << endl; }
#else
#  define TRYBEGIN()
#  define CATCHERROR()
#endif

#define PI 3.14159265

int main()
{
	double height,arm1,arm2,angle1,angle2,vertical,terminal;
	double x=1,y=2,z=3;

	cout<<"输入台高:  ";
	cin>>height;
	cout<<endl<<"输入第一条臂长:  ";
	cin>>arm1;
	cout<<endl<<"输入第二条臂长:  ";
	cin>>arm2;
    cout<<endl<<"输入角度1:  ";
	cin>>angle1;
	cout<<endl<<"输入角度2:  ";
	cin>>angle2;
	cout<<endl<<"输入垂直距离:  ";
	cin>>vertical;
	cout<<endl<<"输入末端旋转角度:  ";
	cin>>terminal;

	cout<<endl<<"输入末端位置(x, y, z):  ";
 	cin>>x>>y>>z;

	angle1=angle1/180*PI;
	angle2=angle2/180*PI;
	terminal=terminal/180*PI;

	Matrix first;
	Matrix second;
	Matrix third;
	Matrix fourth;

	first.SetSize(4,4);
	second.SetSize(4,4);
	third.SetSize(4,4);
	fourth.SetSize(4,4);

	first(0,0)=cos(angle1);
	first(0,1)=0-sin(angle1);
	first(0,2)=0;
	first(0,3)=0;
	first(1,0)=sin(angle1);
	first(1,1)=cos(angle1);
	first(1,2)=0;
	first(1,3)=arm1;
	first(2,0)=0;
	first(2,1)=0;
	first(2,2)=1;
	first(2,3)=height;
	first(3,0)=0;
	first(3,1)=0;
	first(3,2)=0;
	first(3,3)=1;

	second(0,0)=cos(angle2);
	second(0,1)=0-sin(angle2);
	second(0,2)=0;
	second(0,3)=0;
	second(1,0)=sin(angle2);
	second(1,1)=cos(angle2);
	second(1,2)=0;
	second(1,3)=arm2;
	second(2,0)=0;
	second(2,1)=0;
	second(2,2)=1;
	second(2,3)=0;
	second(3,0)=0;
	second(3,1)=0;
	second(3,2)=0;
	second(3,3)=1;

	third(0,0)=1;
	third(0,1)=0;
	third(0,2)=0;
	third(0,3)=0;
	third(1,0)=0;
	third(1,1)=1;
	third(1,2)=0;
	third(1,3)=0;
	third(2,0)=0;
	third(2,1)=0;
	third(2,2)=1;
	third(2,3)=vertical;
	third(3,0)=0;
	third(3,1)=0;
	third(3,2)=0;
 	third(3,3)=1;

	fourth(0,0)=cos(terminal);
	fourth(0,1)=0-sin(terminal);
	fourth(0,2)=0;
	fourth(0,3)=0;
	fourth(1,0)=sin(terminal);
	fourth(1,1)=cos(terminal);
	fourth(1,2)=0;
	fourth(1,3)=0;
	fourth(2,0)=0;
	fourth(2,1)=0;
	fourth(2,2)=1;
	fourth(2,3)=0;
	fourth(3,0)=0;
	fourth(3,1)=0;
	fourth(3,2)=0;
 	fourth(3,3)=1;

	Matrix T;
	T.SetSize(4,4);
	T=first*second*third*fourth;
	
	Matrix p;
	p.SetSize(4,1);
	p(0,0)=x;
	p(1,0)=y;
	p(2,0)=z;
	p(3,0)=1;

	Matrix q;
	q=T*p;

	cout<<"the result is: "<<endl<<q;

	return 0;

}