#include<iostream>

#include <Eigen/Dense>

#include <math.h>

#include <vector>

using namespace std;

using namespace Eigen;

using Eigen::MatrixXd;

const double Dk=3.1415926/;

//定义Frame（二维笛卡尔坐标系）类

class Frame{

private:

double Ox;

double Oy;//Frame对象在WF(世界坐标系)下的原点坐标

double Angle;//方位角(弧度制)

public:

Frame();

Frame(double ox,double oy,double an);

MatrixXd CalPose();//获得齐次位姿矩阵

};

Frame::Frame() {

Ox=;Oy=;Angle=;

}

Frame::Frame(double ox,double oy,double an){

Ox=ox;Oy=oy;Angle=an*Dk;//输入为角度制

}

MatrixXd Frame::CalPose(){

MatrixXd Pose(,);

Pose(,)=cos(Angle);Pose(,)=-sin(Angle);Pose(,)=Ox;

Pose(,)=sin(Angle);Pose(,)=cos(Angle);Pose(,)=Oy;

Pose(,)=;Pose(,)=;Pose(,)=;

return Pose;

}//由原点和方位角计算位姿矩阵

//Frame定义结束

//定义Solver（运动学求解器）类

class Solver{

private:

vector<Frame> frameVec;

public:

MatrixXd CalTheta(double l1,double l2,Frame frame,double px,double py);//运动学反解

void addFrame(Frame frame);//添加任务框

void deleteFrame();    //移除任务框

};

MatrixXd Solver::CalTheta(double l1,double l2,Frame frame,double px,double py) {

MatrixXd P_task(,),P_world(,),T(,);

P_task(,)=px;P_task(,)=py;P_task(,)=;

P_world=frame.CalPose()*P_task;

T(,)=acos((px*px+py*py-l1*l1-l2*l2)//l1/l2);//取0°到180°

double beta=atan2(py,px);

double lp=sqrt(px*px+py*py);//端点模值

double fea=acos((lp*lp+l1*l1-l2*l2)//l1/lp);

T(,)=beta-fea;

return T;

}

void Solver::addFrame(Frame frame) {

frameVec.push_back(frame);

}

void Solver::deleteFrame() {

frameVec.pop_back();

}

//Solver类定义结束

//定义Robot类

class Robot{

public:

double L1;

double L2;//臂长

double theta1;

double theta2;//SCARA关节变量 （弧度角） 

Robot();

Robot(double l1,double l2,double t1,double t2);

Vector2d getTerminal();//手臂末端的世界坐标（笛卡尔坐标） ，正运动学计算

void ShowState();//状态显示

void PTPmove(Frame frame,double px,double py);//点到点运动 

};

Robot::Robot() {

L1=;L2=;theta1=;theta2=;

};

Robot::Robot(double l1,double l2,double t1,double t2){

L1=l1;L2=l2;theta1=t1*Dk;theta2=t2*Dk;//输入为角度制角

}

Vector2d Robot::getTerminal() {

double Tx,Ty;

Tx=L1*cos(theta1)+L2*cos(theta1+theta2);

Ty=L1*sin(theta1)+L2*sin(theta1+theta2);

Vector2d T(Tx,Ty);

return T;

}

void Robot::ShowState(){

cout<<"theta1="<<theta1/Dk<<endl<<"theta2="<<theta2/Dk<<endl;

}

void Robot::PTPmove(Frame frame,double px,double py){

Solver solver;

MatrixXd T(,);

T=solver. CalTheta(L1,L2,frame,px,py);

theta1=T(,);theta2=T(,);

}

//Robot定义结束

int main(){

Robot myRobot(,,,);//构造Robot对象

Frame TF1(,,),TF2(-,,),TF3(0.5,1.5,);//定义任务坐标系

myRobot.ShowState();

myRobot.PTPmove(TF1,,);

myRobot.ShowState();

myRobot.PTPmove(TF2,,);

myRobot.ShowState();

return ;

}