/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
double wristX = x + Manipulator.Palm * Math.Cos(Math.PI - angle);
double wristY = y + Manipulator.Palm * Math.Sin(Math.PI - angle);
double distanceFromShoulderToWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
double elbow = TriangleTask.GetABAngle(
Manipulator.UpperArm,
Manipulator.Forearm,
distanceFromShoulderToWrist);
if (Double.IsNaN(elbow))
{ // что-то с треугольником... не срослось.
return(new[] { double.NaN, double.NaN, double.NaN });
}
double angleToWrist = Math.Atan2(wristY, wristX);
if (Double.IsNaN(angleToWrist))
{ // случилась довольно странная ошибка.
// такое может быть только если wristX или wristY NaN или бесконечны
return(new[] { double.NaN, double.NaN, double.NaN });
}
double shoulder = TriangleTask.GetABAngle(
Manipulator.UpperArm,
distanceFromShoulderToWrist,
Manipulator.Forearm) + angleToWrist;
double wrist = -angle - shoulder - elbow;
return(new[] { shoulder, elbow, wrist });
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному alpha (в радианах)
/// См. чертеж manipulator.png!
/// Используйте поля Forearm, UpperArm, Palm класса Manipulator
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double alpha)
{
double wristX = x >= 0 ?
x - Manipulator.Palm * Math.Cos(alpha) :
Math.Abs(x) + Manipulator.Palm * Math.Cos(alpha);
double wristY = y + Manipulator.Palm * Math.Sin(alpha);
double distanceToWrist = distance(0, 0, wristX, wristY);
double distanceWristToX = distance(x, 0, wristX, wristY);
double elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, distanceToWrist);
double shoulder1 = TriangleTask.GetABAngle(Manipulator.UpperArm, distanceToWrist, Manipulator.Forearm);
double shoulder2 = TriangleTask.GetABAngle(distanceToWrist, Math.Abs(x), distanceWristToX);
shoulder2 *= wristY < 0 ? -1 : 1;
double shoulder = shoulder1 + shoulder2;
double wrist = -alpha - shoulder - elbow;
if (double.IsNaN(shoulder) || double.IsNaN(elbow) || double.IsNaN(wrist))
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
return(new[] { shoulder, elbow, wrist });
}
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
double cosAngle = (Math.Abs(Math.Cos(angle)) < 1e-12) ? 0 : Math.Cos(angle);
double sinAngle = (Math.Abs(Math.Sin(angle)) < 1e-12) ? 0 : Math.Sin(angle);
double wristX = x - Manipulator.Palm * cosAngle;
double wristY = y + Manipulator.Palm * sinAngle;
double shoulderWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
double elbow = TriangleTask.GetABAngle(Manipulator.UpperArm,
Manipulator.Forearm, shoulderWrist);
if (elbow == double.NaN)
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
double shoulder = TriangleTask.GetABAngle(
shoulderWrist,
Manipulator.UpperArm,
Manipulator.Forearm) + Math.Atan2(wristY, wristX);
double wrist = 2 * Math.PI - angle - shoulder - elbow;
return(new[] { shoulder, elbow, wrist });
}
}
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
if (x * x + y * y <= Math.Pow(Manipulator.UpperArm + Manipulator.Forearm + Manipulator.Palm, 2))
{
var wristPos = GetWristPos(x, y, angle);
var lineConnectingUpperarmForearm = GetVectorLength(wristPos[0], wristPos[1]);
var elbow = TriangleTask.GetABAngle(Manipulator.UpperArm,
Manipulator.Forearm,
lineConnectingUpperarmForearm);
if (elbow == Double.NaN)
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
var shoulderAnglePart1 = TriangleTask.GetABAngle(lineConnectingUpperarmForearm,
Manipulator.UpperArm,
Manipulator.Forearm);
var shoulderAnglePart2 = Math.Atan2(wristPos[1], wristPos[0]);
if (shoulderAnglePart1 == Double.NaN || shoulderAnglePart2 == Double.NaN)
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
var shoulder = shoulderAnglePart1 + shoulderAnglePart2;
var wrist = -angle - shoulder - elbow;
return(new[] { shoulder, elbow, wrist });
}
return(new[] { double.NaN, double.NaN, double.NaN });
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
double wristX = x + Manipulator.Palm * Math.Cos(Math.PI - angle);
double wristY = y + Manipulator.Palm * Math.Sin(Math.PI - angle);
double distanceFromShoulderToWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
double elbow = TriangleTask.GetABAngle(
Manipulator.UpperArm,
Manipulator.Forearm,
distanceFromShoulderToWrist);
if (Double.IsNaN(elbow))
{
return(new[] { double.NaN, double.NaN, double.NaN });
}
double angleToWrist = Math.Atan2(wristY, wristX);
if (Double.IsNaN(angleToWrist))
{
return(new[] { double.NaN, double.NaN, double.NaN });
}
double shoulder = TriangleTask.GetABAngle(
Manipulator.UpperArm,
distanceFromShoulderToWrist,
Manipulator.Forearm) + angleToWrist;
double wrist = -angle - shoulder - elbow;
return(new[] { shoulder, elbow, wrist });
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному alpha (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double alpha)
{
var(wristX, wristY) = (
x + Manipulator.Palm * Math.Cos(Math.PI - alpha),
y + Manipulator.Palm * Math.Sin(Math.PI - alpha)
);
var fromShoulderToWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
var elbow = TriangleTask.GetABAngle(
Manipulator.UpperArm,
Manipulator.Forearm,
fromShoulderToWrist);
if (double.IsNaN(elbow))
{
return(new[] { double.NaN, double.NaN, double.NaN });
}
var wristOxAngle = Math.Atan2(wristY, wristX);
if (double.IsNaN(wristOxAngle))
{
return(new[] { double.NaN, double.NaN, double.NaN });
}
var shoulderAngle = TriangleTask.GetABAngle(
Manipulator.UpperArm,
fromShoulderToWrist,
Manipulator.Forearm
) + wristOxAngle;
var wrist = -alpha - shoulderAngle - elbow;
return(new[] { shoulderAngle, elbow, wrist });
}
// добавьте ещё тестовых случаев!
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
Assert.AreEqual(TriangleTask.GetABAngle(a, b, c), expectedAngle, 1e-8);
if (TriangleTask.GetABAngle(a, b, c) == double.NaN)
{
Assert.Fail("Not implemented yet");
}
}
[TestCase(3, 1, 2, 0)] // вырожденный треугольник, нулевой угол 2
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
double angle = TriangleTask.GetABAngle(a, b, c);
if (Double.IsNaN(expectedAngle))
{
Assert.IsNaN(angle);
}
Assert.AreEqual(expectedAngle, angle, 0.0001);
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
var wristX = x - Palm * Math.Cos(angle);
var wristY = y + Palm * Math.Sin(angle);
var shoulderWristLen = Math.Sqrt(wristX * wristX + wristY * wristY);
var elbow = TriangleTask.GetABAngle(Forearm, UpperArm, shoulderWristLen);
var shoulder = TriangleTask.GetABAngle(shoulderWristLen, UpperArm, Forearm)
+ Math.Atan2(wristY, wristX);
var wrist = -angle - elbow - shoulder;
return(new[] { shoulder, elbow, wrist });
}
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
var angle = TriangleTask.GetABAngle(a, b, c);
if (expectedAngle == double.NaN || angle == double.NaN)
{
Assert.AreEqual(angle, expectedAngle);
}
else
{
Assert.AreEqual(angle, expectedAngle, 1e-5);
}
}
public static double[] MoveManipulatorTo(double x, double y, double alpha)
{
double wristX = x + Manipulator.Palm * Math.Cos(Math.PI - alpha);
double wristY = y + Manipulator.Palm * Math.Sin(Math.PI - alpha);
var distanseFromShoulderToWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
var elbow = TriangleTask.GetABAngle(Manipulator.Forearm, Manipulator.UpperArm, distanseFromShoulderToWrist);
var shoulder = TriangleTask.GetABAngle(Manipulator.UpperArm, distanseFromShoulderToWrist, Manipulator.Forearm)
+ Math.Atan2(wristY, wristX);
var wrist = -alpha - shoulder - elbow;
if (double.IsNaN(shoulder) || double.IsNaN(elbow) || double.IsNaN(wrist))
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
return(new[] { shoulder, elbow, wrist });
}
}
public static double[] MoveManipulatorTo(double x, double y, double alpha)
{
var wristX = x - Manipulator.Palm * Math.Cos(alpha);
var wristY = y + Manipulator.Palm * Math.Sin(alpha);
var fromStartToWrist = Math.Sqrt(wristX * wristX + wristY * wristY);
var elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, fromStartToWrist);
var firstPart = TriangleTask.GetABAngle(Manipulator.UpperArm, fromStartToWrist, Manipulator.Forearm);
var secondPart = Math.Atan2(wristY, wristX);
var shoulder = firstPart + secondPart;
var wrist = -alpha - shoulder - elbow;
if (double.IsNaN(wrist) || double.IsNaN(shoulder) || double.IsNaN(elbow))
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
return(new[] { shoulder, elbow, wrist });
}
}
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
var a = Manipulator.UpperArm;
var b = Manipulator.Forearm;
var c = Manipulator.Palm;
double wristX = x + (c * Math.Cos(Math.PI - angle));
double wristY = y + (c * Math.Sin(Math.PI - angle));
double distanceFromShoulderToWrist = Math.Sqrt((wristX * wristX) + (wristY * wristY));
var elbow = TriangleTask.GetABAngle(a, b, distanceFromShoulderToWrist);
var angleToWrist = Math.Atan2(wristY, wristX);
var count = TriangleTask.GetABAngle(a, distanceFromShoulderToWrist, b);
var shoulder = count + angleToWrist;
var wrist = -angle - shoulder - elbow;
return(new[]
{
shoulder,
elbow,
wrist
});
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
// Используйте поля Forearm, UpperArm, Palm класса Manipulator
DPoint wristPoint = new DPoint
{
X = y + Math.Sin(angle) * Manipulator.Palm,
Y = x - Math.Sin((Math.PI / 2) - angle) * Manipulator.Palm
};
double elbowAngle = TriangleTask.GetABAngle(
Manipulator.UpperArm,
Manipulator.Forearm,
wristPoint.GetLengthLine()
);
double shoulderAngle = TriangleTask.GetABAngle(
wristPoint.GetLengthLine(),
Manipulator.UpperArm,
Manipulator.Forearm
) + Math.Atan2(wristPoint.X, wristPoint.Y);
double wristAngle = -angle - shoulderAngle - elbowAngle;
return(new[] { shoulderAngle, elbowAngle, wristAngle });
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному alpha (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double alpha)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(VisualizerTask.Shoulder, VisualizerTask.Elbow, VisualizerTask.Wrist);
Func <double, double> cos = angle => Math.Round(Math.Cos(angle), 10);
Func <double, double> sin = angle => Math.Round(Math.Sin(angle), 10);
double wrist = -alpha - VisualizerTask.Shoulder - VisualizerTask.Elbow;
var wristPos = new Vector(x + cos(Math.PI - alpha) * Manipulator.Palm, y + sin(Math.PI - alpha) * Manipulator.Palm);
double elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, Math.Sqrt(wristPos.X * wristPos.X + wristPos.Y * wristPos.Y));
double shoulder = TriangleTask.GetABAngle(Math.Sqrt(wristPos.X * wristPos.X + wristPos.Y * wristPos.Y), Manipulator.UpperArm, Manipulator.Forearm);
shoulder += Math.Atan2(wristPos.Y, wristPos.X);
if (wrist == double.NaN || elbow == double.NaN || shoulder == double.NaN)
{
wrist = double.NaN;
elbow = double.NaN;
shoulder = double.NaN;
}
return(new double[] { shoulder, elbow, wrist });
}
/// <summary>
/// Возвращает массив углов (shoulder, elbow, wrist),
/// необходимых для приведения эффектора манипулятора в точку x и y
/// с углом между последним суставом и горизонталью, равному angle (в радианах)
/// См. чертеж manipulator.png!
/// </summary>
public static double[] MoveManipulatorTo(double x, double y, double angle)
{
var wristX = x + Manipulator.Palm * Math.Cos(Math.PI - angle);
var wristY = y + Manipulator.Palm * Math.Sin(Math.PI - angle);
var shoulderWristLength = Math.Sqrt(wristX * wristX + wristY * wristY);
var elbow = TriangleTask.GetABAngle(
Manipulator.UpperArm, Manipulator.Forearm, shoulderWristLength);
if (double.IsNaN(elbow))
{
return new[] { double.NaN, double.NaN, double.NaN }
}
;
var shoulderWrist = Math.Atan2(wristY, wristX);
var shoulder = TriangleTask.GetABAngle(
Manipulator.UpperArm, shoulderWristLength, Manipulator.Forearm)
+ shoulderWrist;
return(double.IsNaN(shoulder)
? new[] { double.NaN, double.NaN, double.NaN }
: new[] { shoulder, elbow, -angle - shoulder - elbow });
}
}
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
Assert.AreEqual(expectedAngle, TriangleTask.GetABAngle(a, b, c), 1e+10);
}
// добавьте ещё тестовых случаев!
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
//Assert.Fail("Not implemented yet");
Assert.AreEqual(expectedAngle, TriangleTask.GetABAngle(a, b, c), 1e-5);
}
public static double CalcShoulderAngle(double shoulderWristLength, double xWristPos, double yWristPos) => TriangleTask.GetABAngle(Manipulator.UpperArm, shoulderWristLength, Manipulator.Forearm) + Math.Atan2(yWristPos, xWristPos);
public static double CalcElbowAngle(double shoulderWristLength) => TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, shoulderWristLength);
[TestCase(2, 2, 2 * 1.73205080757, 2 * Math.PI / 3)] // обычный с тупым углом
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
var actualAngle = TriangleTask.GetABAngle(a, b, c);
Assert.AreEqual(expectedAngle, actualAngle, 1e-5);
}
// добавьте ещё тестовых случаев!
public void TestGetABAngle(double a, double b, double c, double expectedAngle)
{
Assert.AreEqual(TriangleTask.GetABAngle(a, b, c), expectedAngle, 1e-5, "test");
}
