CSARA机械手正反解代码解读和左右手定则应用

前言：前段时间在某鱼上买了一份CSARA的机械臂的程序，拿出来分享一下，并记录一下。说明一下并非是公司的核心代码，我也不搞这个....侵权就删了。

首先简单回顾一下CSARA的正逆解。

根据几何的方法能求出末端在平面坐标系中的xy坐标。也就是正解。知道了各个关节的角度，求末端的位置。 一般的思路是这样的，但是有一个左右手定则的方式，也就是可以建立不同的坐标系。左右手定则，一般使用在还有摄像头的一个情况，摄像头进行识别的情况。

比如你建立的坐标系刚好是右手定则，而摄像头看物体就是符合左手定则，所以会有左右手坐标转换。如图

我们假设机器关节的theta2角度在右手定则是大于180度的，比如280度，而在左手定则中却是360-270 = 80的，进而对其转换成统一坐标系的。

左右手定则公式：

推理是很简单的，仅仅用到了三角函数和三角形求角度的通用公式。注意一点哈，左右手的主要目的是对其坐标系的转换。左->右或者右->左。

写成代码则是：

 看的出来，实际上主要还是a的不同，通过a来进行转换的。

好了，左右手的用法已经知道了，相信利用几何法也能够进行出正逆解的公式。大家可以自行推导，这片文章的重点并不在这里。

主代码：

int main()
{

	ScaraLib scara;
	scara.createUserCrd(cv::Point2f(100,100),90.0/180.0*CV_PI);

	cv::Point2f rp1(100*sqrt(2), 0);
	cv::Point2f cp2;
	scara.cvtRobotUserCrd(rp1, cp2);
	double atn = atan2(1, 2);
	//反解
	scara.setXYZC(34, 22, 0, 0.56);
	double j1, j2, j3, j4;
	scara.getJn(j1, j2, j3, j4);
	double x, y,