static Robot puma260b_robot_tool() { Robot robot = puma260b_robot(); robot.R_tool = GeneralRoboticsToolbox.Rot(v_builder.DenseOfArray(new[] { 0, 1.0, 0 }), Math.PI / 2.0); robot.P_tool = v_builder.DenseOfArray(new[] { 0.05, 0, 0 }); return(robot); }
static void TestQ2Rot() { Matrix <double> rot_t = m_builder.DenseOfRowArrays( new[] { -0.5057639, -0.1340537, 0.8521928 }, new[] { 0.6456962, -0.7139224, 0.2709081 }, new[] { 0.5720833, 0.6872731, 0.4476342 }); //Console.WriteLine(rot_t); Vector <double> q = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 }); Tuple <Vector <double>, double> rot = GeneralRoboticsToolbox.Q2Rot(q); Vector <double> k = rot.Item1; double theta = rot.Item2; //Console.WriteLine(GeneralRoboticsToolbox.Rot(k, theta)); //Console.WriteLine(rot_t); if (!AlmostEqualsMatrix(GeneralRoboticsToolbox.Rot(k, theta), rot_t, 1e-6)) { Console.WriteLine("Q2Rot failed"); } else { Console.WriteLine("Q2Rot succeeded"); } }
static void TestRot() { Vector <double> k = v_builder.DenseOfArray(new[] { 1.0, 0.0, 0.0 }); Matrix <double> rot1 = m_builder.Dense(3, 3); rot1[0, 0] = 1; rot1[0, 1] = 0; rot1[0, 2] = 0; rot1[1, 0] = 0; rot1[1, 1] = 0; rot1[1, 2] = 1; rot1[2, 0] = 0; rot1[2, 1] = -1; rot1[2, 2] = 0; rot1 = rot1.Transpose(); Console.WriteLine("Testing Rot..."); Matrix <double> rot = GeneralRoboticsToolbox.Rot(k, Math.PI / 2.0); //Console.WriteLine(k.ToString()); //Console.WriteLine(rot1.ToString()); //Console.WriteLine(rot.ToString()); if (!AlmostEqualsMatrix(rot1, rot, 1e-6)) { Console.WriteLine("rot1 failed"); } else { Console.WriteLine("rot1 succeeded"); } Matrix <double> rot2 = m_builder.DenseOfRowArrays(new[] { 0, 0, -1.0 }, new[] { 0, 1.0, 0 }, new[] { 1.0, 0, 0 }).Transpose(); Vector <double> k2 = v_builder.DenseOfArray(new[] { 0, 1.0, 0 }); Matrix <double> rot_2 = GeneralRoboticsToolbox.Rot(k2, Math.PI / 2); if (!AlmostEqualsMatrix(rot2, rot_2, 1e-6)) { Console.WriteLine("rot2 failed"); } else { Console.WriteLine("rot2 succeeded"); } Matrix <double> rot3 = m_builder.DenseOfRowArrays(new[] { 0, 1.0, 0 }, new[] { -1.0, 0, 0 }, new[] { 0, 0, 1.0 }).Transpose(); Vector <double> k3 = v_builder.DenseOfArray(new[] { 0, 0, 1.0 }); Matrix <double> rot_3 = GeneralRoboticsToolbox.Rot(k3, Math.PI / 2); if (!AlmostEqualsMatrix(rot3, rot_3, 1e-6)) { Console.WriteLine("rot3 failed"); } else { Console.WriteLine("rot3 succeeded"); } Matrix <double> rot4 = m_builder.DenseOfRowArrays(new[] { -0.5057639, -0.1340537, 0.8521928 }, new[] { 0.6456962, -0.7139224, 0.2709081 }, new[] { 0.5720833, 0.6872731, 0.4476342 }); Vector <double> k4 = v_builder.DenseOfArray(new[] { 0.4490221, 0.30207945, 0.84090853 }); Matrix <double> rot_4 = GeneralRoboticsToolbox.Rot(k4, 2.65949884); if (!AlmostEqualsMatrix(rot4, rot_4, 1e-5)) { Console.WriteLine("rot4 failed"); } else { Console.WriteLine("rot4 succeeded"); } }
static void TestFwdkin() { Robot puma = puma260b_robot(); Transform pose = GeneralRoboticsToolbox.Fwdkin(puma, new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); if (!AlmostEqualsMatrix(pose.R, m_builder.DenseIdentity(3), 1e-8)) { Console.WriteLine("FwdKin failed"); return; } if (!AlmostEqualsVector(pose.P, v_builder.DenseOfArray(new[] { 10.0 * in_2_m, -4.9 * in_2_m, 4.25 * in_2_m }), 1e-6)) { Console.WriteLine("FwdKin failed"); return; } // Another right-angle configuration double[] joints2 = new[] { 180.0, -90, -90, 90, 90, 90 }; for (int i = 0; i < joints2.Length; i++) { joints2[i] = joints2[i] * Math.PI / 180.0; } Transform pose2 = GeneralRoboticsToolbox.Fwdkin(puma, joints2); Matrix <double> rot2 = GeneralRoboticsToolbox.Rot(v_builder.DenseOfArray(new[] { 0, 0, 1.0 }), Math.PI).Multiply(GeneralRoboticsToolbox.Rot(v_builder.DenseOfArray(new[] { 0, 1.0, 0 }), -Math.PI / 2)); if (!AlmostEqualsMatrix(pose2.R, rot2, 1e-6)) { Console.WriteLine("FwdKin failed"); return; } if (!AlmostEqualsVector(pose2.P, v_builder.DenseOfArray(new[] { -0.75, 4.9, 31 }) * 0.0254, 1e-6)) { Console.WriteLine("FwdKin failed"); return; } //Random configuration double[] joints3 = new[] { 50.0, -105, 31, 4, 126, -184 }; for (int i = 0; i < joints3.Length; i++) { joints3[i] = joints3[i] * Math.PI / 180.0; } Transform pose3 = GeneralRoboticsToolbox.Fwdkin(puma, joints3); Matrix <double> pose3_R_t = m_builder.DenseOfRowArrays( new[] { 0.4274, 0.8069, -0.4076 }, new[] { 0.4455, -0.5804, -0.6817 }, new[] { -0.7866, 0.1097, -0.6076 }); Vector <double> pose3_P_t = v_builder.DenseOfArray(new[] { 0.2236, 0.0693, 0.4265 }); if (!AlmostEqualsMatrix(pose3.R, pose3_R_t, 1e-4)) { Console.WriteLine("FwdKin failed"); return; } if (!AlmostEqualsVector(pose3.P, pose3_P_t, 1e-4)) { Console.WriteLine("FwdKin failed"); return; } Robot puma_tool = puma260b_robot_tool(); Transform pose4 = GeneralRoboticsToolbox.Fwdkin(puma_tool, joints3); Matrix <double> pose4_R_t = m_builder.DenseOfRowArrays( new[] { 0.4076, 0.8069, 0.4274 }, new[] { 0.681654, -0.580357, 0.44557 }, new[] { 0.60759, 0.1097, -0.7866 }); Console.WriteLine("Robot R tool={0}", pose4.R); Console.WriteLine("Robot R calculated tool={0}", pose4_R_t); Console.WriteLine("Robot p tool={0}", pose4.P); Vector <double> pose4_P_t = v_builder.DenseOfArray(new[] { 0.2450, 0.0916, 0.3872 }); Console.WriteLine("Robot p calculated tool={0}", pose4_P_t); if (!AlmostEqualsMatrix(pose4.R, pose4_R_t, 1 * 10 ^ 4)) { Console.WriteLine("FwdKin failed"); return; } if (!AlmostEqualsVector(pose4.P, pose4_P_t, 1 * 10 ^ 4)) { Console.WriteLine("FwdKin failed"); return; } Console.WriteLine("FwdKin succeeded"); }
static void Test_Subproblems() { Vector <double> x = v_builder.DenseOfArray(new[] { 1.0, 0, 0 }); Vector <double> y = v_builder.DenseOfArray(new[] { 0, 1.0, 0 }); Vector <double> z = v_builder.DenseOfArray(new[] { 0, 0, 1.0 }); // Subproblem0 if (GeneralRoboticsToolbox.Subproblem0(x, y, z) == Math.PI / 2) { Console.WriteLine("Subproblem0 succeeded"); } else { Console.WriteLine("Subproblem0 failed"); } // Subproblem1 Vector <double> k1 = (x + z) / (x + z).L2Norm(); Vector <double> k2 = (y + z) / (y + z).L2Norm(); if (GeneralRoboticsToolbox.Subproblem1(k1, k2, z) == Math.PI / 2) { Console.WriteLine("Subproblem1 succeeded"); } else { Console.WriteLine("Subproblem1 failed"); } // Subproblem2 Vector <double> p2 = x; Vector <double> q2 = x.Add(y).Add(z); q2 = q2 / q2.L2Norm(); double[] a2 = GeneralRoboticsToolbox.Subproblem2(p2, q2, z, y); if (a2.Length != 4) { Console.WriteLine("Subproblem2 failed"); return; } //NOTE: DIFFERENT THAN PYTHON VERSION Matrix <double> r1_0 = GeneralRoboticsToolbox.Rot(z, a2[0]); Matrix <double> r1_1 = GeneralRoboticsToolbox.Rot(y, a2[1]); Vector <double> r1 = (r1_0 * r1_1).Column(0); Matrix <double> r2_0 = GeneralRoboticsToolbox.Rot(z, a2[2]); Matrix <double> r2_1 = GeneralRoboticsToolbox.Rot(y, a2[3]); Vector <double> r2 = (r2_0 * r2_1).Column(0); if (!AlmostEqualsVector(r1, q2, 1e-4)) { Console.WriteLine("Subproblem2 failed"); return; } if (!AlmostEqualsVector(r2, q2, 1e-4)) { Console.WriteLine("Subproblem2 failed"); return; } double[] a3 = GeneralRoboticsToolbox.Subproblem2(x, z, z, y); if (a3.Length != 2) { Console.WriteLine("Subproblem2 failed"); return; } //NOTE: DIFFERENT THAN PYTHON VERSION Matrix <double> r3_0 = GeneralRoboticsToolbox.Rot(z, a3[0]); Matrix <double> r3_1 = GeneralRoboticsToolbox.Rot(y, a3[1]); Vector <double> r3 = (r3_0 * r3_1).Column(0); if (!AlmostEqualsVector(r3, z, 1e-4)) { Console.WriteLine("Subproblem2 failed"); return; } Console.WriteLine("Subproblem2 succeeded"); // Subproblem3 Vector <double> p4 = v_builder.DenseOfArray(new[] { .5, 0, 0 }); Vector <double> q4 = v_builder.DenseOfArray(new[] { 0, .75, 0 }); double[] a4 = GeneralRoboticsToolbox.Subproblem3(p4, q4, z, .5); double[] a5 = GeneralRoboticsToolbox.Subproblem3(p4, q4, z, 1.25); if (a4.Length != 2) { Console.WriteLine("Subproblem3 failed"); return; } if (!AlmostEquals((q4 + GeneralRoboticsToolbox.Rot(z, a4[0]) * p4).L2Norm(), 0.5, 1e-8)) { Console.WriteLine("Subproblem3 failed"); return; } if (!AlmostEquals((q4 + GeneralRoboticsToolbox.Rot(z, a4[1]) * p4).L2Norm(), 0.5, 1e-8)) { Console.WriteLine("Subproblem3 failed"); return; } if (a5.Length != 1) { Console.WriteLine("Subproblem3 failed"); return; } if (!AlmostEquals((q4 + GeneralRoboticsToolbox.Rot(z, a5[0]) * p4).L2Norm(), 1.25, 1e-8)) { Console.WriteLine("Subproblem3 failed"); return; } Console.WriteLine("Subproblem3 succeeded"); // Subproblem4 Vector <double> p6 = y; Vector <double> q6 = v_builder.DenseOfArray(new[] { .8, .2, .5 }); double d6 = .3; double[] a6 = GeneralRoboticsToolbox.Subproblem4(p6, q6, z, d6); if (!AlmostEquals((p6 * GeneralRoboticsToolbox.Rot(z, a6[0]) * q6), d6, 1e-4)) { Console.WriteLine("Subproblem4 failed"); return; } if (!AlmostEquals((p6 * GeneralRoboticsToolbox.Rot(z, a6[1]) * q6), d6, 1e-4)) { Console.WriteLine("Subproblem4 failed"); return; } Console.WriteLine("Subproblem4 succeeded"); }
static void TestR2Rot() { void _R2rot_test(Vector <double> k, double theta1) { Matrix <double> R = GeneralRoboticsToolbox.Rot(k, theta1); Tuple <Vector <double>, double> r2_vals = GeneralRoboticsToolbox.R2rot(R); Vector <double> _k2 = r2_vals.Item1; double theta2 = r2_vals.Item2; if (Math.Abs(theta1 - theta2) > (theta1 + theta2)) { _k2 = -_k2; theta2 = -theta2; } if (!AlmostEquals(theta1, theta2, 1e-6)) { Console.WriteLine("R2Rot failed with theta = " + theta1); } if (Math.Abs(theta1) < 1e-9) { Console.WriteLine("R2Rot succeeded"); return; } if ((Math.Abs(theta1) - Math.PI) < 1e-9) { if ((k + _k2).L2Norm() < 1e-6) { if (!AlmostEqualsVector(k, -_k2, 1e-6)) { Console.WriteLine("test1 failed"); //Console.WriteLine((-_k2).ToString()); Console.WriteLine("R2Rot failed with -_k2 = " + (-_k2).ToString()); } else { Console.WriteLine("R2Rot succeeded"); } return; } if (!AlmostEqualsVector(k, _k2, 1e-6)) { Console.WriteLine("test2 failed"); //Console.WriteLine(_k2.ToString()); Console.WriteLine("R2Rot failed with _k2 = " + _k2.ToString()); } else { Console.WriteLine("R2Rot succeeded"); } return; } if (!AlmostEqualsVector(k, _k2, 1e-6)) { Console.WriteLine("test3 failed"); //Console.WriteLine(_k2.ToString()); Console.WriteLine("R2Rot failed with _k2 = " + _k2.ToString()); return; } Console.WriteLine("R2Rot succeeded"); } _R2rot_test(v_builder.DenseOfArray(new[] { 1.0, 0, 0 }), Math.PI / 2.0); _R2rot_test(v_builder.DenseOfArray(new[] { 0, 1.0, 0 }), Math.PI / 2.0); _R2rot_test(v_builder.DenseOfArray(new[] { 0, 0, 1.0 }), Math.PI / 2.0); _R2rot_test(v_builder.DenseOfArray(new[] { 0.4490221, 0.30207945, 0.84090853 }), 2.65949884); //Singularities Vector <double> k1 = v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 }) / v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 }).L2Norm(); _R2rot_test(k1, 1e-10); Vector <double> k2 = v_builder.DenseOfArray(new[] { 2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { 2.0, -1.0, 3.0 }).L2Norm(); _R2rot_test(k2, Math.PI + 1e-10); Vector <double> k3 = v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }).L2Norm(); _R2rot_test(k3, Math.PI + 1e-10); Vector <double> k4 = v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }) / v_builder.DenseOfArray(new[] { -2.0, -1.0, 3.0 }).L2Norm(); _R2rot_test(k4, Math.PI + 1e-10); Vector <double> k5 = v_builder.DenseOfArray(new[] { 0, -1.0, -3.0 }) / v_builder.DenseOfArray(new[] { 0, -1.0, -3.0 }).L2Norm(); _R2rot_test(k5, Math.PI + 1e-10); Vector <double> k6 = v_builder.DenseOfArray(new[] { 0, 0, 1.0 }); _R2rot_test(k6, Math.PI + 1e-10); }