kinematics-inverse类
作者：互联网
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Leadshine.SMC.IDE.Motion;
using System.Threading;
using System.Drawing.Drawing2D;
using MathNet.Numerics.LinearAlgebra;
using System.IO;
namespace 机器人
{
public class Inverse
{
public Inverse()
{
}
double con;
double[] a;
double[] b;
double d1min, d1max;
double d2min, d2max;
double d3min, d3max;
double R1_min, R1_max;
double R2_min, R2_max;
double R3_min, R3_max;
double U1_min, U1_max;
double U12_min, U12_max;
double U2_min, U2_max;
double U22_min, U22_max;
double S3_min, S3_max;
double S32_min, S32_max;
double psi, theta, z;
public static double d1, d2, d3;
double R1, R2, R3, U1, U2, U12, U22, S3, S32;
public static double[,] A1; //记录各位置下各运动轴位置
public static double[,] B1; //记录所有当前位置到下一位置电机运动量
public static double[,] D1; //记录所有当前位置到下一位置电机脉冲量
public void inverse_run(double[] G_Z, double[] G_A, double[] G_B)
{
con = Math.PI / 180;
a = new double[3]; b = new double[3];
a[0] = 系统界面.a1; a[1] = 系统界面.a2; a[2] = 系统界面.a3;
b[0] = 系统界面.b1; b[1] = 系统界面.b2; b[2] = 系统界面.b3;
d1min = 系统界面.d1min; d1max = 系统界面.d1max;
d2min = 系统界面.d2min; d2max = 系统界面.d2max;
d3min = 系统界面.d3min; d3max = 系统界面.d3max;
R1_min = 系统界面.R1_min; R1_max = 系统界面.R1_max;
R2_min = 系统界面.R2_min; R2_max = 系统界面.R2_max;
R3_min = 系统界面.R3_min; R3_max = 系统界面.R3_max;
U1_min = 系统界面.U1_min; U1_max = 系统界面.U1_max;
U12_min = 系统界面.U12_min; U12_max = 系统界面.U12_max;
U2_min = 系统界面.U2_min; U2_max = 系统界面.U2_max;
U22_min = 系统界面.U22_min; U22_max = 系统界面.U22_max;
S3_min = 系统界面.S3_min; S3_max = 系统界面.S3_max;
S32_min = 系统界面.S3_min; S32_max = 系统界面.S3_max;
motor_pulse(G_Z, G_A, G_B);
}
public void motor_pulse( double[] G_Z, double[] G_A, double[] G_B)
{
double PKM_motor_pos0 = 211.8962010041709;
A1 = new double[G_Z.Length + 1, 3]; B1 = new double[G_Z.Length, 3]; D1 = new double[G_Z.Length, 4];
A1[0, 0] = PKM_motor_pos0;
A1[0, 1] = PKM_motor_pos0;
A1[0, 2] = PKM_motor_pos0;
for (int j = 0; j < G_Z.Length; j = j + 1)
{
psi = G_A[j]*con;
theta = G_B[j]*con;
z = G_Z[j];
inverse_position(psi, theta, z);
if (R1_min < R1 && R1 < R1_max && R2_min < R2 && R2 < R2_max && R3_min < R3 && R3 < R3_max && U1_min < U1 && U1 < U1_max && U12_min < U12
&& U12 < U12_max && U2_min < U2 && U2 < U2_max && U22_min < U22 && U22 < U22_max && S3_min < S3 && S3 < S3_max
&& d1 < d1max && d2 < d2max && d3 < d3max && d1 > d1min && d2 > d2min && d3 > d3min)
{
A1[j + 1, 0] = d1;
A1[j + 1, 1] = d2;
A1[j + 1, 2] = d3;
B1[j, 0] = A1[j + 1, 0] - A1[j, 0];//各轴相对位移量
B1[j, 1] = A1[j + 1, 1] - A1[j, 1];
B1[j, 2] = A1[j + 1, 2] - A1[j, 2];
}
else
{
MessageBox.Show("轨迹点不在空间内");
return;
}
}
int lp1 = 4; //丝杠导程
int fp = 10000; //控制器分辨率
for (int n = 0; n < G_Z.Length; n = n + 1)
{
D1[n, 0] = Math.Round(B1[n, 0] * fp / lp1);
D1[n, 1] = Math.Round(B1[n, 1] * fp / lp1);
D1[n, 2] = Math.Round(B1[n, 2] * fp / lp1);
}
}
public void inverse_position(double psi, double theta, double z)
{
var JZ = Matrix<double>.Build; //初始化一个矩阵构建对象
var XL = Vector<double>.Build; //初始化一个向量的构建对象
double phi = 0;
double x = z * Math.Tan(theta);
double y = 0; /
double[] RotateA = { 1, 0, 0, 0, Math.Cos(psi), -Math.Sin(psi), 0, Math.Sin(psi), Math.Cos(psi) };
double[] RotateB = { Math.Cos(theta), 0, Math.Sin(theta), 0, 1, 0, -Math.Sin(theta), 0, Math.Cos(theta) };
double[] RotateC = { Math.Cos(phi), -Math.Sin(phi), 0, Math.Sin(phi), Math.Cos(phi), 0, 0, 0, 1 };
var RZ = JZ.Dense(3, 3, RotateC).Transpose(); //将一维数组转化为3X3的多维数组,数组从上之下，从左至右，transpose转置
var RY = JZ.Dense(3, 3, RotateB).Transpose();
var RX = JZ.Dense(3, 3, RotateA).Transpose();
var R = RZ * RY * RX;
var vb10 = XL.Dense(3); vb10[0] =0; vb10[1] = b[0] * (-1); vb10[2] =0; //vb10 = XL.Dense(3);
var vb20 = XL.Dense(3); vb20[0] = 0; vb20[1] = b[1] ; vb20[2] = 0;
var vb30 = XL.Dense(3); vb30[0] = b[2] ; vb30[1] = 0; vb30[2] = 0;
var vb1 = R * vb10;
var vb2 = R * vb20;
var vb3 = R * vb30;
var va1 = XL.Dense(3); va1[0] =0; va1[1] = a[0] * (-1); va1[2] = 0;
var va2 = XL.Dense(3); va2[0] = 0; va2[1] = a[1] ; va2[2] = 0;
var va3 = XL.Dense(3); va3[0] = a[2] ; va3[1] = 0; va3[2] = 0;
var vp = XL.Dense(3); vp[0] = x; vp[1] = y; vp[2] = z;
var vc0 = XL.Dense(3); vc0[0] = 0; vc0[1] = 1; vc0[2] = 0;
var vc = R * vc0;
var vd = XL.Dense(3); vd[0] = 1; vd[1] = 0; vd[2] = 0;
var qvw1 = vp + vb1 - va1;
var qvw2 = vp + vb2 - va2;
var qvw3 = vp + vb3 - va3;
d1 = Math.Sqrt(qvw1[0] * qvw1[0] + qvw1[1] * qvw1[1] + qvw1[2] * qvw1[2]);
d2 = Math.Sqrt(qvw2[0] * qvw2[0] + qvw2[1] * qvw2[1] + qvw2[2] * qvw2[2]);
d3 = Math.Sqrt(qvw3[0] * qvw3[0] + qvw3[1] * qvw3[1] + qvw3[2] * qvw3[2]);
var vw1 = qvw1 / d1;
var vw2 = qvw2 / d2;
var vw3 = qvw3 / d3;
R1 = Math.Acos((vb1[0] * vw1[0] + vb1[1] * vw1[1] + vb1[2] * vw1[2]) / b[0]);
R1 = R1 / con;
U1 = Math.Acos((va1[0] * vw1[0] + va1[1] * vw1[1] + va1[2] * vw1[2]) / a[0]);
U1 = U1 / con;
U12 = Math.Acos(vd[0] * vw1[0] + vd[1] * vw1[1] + vd[2] * vw1[2]);
U12 = U12 / con;
R2 = Math.Acos((vb2[0] * vw2[0] + vb2[1] * vw2[1] + vb2[2] * vw2[2]) / b[1]);
R2 = R2 / con;
U2 = Math.Acos((va2[0] * vw2[0] + va2[1] * vw2[1] + va2[2] * vw2[2]) / a[1]);
U2 = U2 / con;
U22 = Math.Acos(vd[0] * vw2[0] + vd[1] * vw2[1] + vd[2] * vw2[2]);
U22 = U22 / con;
R3 = Math.Acos((va3[0] * vw3[0] + va3[1] * vw3[1] + va3[2] * vw3[2]) / a[2]);
R3 = R3 / con;
S3 = Math.Acos((vb3[0] * vw3[0] + vb3[1] * vw3[1] + vb3[2] * vw3[2]) / b[2]);
S3 = S3 / con;
S32 = Math.Acos(vc[0] * vw3[0] + vc[1] * vw3[1] + vc[2] * vw3[2]);
S32 = S32 / con;
}
}
}
标签：inverse,界面,min,double,kinematics,max,var,Math 来源： https://www.cnblogs.com/charles48789982/p/14070850.html