using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace RobotCalcForm
{
//Puma560六轴机械臂-逆解计算
public class Puma560IK
{
private double[,] _mdh; // 6x3矩阵 [d, a, alpha]
public Puma560IK(double[,] mdhParameters)
{
if (mdhParameters == null)
throw new ArgumentNullException("mdhParameters");
if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3)
throw new ArgumentException("MDH参数必须是6x3矩阵");
_mdh = (double[,])mdhParameters.Clone();
}
public Puma560IK()
{
// 标准PUMA560 MDH参数 [d, a, alpha]
_mdh = new double[,]
{
{ 0, 0, 0 },
{ 0, 0.180, -Math.PI/2 },
{ 0, 0.600, 0 },
{ 0.630, 0.130, -Math.PI/2 },
{ 0, 0, Math.PI/2 },
{ 0, 0, -Math.PI/2 }
};
}
public double[,] ComputeIK(double[,] Tbe)
{
// 提取目标位姿矩阵元素
double nx = Tbe[0, 0], ny = Tbe[1, 0], nz = Tbe[2, 0];
double ox = Tbe[0, 1], oy = Tbe[1, 1], oz = Tbe[2, 1];
double ax = Tbe[0, 2], ay = Tbe[1, 2], az = Tbe[2, 2];
double px = Tbe[0, 3], py = Tbe[1, 3], pz = Tbe[2, 3];
// 提取MDH参数
double d4 = _mdh[3, 0]; // 第4个关节的d参数
double a1 = _mdh[1, 1]; // 第2个关节的a参数
double a2 = _mdh[2, 1]; // 第3个关节的a参数
double a3 = _mdh[3, 1]; // 第4个关节的a参数
// 固定关节参数
double d2 = 0, d3 = 0;
double f1 = -Math.PI / 2; // alpha1
double f3 = -Math.PI / 2; // alpha3
double f4 = Math.PI / 2; // alpha4
double f5 = -Math.PI / 2; // alpha5
// ===== 计算t1的两个可能解 =====
double denom1 = Math.Pow(px * Math.Sin(f1), 2) + Math.Pow(py * Math.Sin(f1), 2) - Math.Pow(d2 - d3, 2);
if (denom1 < 0) denom1 = 0;
double t11 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1), Math.Sqrt(denom1));
double t12 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1), -Math.Sqrt(denom1));
// ===== 计算中间变量 =====
double m3_1 = pz * Math.Sin(f1);
double n3_1 = a1 - px * Math.Cos(t11) - py * Math.Sin(t11);
double m3_2 = pz * Math.Sin(f1);
double n3_2 = a1 - px * Math.Cos(t12) - py * Math.Sin(t12);
// ===== 计算t3的四个可能解 =====
double expr3_1 = Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2);
double denom3_1 = Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) - Math.Pow(expr3_1, 2);
if (denom3_1 < 0) denom3_1 = 0;
double t31 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +
Math.Atan2(expr3_1 / Math.Sin(f3), Math.Sqrt(denom3_1));
double t32 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +
Math.Atan2(expr3_1 / Math.Sin(f3), -Math.Sqrt(denom3_1));
double expr3_2 = Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2);
double denom3_2 = Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) - Math.Pow(expr3_2, 2);
if (denom3_2 < 0) denom3_2 = 0;
double t33 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +
Math.Atan2(expr3_2 / Math.Sin(f3), Math.Sqrt(denom3_2));
double t34 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) +
Math.Atan2(expr3_2 / Math.Sin(f3), -Math.Sqrt(denom3_2));
// ===== 计算t2的四个可能解 =====
double m2_1 = a2 + a3 * Math.Cos(t31) + d4 * Math.Sin(f3) * Math.Sin(t31);
double n2_1 = a3 * Math.Sin(t31) - d4 * Math.Sin(f3) * Math.Cos(t31);
double t21 = Math.Atan2(m3_1 * m2_1 + n2_1 * n3_1, m3_1 * n2_1 - m2_1 * n3_1);
double m2_2 = a2 + a3 * Math.Cos(t32) + d4 * Math.Sin(f3) * Math.Sin(t32);
double n2_2 = a3 * Math.Sin(t32) - d4 * Math.Sin(f3) * Math.Cos(t32);
double t22 = Math.Atan2(m3_1 * m2_2 + n2_2 * n3_1, m3_1 * n2_2 - m2_2 * n3_1);
double m2_3 = a2 + a3 * Math.Cos(t33) + d4 * Math.Sin(f3) * Math.Sin(t33);
double n2_3 = a3 * Math.Sin(t33) - d4 * Math.Sin(f3) * Math.Cos(t33);
double t23 = Math.Atan2(m3_2 * m2_3 + n2_3 * n3_2, m3_2 * n2_3 - m2_3 * n3_2);
double m2_4 = a2 + a3 * Math.Cos(t34) + d4 * Math.Sin(f3) * Math.Sin(t34);
double n2_4 = a3 * Math.Sin(t34) - d4 * Math.Sin(f3) * Math.Cos(t34);
double t24 = Math.Atan2(m3_2 * m2_4 + n2_4 * n3_2, m3_2 * n2_4 - m2_4 * n3_2);
// ===== 计算t5的八个可能解 =====
// 第一组
double m5_1 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t21) + ay * Math.Sin(t11) * Math.Cos(t21) + az * Math.Sin(f1) * Math.Sin(t21));
double n5_1 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t21) + ay * Math.Sin(t11) * Math.Sin(t21) - az * Math.Sin(f1) * Math.Cos(t21));
double t51 = Math.Atan2(
Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)),
(m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4)));
double t52 = Math.Atan2(
-Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)),
(m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4)));
// 第二组
double m5_2 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t22) + ay * Math.Sin(t11) * Math.Cos(t22) + az * Math.Sin(f1) * Math.Sin(t22));
double n5_2 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t22) + ay * Math.Sin(t11) * Math.Sin(t22) - az * Math.Sin(f1) * Math.Cos(t22));
double t53 = Math.Atan2(
Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)),
(m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4)));
double t54 = Math.Atan2(
-Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)),
(m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4)));
// 第三组
double m5_3 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t23) + ay * Math.Sin(t12) * Math.Cos(t23) + az * Math.Sin(f1) * Math.Sin(t23));
double n5_3 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t23) + ay * Math.Sin(t12) * Math.Sin(t23) - az * Math.Sin(f1) * Math.Cos(t23));
double t55 = Math.Atan2(
Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)),
(m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4)));
double t56 = Math.Atan2(
-Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)),
(m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4)));
// 第四组
double m5_4 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t24) + ay * Math.Sin(t12) * Math.Cos(t24) + az * Math.Sin(f1) * Math.Sin(t24));
double n5_4 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t24) + ay * Math.Sin(t12) * Math.Sin(t24) - az * Math.Sin(f1) * Math.Cos(t24));
double t57 = Math.Atan2(
Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)),
(m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4)));
double t58 = Math.Atan2(
-Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)),
(m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4)));
// ===== 计算t4的八个可能解 =====
double t41 = CalculateT4(t51, t11, t31, ax, ay, f1, f3, f5, m5_1, n5_1);
double t42 = CalculateT4(t52, t11, t31, ax, ay, f1, f3, f5, m5_1, n5_1);
double t43 = CalculateT4(t53, t11, t32, ax, ay, f1, f3, f5, m5_2, n5_2);
double t44 = CalculateT4(t54, t11, t32, ax, ay, f1, f3, f5, m5_2, n5_2);
double t45 = CalculateT4(t55, t12, t33, ax, ay, f1, f3, f5, m5_3, n5_3);
double t46 = CalculateT4(t56, t12, t33, ax, ay, f1, f3, f5, m5_3, n5_3);
double t47 = CalculateT4(t57, t12, t34, ax, ay, f1, f3, f5, m5_4, n5_4);
double t48 = CalculateT4(t58, t12, t34, ax, ay, f1, f3, f5, m5_4, n5_4);
// ===== 计算t6的八个可能解 =====
double e1 = nx * Math.Sin(t11) - ny * Math.Cos(t11);
double f1_val = ox * Math.Sin(t11) - oy * Math.Cos(t11);
double t61 = Math.Atan2(
Math.Cos(t41) * e1 - Math.Cos(t51) * Math.Sin(t41) * f1_val,
Math.Cos(t41) * f1_val + Math.Cos(t51) * Math.Sin(t41) * e1);
double t62 = Math.Atan2(
Math.Cos(t42) * e1 - Math.Cos(t52) * Math.Sin(t42) * f1_val,
Math.Cos(t42) * f1_val + Math.Cos(t52) * Math.Sin(t42) * e1);
double t63 = Math.Atan2(
Math.Cos(t43) * e1 - Math.Cos(t53) * Math.Sin(t43) * f1_val,
Math.Cos(t43) * f1_val + Math.Cos(t53) * Math.Sin(t43) * e1);
double t64 = Math.Atan2(
Math.Cos(t44) * e1 - Math.Cos(t54) * Math.Sin(t44) * f1_val,
Math.Cos(t44) * f1_val + Math.Cos(t54) * Math.Sin(t44) * e1);
double e2 = nx * Math.Sin(t12) - ny * Math.Cos(t12);
double f2_val = ox * Math.Sin(t12) - oy * Math.Cos(t12);
double t65 = Math.Atan2(
Math.Cos(t45) * e2 - Math.Cos(t55) * Math.Sin(t45) * f2_val,
Math.Cos(t45) * f2_val + Math.Cos(t55) * Math.Sin(t45) * e2);
double t66 = Math.Atan2(
Math.Cos(t46) * e2 - Math.Cos(t56) * Math.Sin(t46) * f2_val,
Math.Cos(t46) * f2_val + Math.Cos(t56) * Math.Sin(t46) * e2);
double t67 = Math.Atan2(
Math.Cos(t47) * e2 - Math.Cos(t57) * Math.Sin(t47) * f2_val,
Math.Cos(t47) * f2_val + Math.Cos(t57) * Math.Sin(t47) * e2);
double t68 = Math.Atan2(
Math.Cos(t48) * e2 - Math.Cos(t58) * Math.Sin(t48) * f2_val,
Math.Cos(t48) * f2_val + Math.Cos(t58) * Math.Sin(t48) * e2);
// ===== 组合八组解 =====
double[,] ikine_t = new double[8, 6];
// 第一组解
ikine_t[0, 0] = t11; ikine_t[0, 1] = t21; ikine_t[0, 2] = t31;
ikine_t[0, 3] = t41; ikine_t[0, 4] = t51; ikine_t[0, 5] = t61;
// 第二组解
ikine_t[1, 0] = t11; ikine_t[1, 1] = t21; ikine_t[1, 2] = t31;
ikine_t[1, 3] = t42; ikine_t[1, 4] = t52; ikine_t[1, 5] = t62;
// 第三组解
ikine_t[2, 0] = t11; ikine_t[2, 1] = t22; ikine_t[2, 2] = t32;
ikine_t[2, 3] = t43; ikine_t[2, 4] = t53; ikine_t[2, 5] = t63;
// 第四组解
ikine_t[3, 0] = t11; ikine_t[3, 1] = t22; ikine_t[3, 2] = t32;
ikine_t[3, 3] = t44; ikine_t[3, 4] = t54; ikine_t[3, 5] = t64;
// 第五组解
ikine_t[4, 0] = t12; ikine_t[4, 1] = t23; ikine_t[4, 2] = t33;
ikine_t[4, 3] = t45; ikine_t[4, 4] = t55; ikine_t[4, 5] = t65;
// 第六组解
ikine_t[5, 0] = t12; ikine_t[5, 1] = t23; ikine_t[5, 2] = t33;
ikine_t[5, 3] = t46; ikine_t[5, 4] = t56; ikine_t[5, 5] = t66;
// 第七组解
ikine_t[6, 0] = t12; ikine_t[6, 1] = t24; ikine_t[6, 2] = t34;
ikine_t[6, 3] = t47; ikine_t[6, 4] = t57; ikine_t[6, 5] = t67;
// 第八组解
ikine_t[7, 0] = t12; ikine_t[7, 1] = t24; ikine_t[7, 2] = t34;
ikine_t[7, 3] = t48; ikine_t[7, 4] = t58; ikine_t[7, 5] = t68;
return ikine_t;
}
private double CalculateT4(double t5, double t1, double t3, double ax, double ay,
double f1, double f3, double f5, double m5, double n5)
{
if (Math.Abs(Math.Sin(t5)) < 1e-10)
{
return 0;
}
double numerator = (ay * Math.Cos(t1) - ax * Math.Sin(t1)) * Math.Sin(f1) * Math.Sin(f5);
numerator /= (-Math.Sin(t5) * Math.Sin(f3));
double denominator = (-m5 * Math.Cos(t3) - n5 * Math.Sin(t3)) / Math.Sin(t5);
return Math.Atan2(numerator, denominator);
}
// 辅助方法:创建齐次变换矩阵
public static double[,] CreateTransformMatrix(double x, double y, double z,
double roll, double pitch, double yaw)
{
double[,] T = new double[4, 4];
double cr = Math.Cos(roll), sr = Math.Sin(roll);
double cp = Math.Cos(pitch), sp = Math.Sin(pitch);
double cy = Math.Cos(yaw), sy = Math.Sin(yaw);
T[0, 0] = cy * cp;
T[0, 1] = cy * sp * sr - sy * cr;
T[0, 2] = cy * sp * cr + sy * sr;
T[1, 0] = sy * cp;
T[1, 1] = sy * sp * sr + cy * cr;
T[1, 2] = sy * sp * cr - cy * sr;
T[2, 0] = -sp;
T[2, 1] = cp * sr;
T[2, 2] = cp * cr;
T[0, 3] = x;
T[1, 3] = y;
T[2, 3] = z;
T[3, 0] = 0;
T[3, 1] = 0;
T[3, 2] = 0;
T[3, 3] = 1;
return T;
}
// 获取当前MDH参数
public double[,] GetMDHParameters()
{
return (double[,])_mdh.Clone();
}
// 设置新的MDH参数
public void SetMDHParameters(double[,] newMdh)
{
if (newMdh == null)
throw new ArgumentNullException("newMdh");
if (newMdh.GetLength(0) != 6 || newMdh.GetLength(1) != 3)
throw new ArgumentException("MDH参数必须是6x3矩阵");
_mdh = (double[,])newMdh.Clone();
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace RobotCalcForm
{
//Puma560六轴机械臂-正解计算
public class Puma560FK
{
// MDH参数表:d, a, alpha(每个关节3个参数)
private readonly double[,] _mdhParams;
/// <summary>
/// 构造函数,初始化MDH参数
/// </summary>
/// <param name="mdhParameters">6x3的MDH参数数组 [d, a, alpha]</param>
public Puma560FK(double[,] mdhParameters)
{
if (mdhParameters == null)
throw new ArgumentNullException("mdhParameters", "MDH参数不能为空");
if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3)
throw new ArgumentException("MDH参数必须是6x3的数组");
_mdhParams = mdhParameters;
}
/// <summary>
/// 使用默认MDH参数的构造函数
/// </summary>
public Puma560FK()
{
// 默认PUMA560参数
_mdhParams = new double[6, 3]
{
// d, a, alpha]
{ 0, 0, 0 }, // Joint 1
{ 0, 0.180, -Math.PI/2 }, // Joint 2
{ 0, 0.600, 0 }, // Joint 3
{ 0.630, 0.130, -Math.PI/2 }, // Joint 4
{ 0, 0, Math.PI/2 }, // Joint 5
{ 0, 0, -Math.PI/2 } // Joint 6
};
}
/// <summary>
/// 计算机器人末端位姿(齐次变换矩阵)
/// </summary>
/// <param name="theta">6个关节角度数组(弧度)</param>
/// <returns>4x4齐次变换矩阵</returns>
public double[,] ForwardKinematics(double[] theta)
{
if (theta == null || theta.Length != 6)
throw new ArgumentException("需要6个关节角度参数");
// 计算各关节变换矩阵
double[,] T01 = CalculateTransform(theta[0] * Math.PI / 180.0f, _mdhParams[0, 0], _mdhParams[0, 1], _mdhParams[0, 2]);
double[,] T12 = CalculateTransform(theta[1] * Math.PI / 180.0f, _mdhParams[1, 0], _mdhParams[1, 1], _mdhParams[1, 2]);
double[,] T23 = CalculateTransform(theta[2] * Math.PI / 180.0f, _mdhParams[2, 0], _mdhParams[2, 1], _mdhParams[2, 2]);
double[,] T34 = CalculateTransform(theta[3] * Math.PI / 180.0f, _mdhParams[3, 0], _mdhParams[3, 1], _mdhParams[3, 2]);
double[,] T45 = CalculateTransform(theta[4] * Math.PI / 180.0f, _mdhParams[4, 0], _mdhParams[4, 1], _mdhParams[4, 2]);
double[,] T56 = CalculateTransform(theta[5] * Math.PI / 180.0f, _mdhParams[5, 0], _mdhParams[5, 1], _mdhParams[5, 2]);
// 级联变换矩阵
double[,] T02 = MatrixMultiply(T01, T12);
double[,] T03 = MatrixMultiply(T02, T23);
double[,] T04 = MatrixMultiply(T03, T34);
double[,] T05 = MatrixMultiply(T04, T45);
double[,] T06 = MatrixMultiply(T05, T56);
return T06;
}
/// <summary>
/// 获取当前MDH参数
/// </summary>
public double[,] GetMDHParameters()
{
return (double[,])_mdhParams.Clone();
}
/// <summary>
/// 设置单个关节的MDH参数
/// </summary>
/// <param name="jointIndex">关节索引(0-5)</param>
/// <param name="d">连杆偏移</param>
/// <param name="a">连杆长度</param>
/// <param name="alpha">连杆扭角</param>
public void SetJointParameters(int jointIndex, double d, double a, double alpha)
{
if (jointIndex < 0 || jointIndex > 5)
throw new ArgumentOutOfRangeException("jointIndex", "关节索引必须在0-5之间");
_mdhParams[jointIndex, 0] = d;
_mdhParams[jointIndex, 1] = a;
_mdhParams[jointIndex, 2] = alpha;
}
/// <summary>
/// 计算单个关节的变换矩阵
/// </summary>
private double[,] CalculateTransform(double theta, double d, double a, double alpha)
{
double cosT = Math.Cos(theta);
double sinT = Math.Sin(theta);
double cosA = Math.Cos(alpha);
double sinA = Math.Sin(alpha);
return new double[4, 4]
{
{ cosT, -sinT, 0, a },
{ cosA * sinT, cosA * cosT, -sinA, -d * sinA },
{ sinA * sinT, sinA * cosT, cosA, d * cosA },
{ 0, 0, 0, 1 }
};
}
/// <summary>
/// 4x4矩阵乘法
/// </summary>
private double[,] MatrixMultiply(double[,] a, double[,] b)
{
double[,] result = new double[4, 4];
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
result[i, j] = 0;
for (int k = 0; k < 4; k++)
{
result[i, j] += a[i, k] * b[k, j];
}
}
}
return result;
}
/// <summary>
/// 从变换矩阵中提取位置向量
/// </summary>
public double[] GetPosition(double[,] matrix)
{
return new double[3] { matrix[0, 3], matrix[1, 3], matrix[2, 3] };
}
/// <summary>
/// 从变换矩阵中提取欧拉角(ZYX顺序)
/// </summary>
public double[] GetEulerAngles(double[,] matrix)
{
double sy = Math.Sqrt(matrix[0, 0] * matrix[0, 0] + matrix[1, 0] * matrix[1, 0]);
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = Math.Atan2(matrix[2, 1], matrix[2, 2]);
y = Math.Atan2(-matrix[2, 0], sy);
z = Math.Atan2(matrix[1, 0], matrix[0, 0]);
}
else
{
x = Math.Atan2(-matrix[1, 2], matrix[1, 1]);
y = Math.Atan2(-matrix[2, 0], sy);
z = 0;
}
return new double[3] { x, y, z };
}
/// <summary>
/// 打印齐次变换矩阵
/// </summary>
public void PrintMatrix(double[,] matrix)
{
Console.WriteLine("Homogeneous Transformation Matrix:");
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
Console.Write(string.Format("{0,12:F6}", matrix[i, j]));
}
Console.WriteLine();
}
}
}
}
上述计算六轴机械臂封装的正解和逆解的代码,两个计算出来的结果无法对应起来,查找哪里计算有问题,修改好重新输出