static void TestRpy2R()
{
Vector <double> rpy1 = v_builder.DenseOfArray(new[] { 10 * Math.PI / 180, -30 * Math.PI / 180, 90 * Math.PI / 180 });
Matrix <double> R1 = GeneralRoboticsToolbox.Rpy2R(rpy1);
Matrix <double> R1_t = m_builder.DenseOfRowArrays(
new[] { -0.0000000, -0.9848077, 0.1736482 },
new[] { 0.8660254, -0.0868241, -0.4924039 },
new[] { 0.5000000, 0.1503837, 0.8528686 });
if (!AlmostEqualsMatrix(R1, R1_t, 1e-6))
{
Console.WriteLine("Rpy2R failed");
}
Vector <double> rpy2 = GeneralRoboticsToolbox.R2Rpy(R1);
//Console.WriteLine(rpy2);
if (!AlmostEqualsVector(rpy1, rpy2, 1e-6))
{
Console.WriteLine("Rpy2R failed");
}
// Check singularity
// NOTE: Does not raise exception
Vector <double> rpy3 = v_builder.DenseOfArray(new[] { 10 * Math.PI / 180, 90 * Math.PI / 180, -30 * Math.PI / 180 });
Matrix <double> R3 = GeneralRoboticsToolbox.Rpy2R(rpy3);
Console.WriteLine(GeneralRoboticsToolbox.R2Rpy(R3).ToString());
Console.WriteLine("Rpy2R succeeded");
}
static void TestRot2Q()
{
Tuple <Vector <double>, double> rot = GeneralRoboticsToolbox.R2rot(m_builder.DenseOfRowArrays(
new[] { -0.5057639, -0.1340537, 0.8521928 },
new[] { 0.6456962, -0.7139224, 0.2709081 },
new[] { 0.5720833, 0.6872731, 0.4476342 }));
double[] k = new double[3];
for (int i = 0; i < rot.Item1.Count; i++)
{
k[i] = (double)rot.Item1[i];
}
float theta = (float)rot.Item2;
Vector <double> q_t = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 });
Vector <double> q = GeneralRoboticsToolbox.Rot2Q(k, theta);
if (!AlmostEqualsVector(q, q_t, 1e-6))
{
Console.WriteLine("Rot2Q failed");
}
else
{
Console.WriteLine("Rot2Q succeeded");
}
}
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 TestQuatcomplement()
{
Vector <double> q = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 });
Vector <double> q_c = GeneralRoboticsToolbox.Quatcomplement(q);
if (!AlmostEquals(q[0], q_c[0], 1e-8) ||
!AlmostEqualsVector(q.SubVector(1, 3), -q_c.SubVector(1, 3), 1e-8))
{
Console.WriteLine("Quatcomplement failed");
}
else
{
Console.WriteLine("Quatcomplement succeeded");
}
}
static void TestQuatproduct()
{
Vector <double> q_1 = v_builder.DenseOfArray(new[] { 0.63867877, 0.52251797, 0.56156573, 0.06089615 });
Vector <double> q_2 = v_builder.DenseOfArray(new[] { 0.35764716, 0.61051424, 0.11540801, 0.69716703 });
Matrix <double> R_t = GeneralRoboticsToolbox.Q2R(q_1).Multiply(GeneralRoboticsToolbox.Q2R(q_2));
Vector <double> q_t = GeneralRoboticsToolbox.R2Q(R_t);
Vector <double> q = GeneralRoboticsToolbox.Quatproduct(q_1).Multiply(q_2);
if (!AlmostEqualsVector(q, q_t, 1e-6))
{
Console.WriteLine("Quatproduct failed");
}
else
{
Console.WriteLine("Quatproduct succeeded");
}
}
static void TestQuatjacobian()
{
Vector <double> q = v_builder.DenseOfArray(new[] { 0.63867877, 0.52251797, 0.56156573, 0.06089615 });
Matrix <double> J = GeneralRoboticsToolbox.Quatjacobian(q);
Matrix <double> J_t = m_builder.DenseOfRowArrays(new[] { -0.26125898, -0.28078286, -0.03044808 },
new[] { 0.31933938, 0.03044808, -0.28078286 },
new[] { -0.03044808, 0.31933938, 0.26125898 },
new[] { 0.28078286, -0.26125898, 0.31933938 });
if (!AlmostEqualsMatrix(J, J_t, 1e-6))
{
Console.WriteLine("Quatjacobian failed");
}
else
{
Console.WriteLine("Quatjacobian succeeded");
}
}
static void TestQ2R()
{
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 });
Vector <double> q = v_builder.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 });
Matrix <double> rot = GeneralRoboticsToolbox.Q2R(q);
if (!AlmostEqualsMatrix(rot, rot_t, 1e-6))
{
Console.WriteLine("Q2R failed");
}
else
{
Console.WriteLine("Q2R succeeded");
}
}
static void TestScrewMatrix()
{
Console.WriteLine("Testing ScrewMatrix");
Vector <double> k = v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 });
Matrix <double> G = GeneralRoboticsToolbox.Screw_matrix(k);
Matrix <double> G_t = m_builder.DenseOfRowArrays(
new[] { 1.0, 0, 0, 0, -3, 2 },
new[] { 0, 1.0, 0, 3, 0, -1 },
new[] { 0, 0, 1.0, -2, 1, 0 },
new[] { 0, 0, 0, 1.0, 0, 0 },
new[] { 0, 0, 0, 0, 1.0, 0 },
new[] { 0, 0, 0, 0, 0, 1.0 });
if (!AlmostEqualsMatrix(G, G_t, 1e-8))
{
Console.WriteLine("ScrewMarix failed");
}
else
{
Console.WriteLine("ScrewMatrix succeeded");
}
}
static void TestHat()
{
//Test Hat
Console.WriteLine("Testing Hat...");
Vector <double> k = v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 });
Matrix <double> khat = m_builder.Dense(3, 3);
khat[0, 1] = -3;
khat[0, 2] = 2;
khat[1, 0] = 3;
khat[1, 2] = -1;
khat[2, 0] = -2;
khat[2, 1] = 1;
Matrix <double> k_hat = GeneralRoboticsToolbox.Hat(k);
if (!AlmostEqualsMatrix(k_hat, khat))
{
Console.WriteLine("hat failed");
}
else
{
Console.WriteLine("hat 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);
}
