索驱动柔性机器人三自由度工业机器人工业应用matlab翻译成C#版（初版）（傅里叶轨迹弃案）
仅以此迹记录我出差半年与广东一家公司的合作，与清华博士们合作翻译的索驱动机器人算法。
出差半年，从干软件的Java程序员。到广东这边学习工业机器人安装，维修，认识了许许多多的硬件，开发上位机，开发下位机，开发示教器，学会使用海康视觉等等等。也有幸认识了一些清华的博士们，大牛，也与工业界的泰山北斗面对面交谈。
个人在工作与学习的过程中发现工业机器人的开发与设计与数学有着紧密的联系，特别是在机器人标定过程中博士们基本上使用的是最小二乘法这种优化拟合的知识，与优秀的人在一起总是会感觉到自己的不足。
namespace Robot.utils
{
public class FourierTrajectory : UserControl
{
public static int flag = 1;
//静平台坐标
public static double[] staticPlatformCoordinateOne = new double[3];
public static double[] staticPlatformCoordinateTwo = new double[3];
public static double[] staticPlatformCoordinateThree = new double[3];
//动平台坐标
public static double[] movingPlatformCoordinateOne = new double[3];
public static double[] movingPlatformCoordinateTwo = new double[3];
public static double[] movingPlatformCoordinateThree = new double[3];
public static double ubf = 1;
public static double lbf = 0.05f;
//伺服周期单位ms 默认值为4
private int servoCycle = 4;
private double m = 5.3;
//double t = 1;
double T = 0;
double dt = 0.004;
//重力加速度
//private Matrix<double> g = Matrix<double>.Build.DenseOfArray(new double[,] { { 0 }, { 0 }, { 9.8 } });
//private double w = 1;
//运动时间
//private double T = 1;
private List<PointInfo> points = new List<PointInfo>();
List<Vector<double>> staticVec = new List<Vector<double>>();
List<Vector<double>> moveVec = new List<Vector<double>>();
Matrix<double> pointsMatrix;
Vector<double> p0;
double[] initialCableLength = new double[6];
double[] w = null;
double[] T0 = null;
Matrix<double> Prec = Matrix<double>.Build.Dense(99999, 3);
Matrix<double> Vrec = Matrix<double>.Build.Dense(99999, 3);
Matrix<double> Arec = Matrix<double>.Build.Dense(99999, 3);
List<double> trec = new List<double>();
//Vector<double> trec = Vector<double>.Build.DenseOfArray(new double[274]);
Vector<double> a1 = Vector<double>.Build.DenseOfArray(new double[] { 0.115470053837925, 0.200000000000000, 0 });
Vector<double> a2 = Vector<double>.Build.DenseOfArray(new double[] { 0.115470053837925, 0.200000000000000, 0 });
Vector<double> a3 = Vector<double>.Build.DenseOfArray(new double[] { -0.230940107675850, 0, 0 });
Vector<double> a4 = Vector<double>.Build.DenseOfArray(new double[] { -0.230940107675850, 0, 0 });
Vector<double> a5 = Vector<double>.Build.DenseOfArray(new double[] { 0.115470053837925, -0.200000000000000 , 0 });
Vector<double> a6 = Vector<double>.Build.DenseOfArray(new double[] { 0.115470053837925 , -0.200000000000000 , 0 });
Vector<double> B1 = Vector<double>.Build.DenseOfArray(new double[] { 0.644550624006581 , 0.200000000000000, 0 });
Vector<double> B2 = Vector<double>.Build.DenseOfArray(new double[] { -0.149070231246403 , 0.624863881081478 , 0 });
Vector<double> B3 = Vector<double>.Build.DenseOfArray(new double[] { -0.495480392760178, 0.424863881081478 , 0 });
Vector<double> B4 = Vector<double>.Build.DenseOfArray(new double[] { -0.495480392760178 , -0.424863881081478 , 0 });
Vector<double> B5 = Vector<double>.Build.DenseOfArray(new double[] { -0.149070231246403 , -0.624863881081478 , 0 });
Vector<double> B6 = Vector<double>.Build.DenseOfArray(new double[] { 0.644550624006581, -0.200000000000000 , 0 });
Vector<double> c = Vector<double>.Build.DenseOfArray(new double[] {0, 0, 0 });
Vector<double> g = Vector<double>.Build.DenseOfArray(new double[] {0, 0, 9.8 });
Vector<double> F1rec = Vector<double>.Build.DenseOfArray(new double[99999]);
Vector<double> F2rec = Vector<double>.Build.DenseOfArray(new double[99999]);
Vector<double> F3rec = Vector<double>.Build.DenseOfArray(new double[99999]);
Vector<double> F4rec = Vector<double>.Build.DenseOfArray(new double[99999]);
Vector<double> F5rec = Vector<double>.Build.DenseOfArray(new double[99999]);
Vector<double> F6rec = Vector<double>.Build.DenseOfArray(new double[99999]);
double[] Fminrec = new double[99999];
Matrix<double> E;
Vector<double> FL;
Vector<double> FFxyz;
Vector<double> AM;
Matrix<double> Mxyz = Matrix<double>.Build.Dense(99999,3);
Matrix<double> AM0;
Matrix<double> M = Matrix<double>.Build.Dense(99999,3);
Matrix<double> MF1rec = Matrix<double>.Build.Dense(99999,3);
Matrix<double> MF2rec = Matrix<double>.Build.Dense(99999,3);
Matrix<double> MF3rec = Matrix<double>.Build.Dense(99999,3);
Matrix<double> M0 = Matrix<double>.Build.Dense(99999,3);
double[] D1rec = new double[99999];
double[] D2rec = new double[99999];
double[] D3rec = new double[99999];
double[] Rtrec = new double[99999];
double[] TRAI = new double[99999];
double[] TRBI = new double[99999];
List<FourierTrajectoryPoints> fourierTrajectoryPoints;
/**
%%
% 参数初始化，c为重心，ai为绳索锚点相对于动平台坐标P的位置，Bi为出索点相对于静平台原点O的位置
L=1;
l=1/1.1;
L=l/sqrt(3)*2;
%L=1.14;
h=1;
l0=0.2;
h0=0;
dh0=2;
h1=h0-dh0/2;
h2=h0+dh0/2;
c=[0,0,0]';
*/
public void initializeModelParameters(int flag,int servoCycle)
{
this.servoCycle = servoCycle;
MainView.readMRFDevStatus();
if (flag == 1)
{
staticPlatformCoordinateOne[0] = MainView.mrfd.StaticPlatPos1X;
staticPlatformCoordinateOne[1] = MainView.mrfd.StaticPlatPos1Y;
staticPlatformCoordinateOne[2] = MainView.mrfd.StaticPlatPos1Z;
staticPlatformCoordinateTwo[0] = MainView.mrfd.StaticPlatPos2X;
staticPlatformCoordinateTwo[1] = MainView.mrfd.StaticPlatPos2Y;
staticPlatformCoordinateTwo[2] = MainView.mrfd.StaticPlatPos2Z;
staticPlatformCoordinateThree[0] = MainView.mrfd.StaticPlatPos3X;
staticPlatformCoordinateThree[1] = MainView.mrfd.StaticPlatPos3Y;
staticPlatformCoordinateThree[2] = MainView.mrfd.StaticPlatPos3Z;
movingPlatformCoordinateOne[0] = MainView.mrfd.MovePlatPos1X;
movingPlatformCoordinateOne[1] = MainView.mrfd.MovePlatPos1Y;
movingPlatformCoordinateOne[2] = MainView.mrfd.MovePlatPos1Z;
movingPlatformCoordinateTwo[0] = MainView.mrfd.MovePlatPos2X;
movingPlatformCoordinateTwo[1] = MainView.mrfd.MovePlatPos2Y;
movingPlatformCoordinateTwo[2] = MainView.mrfd.MovePlatPos2Z;
movingPlatformCoordinateThree[0] = MainView.mrfd.MovePlatPos3X;
movingPlatformCoordinateThree[1] = MainView.mrfd .MovePlatPos3Y;
movingPlatformCoordinateThree[2] = MainView .mrfd .MovePlatPos3Z;
}
}
public void pointsInit(List<PointInfo> points)
{
this.points = points;
//pointsMatrix = Matrix<double>.Build.DenseOfArray(new double[points.Count,7]);
//for (int i = 0; i < points.Count; i++)
//{
// pointsMatrix[i,0] = points[i].X;
// pointsMatrix[i,1] = points[i].Y;
// pointsMatrix[i,2] = points[i].Z;
// pointsMatrix[i,3] = points[i].AccX;
// pointsMatrix[i,4] = points[i].AccY;
// pointsMatrix[i,5] = points[i].AccZ;
// pointsMatrix[i,6] = points[i].Time;
//}
pointsMatrix = Matrix<double>.Build.DenseOfArray(new double[,] {{
0,0,1.2,0,0,0,1},
{0.2,0.25,1.2,0,0,0,1},
{ -0.05,-0.5,1.3,0,0,0,1},
{-0.15,0.8,1,0,0,0,1},
{ 0.4,-0.7,1.4,0,0,0,1},
{ -0.6,0.6,1.6,0,0,0,1},
{ 0.7,-0.6,1.6,0,0,0,1} });
staticVec.Add(Vector<double>.Build.DenseOfArray(new double[] { staticPlatformCoordinateOne[0], staticPlatformCoordinateOne[1], staticPlatformCoordinateOne[2] }));
staticVec.Add(Vector<double>.Build.DenseOfArray(new double[] { staticPlatformCoordinateTwo[0], staticPlatformCoordinateTwo[1], staticPlatformCoordinateTwo[2] }));
staticVec.Add(Vector<double>.Build.DenseOfArray(new double[] { staticPlatformCoordinateThree[0], staticPlatformCoordinateThree[1], staticPlatformCoordinateThree[2] }));
moveVec.Add(Vector<double>.Build.DenseOfArray(new double[] { movingPlatformCoordinateOne[0], movingPlatformCoordinateOne[1], movingPlatformCoordinateOne[2] }));
moveVec.Add(Vector<double>.Build.DenseOfArray(new double[] { movingPlatformCoordinateTwo[0], movingPlatformCoordinateTwo[1], movingPlatformCoordinateTwo[2] }));
moveVec.Add(Vector<double>.Build.DenseOfArray(new double[] { movingPlatformCoordinateThree[0], movingPlatformCoordinateThree[1], movingPlatformCoordinateThree[2] }));
initialCableLength = new double[6];
p0 = pointsMatrix.Row(0).SubVector(0, 3);
for (int i = 0;i < staticVec.Count;i++)
{
Vector<double> difference = moveVec[i].Subtract(p0.Add(staticVec[i]));
double len = difference.Norm(2);
initialCableLength[2*i] = len;
initialCableLength[2*i+1] = len;
}
//w为频率所决定的节拍
//w = new double[7];
//T0 = new double[7];
w = new double[7];
T0 = new double[7];
generateTrajectory();
//Vector<double> difference = B1.Subtract(p0.Add(a1));
//double l1 = difference.Norm();
}
/// <summary>
/// 生成轨迹
/// </summary>
private void generateTrajectory()
{
double[] rx = new double[7];
double[] ry = new double[7];
double[] rz = new double[7];
int fLessThanzero = 0;
fourierTrajectoryPoints = new List<FourierTrajectoryPoints>();
//
//for (int i = 0; i < 7; i++)
for (int i = 0; i < 6; i++)
{
//BBx=(-X(i+1,1)+X(i,1))/2;
//w(i) = 1 * sqrt(g(3) * 2 / (X(i, 3) + X(i + 1, 3)));
//T0(i) = pi / w(i);
//X(i, 7) = T0(i);
//t = dt:dt: T0(i);
w[i] = 1 * Math.Sqrt(9.8 * 2 / (pointsMatrix[i, 2] + pointsMatrix[i + 1, 2]));
T0[i] = Math.PI / w[i];
pointsMatrix[i, 6] = T0[i];
var intermediateVariables = Enumerable.Range(1, (int)(T0[i] / dt) + 1).Select(i => T + i * dt).ToArray();
foreach (var variable in intermediateVariables)
{
trec.Add(variable);
}
Vector<double> t = Vector<double>.Build.Dense(Enumerable.Range(1, (int)(T0[i] / dt) + 1).Select(i => i * dt).ToArray());
/**
%AAx为论文中的Ai BBx为论文中的Bi rx为应该为对应的Mi
%x为修正后的轨迹形式 论文第79页
%vx为x的修正后的轨迹的一阶导数，即为速度
%ax为x的修正后的轨迹的二阶导数，即为加速度
BBx=(-X(i+1,1)+X(i,1))/2;
rx(i)=(-BBx-X(i,4)/w(i)/w(i))/4;
AAx=(X(i,1)+X(i+1,1))/2-rx(i);
x=AAx+BBx*cos(w(i)*t)+rx(i)*cos(2*w(i)*t);
vx=-w(i)*BBx*sin(w(i)*t)-2*w(i)*rx(i)*sin(2*w(i)*t);
ax=-w(i)*w(i)*BBx*cos(w(i)*t)-4*w(i)*w(i)*rx(i)*cos(2*w(i)*t);
*/
double BBx = (pointsMatrix[i, 0] - pointsMatrix[i+1,0]) / 2;
rx[i] = (-BBx - pointsMatrix[i, 3] / (w[i] * w[i]))/4;
double AAx = (pointsMatrix[i,0] + pointsMatrix[i+1,0])/2-rx[i];
//因为t是一个从以dt为起始值dt为步长的向量 ，所以其算出来的值也应该是等长度的向量
Vector<double> x = Vector<double>.Build.Dense(Enumerable.Range(0,t.Count)
.Select(index => AAx + BBx * Math.Cos(w[i] * t[index]) + rx[i] * Math.Cos(2 * w[i] * t[index])).ToArray());
Vector<double> vx = Vector<double>.Build.Dense(Enumerable.Range(0,t.Count)
.Select(index => -w[i] * BBx * Math.Sin(w[i] * t[index]) - 2 * w[i] * rx[i] * Math.Sin(2 * w[i] * t[index])).ToArray());
Vector<double> ax = Vector<double>.Build.Dense(Enumerable.Range(0,t.Count)
.Select(index => -Math.Sqrt(w[i]) * BBx * Math.Cos(w[i] * t[index]) - 4 * Math.Sqrt(w[i]) * rx[i] * Math.Cos(2 * w[i] * t[index])).ToArray());
//double x = AAx + BBx * Math.Cos(w[i]*t) + rx[i] * Math.Cos(2 * w[i]*t);
//double vx = -w[i] * BBx * Math.Sin(w[i]*t) - 2 * w[i] * rx[i] * Math.Sin(2 * w[i]*t);
//double ax = -Math.Sqrt(w[i]) * BBx * Math.Cos(w[i] * t) - 4 * Math.Sqrt(w[i]) * rx[i]*Math.Cos(2 * w[i]*t);
/**
BBy=(-X(i+1,2)+X(i,2))/2;
ry(i)=(-BBy-X(i,5)/w(i)/w(i))/4;
AAy=(X(i,2)+X(i+1,2))/2-ry(i);
y=AAy+BBy*cos(w(i)*t)+ry(i)*cos(2*w(i)*t);
vy=-w(i)*BBy*sin(w(i)*t)-2*w(i)*ry(i)*sin(2*w(i)*t);
ay=-w(i)*w(i)*BBy*cos(w(i)*t)-4*w(i)*w(i)*ry(i)*cos(2*w(i)*t);
*/
double BBy = (pointsMatrix[i,1] - pointsMatrix[i+1,1]) / 2;
ry[i] = (-BBy - pointsMatrix[i, 4] / Math.Sqrt(w[i])) / 4;
double AAy = (pointsMatrix[i,1] + pointsMatrix[i+1,1]) / 2 - ry[i];
Vector<double> y = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count)
.Select(index => AAy + BBy * Math.Cos(w[i] * t[index]) + ry[i] * Math.Cos(2 * w[i] * t[index])).ToArray());
Vector<double> vy = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count)
.Select(index => -w[i] * BBy * Math.Sin(w[i] * t[index]) - 2 * w[i] * ry[i] * Math.Sin(2 * w[i] * t[index])).ToArray());
Vector<double> ay = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count)
.Select(index => -Math.Sqrt(w[i]) * BBy * Math.Cos(w[i] * t[index]) - 4 * Math.Sqrt(w[i]) * ry[i] * Math.Cos(2 * w[i] * t[index])).ToArray());
//double y = AAy + BBy* Math.Cos(w[i] * t) + ry[i] * Math.Cos(2 * w[i]*t);
//double vy = -w[i] * BBy * Math.Sin(w[i] * t) - 2 * w[i] * ry[i] * Math.Sin(2 * w[i]*t);
//double ay = -Math.Sqrt(w[i]) * BBy * Math.Cos(w[i] * t) - 4 * Math.Sqrt(w[i]) * ry[i] * Math.Cos(2 * w[i] * t);
/**
zd=(-X(i+1,3)+X(i,3))/2;
BBz=zd/8*9;
rz(i)=-zd/8;
AAz=(X(i,3)+X(i+1,3))/2;
z=AAz+BBz*cos(w(i)*t)+rz(i)*cos(3*w(i)*t);
vz=-w(i)*BBz*sin(w(i)*t)-3*w(i)*rz(i)*sin(3*w(i)*t);
az=-w(i)*w(i)*BBz*cos(w(i)*t)-9*w(i)*w(i)*rz(i)*cos(3*w(i)*t);
*/
double zd = (pointsMatrix[i, 2] - pointsMatrix[i + 1, 2]) / 2;
double BBz = zd / 8 * 9;
rz[i] = -zd / 8;
double AAz = (pointsMatrix[i, 2] + pointsMatrix[i + 1, 2]) / 2;
Vector<double> z = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count).Select(index => AAz + BBz * Math.Cos(w[i] * t[index]) + rz[i] * Math.Cos(3 * w[i] * t[index])).ToArray());
Vector<double> vz = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count)
.Select(index => -w[i] * BBz * Math.Sin(w[i] * t[index]) - 3 * w[i] * rz[i] * Math.Sin(3 * w[i] * t[index])).ToArray());
Vector<double> az = Vector<double>.Build.Dense(Enumerable.Range(0, t.Count)
.Select(index => -Math.Sqrt(w[i]) * BBz * Math.Cos(w[i] * t[index]) - 9 * Math.Sqrt(w[i]) * rz[i] * Math.Cos(3 * w[i] * t[index])).ToArray());
//double z = AAz + BBz * Math.Cos(w[i] * t) + rz[i] * Math.Cos(3 * w[i] * t);
//double vz = -w[i] * BBz * Math.Sin(w[i] * t) - 3 * w[i] *rz[i] * Math.Sin(3 * w[i] * t);
//double az = -Math.Sqrt(w[i]) * BBz * Math.Cos(w[i] * t) - 9 * Math.Sqrt(w[i]) * rz[i] * Math.Cos(3 * w[i] * t);
//Vector<double> prec0 = Vector<double>.Build.DenseOfArray(new double[] { x ,y, z }) ;
//Vector<double> prec0 = Vector<double>.Build.DenseOfEnumerable(Enumerable.Range(0, x.Count)
// .Select(index => new double[] { x[index], y[index], z[index] })
// .SelectMany(array => array));
Matrix<double> prec0 = Matrix<double>.Build.DenseOfColumns(new List<Vector<double>>
{
Vector<double>.Build.DenseOfEnumerable(x),
Vector<double>.Build.DenseOfEnumerable(y),
Vector<double>.Build.DenseOfEnumerable(z)
});
Matrix<double> vrec0 = Matrix<double>.Build.DenseOfColumns(new List<Vector<double>>
{
Vector<double>.Build.DenseOfEnumerable(vx),
Vector<double>.Build.DenseOfEnumerable(vy),
Vector<double>.Build.DenseOfEnumerable(vz)
});
//Vector<double> vrec0 = Vector<double>.Build.DenseOfEnumerable(Enumerable.Range(0, x.Count)
// .Select(index => new double[] { vx[index], vy[index], vz[index] })
// .SelectMany(array => array));
Matrix<double> arec0 = Matrix<double>.Build.DenseOfColumns(new List<Vector<double>>
{
Vector<double>.Build.DenseOfEnumerable(ax),
Vector<double>.Build.DenseOfEnumerable(ay),
Vector<double>.Build.DenseOfEnumerable(az)
});
//Vector<double> arec0 = Vector<double>.Build.DenseOfEnumerable(Enumerable.Range(0, x.Count)
// .Select(index => new double[] { ax[index], ay[index], az[index] })
// .SelectMany(array => array));
//Vector<double> vrec0 = Vector<double>.Build.DenseOfArray(new double[] { vx ,vy, vz }) ;
//Vector<double> arec0 = Vector<double>.Build.DenseOfArray(new double[] { ax ,ay, az }) ;
for (int i_t = 1; i_t <= pointsMatrix[i, 6] / dt; i_t++)
{
double mt = i_t * dt;
Vector<double> P = Vector<double>.Build.DenseOfArray(new double[] { prec0[i_t, 0], prec0[i_t, 1], prec0[i_t, 2] });
Vector<double> v = Vector<double>.Build.DenseOfArray(new double[] { vrec0[i_t, 0], vrec0[i_t, 1], vrec0[i_t, 2] });
Vector<double> a = Vector<double>.Build.DenseOfArray(new double[] { arec0[i_t, 0], arec0[i_t, 1], arec0[i_t, 2] });
Prec.SetRow(trec.Count + i_t, P);
Vrec.SetRow(trec.Count + i_t, v);
Arec.SetRow(trec.Count + i_t, a);
Vector<double> A1 = P + a1;
Vector<double> A2 = P + a2;
Vector<double> A3 = P + a3;
Vector<double> A4 = P + a4;
Vector<double> A5 = P + a5;
Vector<double> A6 = P + a6;
Vector<double> e1 = (B1 - A1).Normalize(2);
Vector<double> e2 = (B2 - A2).Normalize(2);
Vector<double> e3 = (B3 - A3).Normalize(2);
Vector<double> e4 = (B4 - A4).Normalize(2);
Vector<double> e5 = (B5 - A5).Normalize(2);
Vector<double> e6 = (B6 - A6).Normalize(2);
Matrix<double> C1 = CrossMatrix(a1);
Matrix<double> C2 = CrossMatrix(a2);
Matrix<double> C3 = CrossMatrix(a3);
Matrix<double> C4 = CrossMatrix(a4);
Matrix<double> C5 = CrossMatrix(a5);
Matrix<double> C6 = CrossMatrix(a6);
Matrix<double> C = CrossMatrix(c);
// Matrix<double> Aeq = Matrix<double>.Build.DenseOfColumns(new List<Vector<double>> { e1, e2, e3, e4, e5, e6,
//C1 * e1, C2 * e2, C3 * e3, C4 * e4, C5 * e5, C6 * e6 });
//矩阵乘以向量 Aeq与beq维度保持一致此处应是 两个6X6的矩阵相乘 牛顿第二定律得到F
Matrix<double> Aeq1 = Matrix<double>.Build.DenseOfColumnVectors(e1, e2, e3, e4, e5, e6);
Matrix<double> Aeq2 = Matrix<double>.Build.DenseOfColumnVectors(C1.Multiply(e1),
C2.Multiply(e2), C3.Multiply(e3),
C4.Multiply(e4), C5.Multiply(e5), C6.Multiply(e6));
Matrix<double> Aeq = Matrix<double>.Build.Dense(6,6);
Aeq.SetSubMatrix(0,0,Aeq1);
Aeq.SetSubMatrix(3,0,Aeq2);
Vector<double> beqm = -C * g * m;
Vector<double> f = -m * g + m * a;
Vector<double> beq = Vector<double>.Build.DenseOfArray(new double[] { f[0], f[1], f[2], beqm[0], beqm[1], beqm[2] });
Vector<double> F = Aeq.Solve(beq);
F1rec[trec.Count + i_t] = F[0];
F2rec[trec.Count + i_t] = F[1];
F3rec[trec.Count + i_t] = F[2];
F4rec[trec.Count + i_t] = F[3];
F5rec[trec.Count + i_t] = F[4];
F6rec[trec.Count + i_t] = F[5];
Fminrec[trec.Count + i_t] = F.Min();
E = Matrix<double>.Build.DenseOfColumnVectors(e1,e3,e5);
FL = Vector<double>.Build.DenseOfArray(new double[] {F[0] + F[5], F[1] + F[2], F[3] + F[4]});
FFxyz = FL[0] * e1 + FL[1] * e2 + FL[2] * e3;
Matrix<double> FFF = Matrix<double>.Build.DenseOfArray(new double[,]
{
{ FL[0] , 0 , 0 },
{ 0 , FL[1] , 0 },
{ 0 , 0 , FL[2] }
}
);
Vector<double> column1 = FL[0] / 2 * CrossProduct(a1 - a6, e1);
Vector<double> column2 = FL[1] / 2 * CrossProduct(a3 - a2, e3);
Vector<double> column3 = FL[2] / 2 * CrossProduct(a5 - a4, e5);
Matrix<double> AM = Matrix<double>.Build.DenseOfColumnVectors(column1, column2, column3);
Vector<double> a11 = CrossMatrix(P).Multiply(FFxyz);
Vector<double> a10 = (B1 + B6)/ (a1 + a6);
Vector<double> a33 = CrossMatrix(P).Multiply(FFxyz).Divide((ImitationVectorLeftDivision((B1 + B6), (a1 + a6)).L2Norm() - 1));
Mxyz.SetRow(trec.Count + i_t , CrossMatrix(P).Multiply(FFxyz).Divide((ImitationVectorLeftDivision((B1 + B6), (a1 + a6)).L2Norm() - 1)));
Vector<double> AM0column1 = (F[0] - F[5]) / 2 * CrossProduct(a1 - a6, e1);
Vector<double> AM0column2 = (F[2] - F[1]) / 2 * CrossProduct(a3 - a2, e3);
Vector<double> AM0column3 = (F[4] - F[3]) / 2 * CrossProduct(a5 - a4, e5);
Matrix<double> AM0 = Matrix<double>.Build.DenseOfColumnVectors(AM0column1, AM0column2, AM0column3);
Vector<double> maxColumnValue = getMaxColumn(AM0.PointwiseDivide(AM));
// 获取每一列的最大值
M.SetRow(trec.Count+i_t,maxColumnValue);
Vector<double> MF1 = CrossProduct(a1 - a6, FL[0] * e1);
Vector<double> MF2 = CrossProduct(a3 - a2, FL[1] * e3);
Vector<double> MF3 = CrossProduct(a5 - a4, FL[0] * e5);
//MF1rec(length(trec) + i_t, 1:3) = cross((a1 - a6), FL(1) * e1);
//MF2rec(length(trec) + i_t, 1:3) = cross((a3 - a2), FL(2) * e3);
//MF3rec(length(trec) + i_t, 1:3) = cross((a5 - a4), FL(1) * e5);
MF1rec.SetRow(trec.Count + i_t ,CrossProduct(a1 - a6 , FL[0] * e1));
MF2rec.SetRow(trec.Count + i_t ,CrossProduct(a3 - a2 , FL[1] * e3));
MF3rec.SetRow(trec.Count + i_t ,CrossProduct(a1 - a6 , FL[0] * e5));
Vector<double> crossProductResultD1 = CrossProduct(MF3, MF2);
// D2=abs(MF2'*cross(MF1,MF3)/norm(cross(MF1,MF3)));
// D3 = abs(MF3'*cross(MF1,MF2)/norm(cross(MF1,MF2)));
double D1 = Math.Abs(MF1.DotProduct(crossProductResultD1) / crossProductResultD1.L2Norm());
Vector<double> crossProductResultD2 = CrossProduct(MF1 , MF3);
double D2 = Math.Abs(MF2.DotProduct(crossProductResultD2) / crossProductResultD2.L2Norm());
Vector<double> crossProductResultD3 = CrossProduct(MF1 , MF2);
double D3 = Math.Abs(MF3.DotProduct(crossProductResultD3) / crossProductResultD3.L2Norm());
D1rec[trec.Count + i_t] = D1;
D2rec[trec.Count + i_t] = D2;
D3rec[trec.Count + i_t] = D3;
double D = Math.Min( Math.Min(D1, D2),D3);
//Rtrec(length(trec)+i_t)=D/2*min(F)/abs(min(F));
Rtrec[trec.Count + i_t] = D / 2 * F.Min() / Math.Abs(F.Min());
//Mst1=abs(Mxyz(length(trec)+i_t,1:3)*cross(MF3,MF2)/norm(cross(MF3,MF2)));
double Mst1 = Math.Abs(Mxyz.Row(trec.Count + i_t).DotProduct(crossProductResultD1) / crossProductResultD1.L2Norm());
double Mst2 = Math.Abs(Mxyz.Row(trec.Count + i_t).DotProduct(crossProductResultD2) / crossProductResultD2.L2Norm());
double Mst3 = Math.Abs(Mxyz.Row(trec.Count + i_t).DotProduct(crossProductResultD3) / crossProductResultD3.L2Norm());
double Mstm = Math.Max(Math.Max(Mst1,Mst2),Mst3);
//TRAI(length(trec) + i_t) = m * g(3) * l0 / sqrt(3) / Rtrec(length(trec) + i_t);
//TRBI(length(trec) + i_t) = Mstm / Rtrec(length(trec) + i_t);
//M0(length(trec) + i_t, 1:3) = inv(FFF) * inv(E) * crossmatrix(P) * FFxyz / (norm((B1 + B6) / (a1 + a6)) - 1);
TRAI[trec.Count + i_t] = m * g[2] * 10 / Math.Sqrt(3) / Rtrec[trec.Count + i_t];
TRBI[trec.Count + i_t] = Mstm / Rtrec[trec.Count + i_t];
M0.SetRow(trec.Count + i_t,FFF.Inverse() * E.Inverse() * CrossMatrix(P) * FFxyz / (((B1 + B6) / (a1 - a6)).L2Norm() - 1));
//TRBI0(length(trec)+i_t)=max(abs(M(length(trec)+i_t,1:3)));
pointsMatrix[i, 3] = a[0];
pointsMatrix[i, 4] = a[1];
pointsMatrix[i, 5] = a[2];
//合并这里有问题，下标不正确也许修改
//trec.Concat(t);
fourierTrajectoryPoints.Add(
new FourierTrajectoryPoints(1, i_t, prec0[i_t, 0], prec0[i_t, 1]
, prec0[i_t, 2], F.Minimum(), F[0], F[1], F[2], F[3], F[4], F[5]
, vrec0[i_t, 0], vrec0[i_t, 1], vrec0[i_t, 2], arec0[i_t, 0]
, arec0[i_t, 1], arec0[i_t,2]));
//PalletizingParameterSettingsView.sQLiteConnection
// .Insert(
// new FourierTrajectoryPoints(1, i_t, prec0[i_t, 0], prec0[i_t, 1], prec0[i_t, 2], F.Minimum()));
if (F.Minimum() < 0)
{
fLessThanzero++;
//MessageBox.Show("点位最小受力小于0,轨迹不成立");
}
}
T = T + pointsMatrix[i, 6];
}
if (fLessThanzero > 0)
{
PalletizingParameterSettingsView.sQLiteConnection.Execute("DELETE FROM FourierTrajectoryPoints WHERE programId = ?", fourierTrajectoryPoints[0].ProgramId);
PalletizingParameterSettingsView.sQLiteConnection.InsertAll(fourierTrajectoryPoints);
}
else
{
MessageBox.Show("轨迹中存在"+ fLessThanzero + "个点位索力小于0,生成轨迹失败");
}
}
/// <summary>
/// 返回最大的列组成的向量
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
static Vector<double> getMaxColumn(Matrix<double> matrix) {
return Vector<double>.Build.Dense(matrix.ColumnCount, colIndex =>
{
return matrix.Column(colIndex).Enumerate().Max();
}); ;
}
/// <summary>
/// 叉乘矩阵
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
static Matrix<double> CrossMatrix(Vector<double> v)
{
return Matrix<double>.Build.DenseOfArray(new double[,]
{
{ 0, -v[2], v[1] },
{ v[2], 0, -v[0] },
{ -v[1], v[0], 0 }
});
}
/// <summary>
/// 两向量叉乘
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
static Vector<double> CrossProduct(Vector<double> a, Vector<double> b)
{
// 向量叉乘计算
return Vector<double>.Build.DenseOfArray(new double[]
{
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0]
});
}
/// <summary>
/// 转换成矩阵，模仿左除法进行运算，防止出现除零异常情况
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns></returns>
static Matrix<double> ImitationVectorLeftDivision(Vector<double> A, Vector<double> B)
{
将向量 B 视为列向量构造矩阵
//Matrix<double> BMatrix = Matrix<double>.Build.DenseOfColumnVectors(B);
计算矩阵 BMatrix 的逆 (向量非方阵不可用Inverse())
//Matrix<double> BInverse = BMatrix.Inverse();
将向量 A 视为列向量构造矩阵
//Matrix<double> AMatrix = Matrix<double>.Build.DenseOfColumnVectors(A);
通过矩阵的逆运算模拟左除法
//Matrix<double> resultMatrix = BInverse * AMatrix;
从结果矩阵中提取向量结果
//return resultMatrix.Column(0);
// 将向量 B 视为列向量构造矩阵
Matrix<double> BMatrix = Matrix<double>.Build.DenseOfColumnVectors(B);
// 使用最小二乘法解决线性系统 Ax = B
return BMatrix.Solve(A.ToColumnMatrix());
}
}
}
扫一扫
4024
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
