public async Task ANFIS_OutputFromFuzzy()
{
IRobotArm robotArm = new RobotArm1(10, 7, 0, 1.57, 0, 3.1);
robotArm.IsDataSetCalculated.ShouldBeFalse();
await robotArm.CalculateWholeDataSet(0.1);
await robotArm.TrainANFIS(25, 150, false);
robotArm.IsDataSetCalculated.ShouldBeTrue();
var point1 = new Point {
X = 1.10413546487088, Y = 2.81104319371924
};
var point2 = new Point {
X = 2.31665592712393, Y = 1.9375717475909
};
var point3 = new Point {
X = 2.88944142930409, Y = 16.7526454098038
};
var output1 = await robotArm.CalculateAngelsUsingANFIS(point1); // 1.1
var output2 = await robotArm.CalculateAngelsUsingANFIS(point2); // 0.6
var output3 = await robotArm.CalculateAngelsUsingANFIS(point3); // 1.4
}
public async Task draw_graph_test(int l1, int l2, double theta1, double theta2)
{
var robotArm = new RobotArm1(l1, l2, 0, theta1, 0, theta2);
double agleStep = 0.1;
var result = await robotArm.CalculateWholeDataSet(agleStep);
var result1 = "";
var input = "";
var outputTheta1 = "";
for (int i = 0; i < robotArm.X.Length; i++)
{
result1 += $"({robotArm.X[i]},{robotArm.Y[i]})";
input += $"new []{{{robotArm.X[i]}, {robotArm.Y[i]}}},{Environment.NewLine}";
}
outputTheta1 = robotArm.AnglesGrid.First().ConvertToSingleArray().Aggregate("", (s, d) => s + $"new double[]{{{d}}}, {Environment.NewLine}");
var inp = robotArm.Positions.ConvertToANFISParameter()
.Aggregate("", (a, x) => a + $"new []{{{string.Join(", ", x)}}}, {Environment.NewLine}");
input.ShouldNotBeNullOrEmpty();
outputTheta1.ShouldNotBeNullOrEmpty();
Trace.WriteLine(string.Join("; ", robotArm.Theta1Vector));
result.ShouldBe(true);
}
public void initialize_fuzzy_helper(int l, int t, double theta1, double theta2)
{
var robotArm = new RobotArm1(l, t, 0, theta1, 0, theta2);
robotArm.L1.ShouldNotBeNull();
robotArm.L2.ShouldNotBeNull();
}
public async Task CalculateJointPostionWhenPointPostionKnown(double x, double y, double z, int expectingPositions)
{
var robotArm = new RobotArm1(3, 4, 0, 1.5, 0, 3.1);
var agleStep = 0.1;
var result = await robotArm.CalculateWholeDataSet(agleStep);
result.ShouldBe(true);
var zeroPoint = new Point {
X = 0, Y = 0, Z = 0
};
var endPoint = new Point {
X = x, Y = y, Z = 0
};
var res = await robotArm.CalculateArmJoint(endPoint);
res.ShouldNotBeNull();
res.Count().ShouldBe(expectingPositions);
var points = res.Select(te => te.JointPosition).ToArray();
points[0].X.ShouldBeGreaterThan(0);
points[0].Y.ShouldBeGreaterThan(0);
points[0].Z.ShouldBe(0);
(Math.Abs(points[0].DistanceFromOtherPoint(zeroPoint) - robotArm.L1) < 0.0000001).ShouldBe(true);
(Math.Abs(points[0].DistanceFromOtherPoint(endPoint) - robotArm.L2) < 0.0000001).ShouldBe(true);
if (expectingPositions == 2)
{
(Math.Abs(points[1].DistanceFromOtherPoint(zeroPoint) - robotArm.L1) < 0.0000001).ShouldBe(true);
(Math.Abs(points[1].DistanceFromOtherPoint(endPoint) - robotArm.L2) < 0.0000001).ShouldBe(true);
}
}
public async Task calculate_data_sets(int l, int t, double theta1, double theta2)
{
var robotArm = new RobotArm1(l, t, 0, theta1, 0, theta2);
double agleStep = 0.1;
var result = await robotArm.CalculateWholeDataSet(agleStep);
Trace.WriteLine(string.Join("; ", robotArm.Theta1Vector));
result.ShouldBe(true);
}
void Start()
{
_robotArm = GetComponent <RobotArm>();
_ik = GetComponent <RobotArmIK>();
_targetAngles = new TargetPositions();
_targetPositions = new List <PositionAndEndeffectorAngle>();
/*
* // look up/down
* _targetPositions.Add(new PositionAndEndeffectorAngle {
* Position = new Vector3(134f, 60f, 0f),
* EndeffectorAngle = Mathf.PI / 2f + 0.4f,
* ElbowUp = true
* });
*
* _targetPositions.Add(new PositionAndEndeffectorAngle {
* Position = new Vector3(144f, 94f, 0f),
* EndeffectorAngle = Mathf.PI / 2f,
* ElbowUp = true
* });
*
* _targetPositions.Add(new PositionAndEndeffectorAngle {
* Position = new Vector3(144f, 94f, 0f),
* EndeffectorAngle = Mathf.PI / 2f - 0.4f,
* ElbowUp = true
* });
*
* _targetPositions.Add(new PositionAndEndeffectorAngle {
* Position = new Vector3(144f, 94f, 0f),
* EndeffectorAngle = Mathf.PI / 2f + 0.4f,
* ElbowUp = true
* });
*/
// drill left
_targetPositions.Add(new PositionAndEndeffectorAngle {
Position = new Vector3(124f, 74f, 0f),
EndeffectorAngle = Mathf.PI / 2f,
ElbowUp = true
});
_targetPositions.Add(new PositionAndEndeffectorAngle {
Position = new Vector3(144f, 74f, 0f),
EndeffectorAngle = Mathf.PI / 2f,
ElbowUp = true
});
_targetPositions.Add(new PositionAndEndeffectorAngle {
Position = new Vector3(124f, 74f, 0f),
EndeffectorAngle = Mathf.PI / 2f,
ElbowUp = true
});
}
private void InitLocalRobotArmParameters(int l1, int l2, double theta1Min, double theta1Max, double theta2Min, double theta2Max, double agleStep)
{
_l1 = l1;
_l2 = l2;
_theta1Max = theta1Max;
_theta1Min = theta1Min;
_theta2Min = theta2Min;
_theta2Max = theta2Max;
_agleStep = agleStep;
_robotArm = new RobotArm.RobotArm(l1, l2, theta1Min, theta1Max, theta2Min, theta2Max);
}
public async Task CalculateErrorBetweenANFIS_and_Mathematical()
{
IRobotArm robotArm = new RobotArm1(10, 7, 0, 1.57, 0, 3.1);
robotArm.IsDataSetCalculated.ShouldBeFalse();
await robotArm.CalculateWholeDataSet(0.1);
await robotArm.TrainANFIS(25, 150, false);
robotArm.IsDataSetCalculated.ShouldBeTrue();
var result = await robotArm.CalculateMathError();
result.ShouldNotBeNull();
}
void Start()
{
_robotArm = GetComponent <RobotArm>();
}
