正逆运动学解（三维）

我又来了！之前写了一个在二维直角坐标下的正逆运动学解今天写一下在三维坐标下如何进行正逆运动学的计算及程序实现。
一、正运动学
首先还是要建立一个坐标系，依然以机械臂举例，这里放一个正视图和一个俯视图。


正运动学：已知α、β、γ求C点坐标
咱们废话不多说了直接上公式
先求出AEAE=L2cos(α)+L3cos(β)
然后是X坐标X=AEcos(γ)
Y轴Y=AEsin(γ)
最后Z轴Z=L2sin(α)-L3sin(β)
转换成程序实现

#include<math.h>
#include<stdio.h>
float Alpha,Beta,Gamma,AE,L2,L3;
struct ForwardKinematics
{
   
	float x;
	float y;
	float z;
}h;
void main()
{
   
	AE=L2*