Scribd
Upload
9 views

代码

The document contains C# code that defines methods for matrix multiplication and printing matrices. It also contains code for calculating the forward and inverse kinematics of a 6 degree of freedom robotic arm by performing transformations between reference frames using transformation matrices. The code initializes transformation matrices between each joint of the arm, calculates the end effector transformation matrix by matrix multiplication, and calculates the joint angles through inverse kinematics given the end effector pose.

Uploaded by

haocheng zhang
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
9 views

代码

The document contains C# code that defines methods for matrix multiplication and printing matrices. It also contains code for calculating the forward and inverse kinematics of a 6 degree of freedom robotic arm by performing transformations between reference frames using transformation matrices. The code initializes transformation matrices between each joint of the arm, calculates the end effector transformation matrix by matrix multiplication, and calculates the joint angles through inverse kinematics given the end effector pose.

Uploaded by

haocheng zhang
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

using System;

namespace jixiebi.net
{
/// <summary>
/// <summary>
/// 矩阵乘法
/// </summary>
/// <param name=”matrix1″>矩阵 1</param>
/// <param name=”matrix2″>矩阵 2</param>
/// <returns>积</returns>
public static double[][] MatrixMult(double[][] matrix1, double[][] matrix2)
{
//matrix1 是 m*n 矩阵，matrix2 是 n*p 矩阵，则 result 是 m*p 矩阵
int m = matrix1.Length, n = matrix2.Length, p = matrix2[0].Length;
double[][] result1 = new double[m][];
for (int i = 0; i < result1.Length; i++)
{
result1[i] = new double[p];
}
//矩阵乘法：c[i,j]=Sigma(k=1→n,a[i,k]*b[k,j])
for (int i = 0; i < m; i++)
{
for (int j = 0; j < p; j++)
{
//对乘加法则
for (int k = 0; k < n; k++)
{
result1[i][j] += (matrix1[i][k] * matrix2[k][j]);
}
}
}
return result1;
}
/// <summary>
/// 打印矩阵
/// </summary>
/// <param name=”matrix”>待打印矩阵</param>
public static void PrintMatrix(double[][] matrix)
{
for (int i = 0; i < matrix.Length; i++)
{
for (int j = 0; j < matrix[i].Length; j++)
{
Console.Write(matrix[i][j] + “\t”);
}
Console.WriteLine();
}
}

public class zhengjie


{
public static void Main(string[] args){
int a2, a3, d1, d4, d5, d6；
double theta1, theta2, theta3, theta4, theta5, theta6, r11, r12,
r13, r21, r22, r23, r31, r32, r33, p_x, p_y, p_z;
/* 初始化变量 */
/* a2 = 10....;等等根据机械臂自己赋值，一定记得赋值 一定记得赋值 一定记得赋
值！！！！ */
double[,] T01 = new double[4,4]{
{Math.Cos(theta1), 0, Math.Sin(theta1), 0},
{Math.Sin(theta1), 0, -Math.Cos(theta1), 0},
{0, 1, 0, d1}
{0, 0, 0, 1}
};
double[,] T12 = new double[4,4]{
{-Math.Sin(theta2), Math.Cos(theta2), 0, -a2*Math.Sin(theta2)},
{Math.Cos(theta2), Math.Sin(theta2), 0, a2*Math.Cos(theta2)},
{0, 0, -1, 0}
{0, 0, 0, 1}
};
double[,] T23 = new double[4,4]{
{Math.Cos(theta3), Math.Sin(theta3), 0, a3*Math.Cos(theta3)},
{Math.Sin(theta3), -Math.Cos(theta3), 0, a3*Math.Sin(theta3)},
{0, 0, -1, 0}
{0, 0, 0, 1}
};
double[,] T34 = new double[4,4]{
{Math.Sin(theta4), 0, Math.Cos(theta4), 0},
{-Math.Cos(theta4), 0, Math.Sin(theta4), 0},
{0, -1, 0, d4}
{0, 0, 0, 1}
};
double[,] T45 = new double[4,4]{
{Math.Cos(theta5), 0, Math.Sin(theta5), 0},
{Math.Sin(theta5), 0, -Math.Cos(theta5), 0},
{0, 1, 0, d5}
{0, 0, 0, 1}
};
double[,] T56 = new double[4,4]{
{Math.Cos(theta6), -Math.Sin(theta6), 0, 0},
{Math.Sin(theta6), Math.Cos(theta6), 0, 0},
{0, 0, 1, d6}
{0, 0, 0, 1}
};

/* 末端矩阵 int[,] T06 = new int[4,4]{


{r11, r12, r13, p_x},
{r21, r22, r23, p_y},
{r31, r32, r33, p_z}
{0, 0, 0, 1}}; */

/* 正解 */
/* 初始化数组 */
double[][] T06 = new double[4][]{
new double[4],new double[4],new double[4],new double[4]{0,0,0,1}
};
T06 = MatrixMult(T01, T12);
T06 = MatrixMult(T06, T23);
T06 = MatrixMult(T06, T34);
T06 = MatrixMult(T06, T45);
T06 = MatrixMult(T06, T56);
PrintMatrix(T06);
}
}

public class nijie


{
public static void Main(string[] args){
/* 输入末端矩阵 */
/* p_x = 10....;等等根据机械臂自己之前的赋值，一定记得赋值 */
double[,] EndPose = new double[4,4]{
{r11, r12, r13, p_x},
{r21, r22, r23, p_y},
{r31, r32, r33, p_z}
{0, 0, 0, 1}
};
/* 初始化逆解结果数组 8*6 */
double[][] Results = new double[8][]{
new double[6],new double[6],new double[6],new double[6],new
double[6],new double[6],new double[6],new double[6]
};
/* 角 1 */
const int count = 8;
double A1 = d6 * EndPose[1][2] - EndPose[1][3];
double B1 = EndPose[0][3] - d6 * EndPose[0][2];
double C1 = d4;
double angle1_1 = Math.Atan2(C1, Math.Sqrt(A1 * A1 + B1 * B1 - C1 *
C1)) - Math.Atan2(A1, B1);
double angle1_2 = Math.Atan2(C1, -Math.Sqrt(A1 * A1 + B1 * B1 - C1 *
C1)) - Math.Atan2(A1, B1);

for (long i = 0; i < 4; ++i) {


Results[i][0] = angle1_1;
Results[i + 4][0] = angle1_2;
}
/* 角 5 */
for(long i = 0; i < 2; ++i) {
double cos5 = EndPose[0][2] * Math.Sin(Results[i * 4][0]) - EndPose[1]
[2] * Math.Cos(Results[i * 4][0]);
double sin5 = Math.Sqrt(1 - cos5 * cos5);
double angle5_1 = Math.Atan2(sin5, cos5);
double angle5_2 = Math.Atan2(-sin5, cos5);
Results[4 * i][4] = angle5_1;
Results[4 * i + 1][4] = angle5_1;
Results[4 * i + 2][4] = angle5_2;
Results[4 * i + 3][4] = angle5_2;
}

/* 角 6,2,3,4 */
for (int i = 0; i < 8; i++) {
/* 角 6 */
double A6 = (EndPose[0][1] * Math.Sin(Results[i][0]) - EndPose[1][1] *
Math.Sin(Results[i][0])) / Math.Sin(Results[i][4]);
double B6 = -(EndPose[0][0] * Math.Sin(Results[i][0]) - EndPose[1][0] *
Math.Cos(Results[i][0])) / Math.Sin(Results[i][4]);
Results[i][5] = Math.Atan2(A6, B6);
/*
@brief A234 = sin(theta2 - theta2 + theta4)
*/
double A234 = EndPose[2][2] / Math.Sin(Results[i][4]);
double B234 = (EndPose[0][2] * Math.Cos(Results[i][0]) + EndPose[1][2]
* Math.Sin(Results[i][0])) / Math.Sin(Results[i][4]);
/* 角 2 */
double M2 = d5 * A234 - d6 * Math.Sin(Results[i][4]) * B234 +
EndPose[0][3] * Math.Cos(Results[i][0]) + EndPose[1][3] * Math.Sin(Results[i][0]);
double N2 = -d5 * B234 - d6 * Math.Sin(Results[i][4]) * A234 - d1 +
EndPose[2][3];
double L2 = (M2 * M2 + N2 * N2 + a2 * a2 - a3 * a3) / (2 * a2);
if (i % 2 == 0) {
Results[i][1] = Math.Atan2(N2, M2) - Math.Atan2(L2, Math.Sqrt(M2 * M2 +
N2 * N2 - L2 * L2));
} else {
Results[i][1] = Math.Atan2(N2, M2) - Math.Atan2(L2, -Math.Sqrt(M2 * M2
+ N2 * N2 - L2 * L2));
}
double A23 = (-M2 - a2 * sin(Results[i][1])) / a3;
double B23 = (N2 - a2 * cos(Results[i][1])) / a3;
/* 角 3 */
Results[i][2] = Results[i][1] - Math.Atan2(A23, B23);
/* 角 4 */
Results[i][3] = Math.Atan2(A234, B234) - Math.Atan2(A23, B23);
}
/* 将关节角度值限制在 -180 或 180 度内 */
for (int i = 0; i < 8; i++) {
for(int j = 0; j < 6; j++) {
if (Results[i][j] > Math.PI) {
Results[i][j] = Results[i][j] - 2 * Math.PI;
} else if(Results[i][j] < -Math.PI) {
Results[i][j] = Results[i][j] + 2 * Math.PI;
}

}
}
/* 八组逆解结果 */
PrintMatrix(Results);
}

You might also like