public bool _test_configuration(Robot r, double[] theta)
{
Transform pose1 = Fwdkin(r, theta);
double[][] theta2 = InverseKin.robot6_sphericalwrist_invkin(r, pose1);
Console.WriteLine("hello");
foreach (double limit in r.Joint_upper_limit)
{
Console.WriteLine("Upper limits: {0}", limit);
}
foreach (double limit in r.Joint_lower_limit)
{
Console.WriteLine("Lower limits: {0}", limit);
}
foreach (double[] thetaset in theta2)
{
foreach (double thetas in thetaset)
{
Console.WriteLine("Theta values: {0}", thetas);
}
}
if (!(theta2.Length > 0))
{
Console.WriteLine("length of theta2 no good");
return(false);
}
foreach (double[] thetavals in theta2)
{
Transform pose2 = Fwdkin(r, thetavals);
Console.WriteLine("pose 1={0}", pose1);
Console.WriteLine("pose 2={0}", pose2);
if (!(pose1 == pose2))
{
return(false);
}
}
return(true);
}
public bool _test_last_configuration(Robot r, double[] theta, double[] last_theta)
{
Transform pose1 = Fwdkin(r, theta);
double[][] theta2 = InverseKin.robot6_sphericalwrist_invkin(r, pose1);
Transform pose2 = Fwdkin(r, theta2[0]);
if (!(pose1 == pose2))
{
return(false);
}
Vector <double> theta2_vec = Vector <double> .Build.DenseOfArray(theta2[0]);
Vector <double> last_vec = Vector <double> .Build.DenseOfArray(last_theta);
Console.WriteLine("last config: {0}", theta2_vec);
Console.WriteLine("last config: {0}", last_theta);
if (theta2_vec.AlmostEqual(last_vec, 1e-8))
{
return(true);
}
return(false);
}