标签：inverse 界面 min double kinematics max var Math
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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。