/// Нарисуйте сегменты манипулятора методом graphics.DrawLine используя ManipulatorPen.
/// Нарисуйте суставы манипулятора окружностями методом graphics.FillEllipse используя JointBrush.
/// Не забудьте сконвертировать координаты из логических в оконные
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
for (int i = 0; i < joints.Length; i++)
{
joints[i] = ConvertMathToWindow(joints[i], shoulderPos);
}
graphics.DrawLine(ManipulatorPen, shoulderPos, joints[0]);
graphics.DrawLine(ManipulatorPen, joints[0], joints[1]);
graphics.DrawLine(ManipulatorPen, joints[1], joints[2]);
float r = 7;
graphics.FillEllipse(JointBrush, shoulderPos.X - r, shoulderPos.Y - r, 2 * r, 2 * r);
foreach (var e in joints)
{
graphics.FillEllipse(JointBrush, e.X - r, e.Y - r, 2 * r, 2 * r);
}
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
joints[0] = ConvertMathToWindow(joints[0], shoulderPos);
joints[1] = ConvertMathToWindow(joints[1], shoulderPos);
joints[2] = ConvertMathToWindow(joints[2], shoulderPos);
graphics.DrawLine(ManipulatorPen, shoulderPos, joints[0]);
graphics.DrawLine(ManipulatorPen, joints[0], joints[1]);
graphics.DrawLine(ManipulatorPen, joints[1], joints[2]);
graphics.FillEllipse(JointBrush, new RectangleF(shoulderPos, new SizeF(1, 1)));
graphics.FillEllipse(JointBrush, new RectangleF(joints[0], new SizeF(1, 1)));
graphics.FillEllipse(JointBrush, new RectangleF(joints[1], new SizeF(1, 1)));
graphics.FillEllipse(JointBrush, new RectangleF(joints[2], new SizeF(1, 1)));
}
public void TestGetForeArmPositions(double shoulder, double elbow, double wrist, double cordX, double cordY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(cordX, joints[1].X, 1e-5, "X");
Assert.AreEqual(cordY, joints[1].Y, 1e-5, "Y");
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
var shoulder = ConvertMathToWindow(new PointF(0, 0), shoulderPos);
var elbow = ConvertMathToWindow(joints[0], shoulderPos);
var wrist = ConvertMathToWindow(joints[1], shoulderPos);
var palmEnd = ConvertMathToWindow(joints[2], shoulderPos);
graphics.DrawLine(ManipulatorPen, shoulder, elbow);
graphics.DrawLine(ManipulatorPen, elbow, wrist);
graphics.DrawLine(ManipulatorPen, wrist, palmEnd);
const int jointRadius = 10;
DrawJoint(graphics, shoulder, jointRadius);
DrawJoint(graphics, elbow, jointRadius);
DrawJoint(graphics, wrist, jointRadius);
DrawJoint(graphics, palmEnd, jointRadius);
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
Assert.Fail("TODO: проверить, что расстояния между суставами равны длинам соответствующих сегментов манипулятора!");
}
public void TestMoveManipulatorTo()
{
var rng = new Random(312312);
for (var testNo = 0; testNo < 1000; ++testNo)
{
var x = rng.NextDouble() * 2 * Length - Length;
var y = rng.NextDouble() * 2 * Length - Length;
var a = rng.NextDouble() * 2 * Math.PI;
var angles = ManipulatorTask.MoveManipulatorTo(x, y, a);
Assert.AreEqual(3, angles.Length);
if (double.IsNaN(angles[0]))
{
continue;
}
var joints = AnglesToCoordinatesTask.GetJointPositions(
angles[0], angles[1], angles[2]);
Assert.AreEqual(3, joints.Length);
Assert.AreEqual(joints[2].X, x, 0.001);
Assert.AreEqual(joints[2].Y, y, 0.001);
}
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
// Нарисуйте сегменты манипулятора методом graphics.DrawLine используя ManipulatorPen.
// Нарисуйте суставы манипулятора окружностями методом graphics.FillEllipse используя JointBrush.
// Не забудьте сконвертировать координаты из логических в оконные
var windowPoint = ConvertMathToWindow(new PointF((float)X, (float)Y), shoulderPos);
X = windowPoint.X;
Y = windowPoint.Y;
graphics.DrawLine(ManipulatorPen, shoulderPos.X, shoulderPos.Y, shoulderPos.X + joints[0].X, shoulderPos.Y - joints[0].Y); //draw Shoulder
graphics.DrawLine(ManipulatorPen, shoulderPos.X + joints[0].X, shoulderPos.Y - joints[0].Y, shoulderPos.X + joints[1].X, shoulderPos.Y - joints[1].Y); //draw Elbow
graphics.DrawLine(ManipulatorPen, shoulderPos.X + joints[1].X, shoulderPos.Y - joints[1].Y, shoulderPos.X + joints[2].X, shoulderPos.Y - joints[2].Y); //draw Wrist
foreach (var joint in joints)
{
graphics.FillEllipse(JointBrush, shoulderPos.X + joint.X - 5, shoulderPos.Y - joint.Y - 5, 5, 5);
}
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}\n",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
// Нарисуйте сегменты манипулятора методом graphics.DrawLine используя ManipulatorPen.
// Нарисуйте суставы манипулятора окружностями методом graphics.FillEllipse используя JointBrush.
// Не забудьте сконвертировать координаты из логических в оконные
graphics.DrawLine(ManipulatorPen, ConvertMathToWindow(new PointF()
{
X = 0, Y = 0
}, shoulderPos),
ConvertMathToWindow(joints[0], shoulderPos));
graphics.DrawLine(ManipulatorPen, ConvertMathToWindow(joints[0], shoulderPos),
ConvertMathToWindow(joints[1], shoulderPos));
graphics.DrawLine(ManipulatorPen, ConvertMathToWindow(joints[1], shoulderPos),
ConvertMathToWindow(joints[2], shoulderPos));
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
var points = new PointF[joints.Length + 1];
points[0] = ConvertMathToWindow(new PointF(0, 0), shoulderPos);
for (var i = 0; i < joints.Length; i++)
{
points[i + 1] = ConvertMathToWindow(joints[i], shoulderPos);
}
graphics.DrawLines(ManipulatorPen, points);
for (var i = 0; i <= joints.Length; i++)
{
graphics.FillEllipse(JointBrush,
points[i].X - Radius, points[i].Y - Radius,
2 * Radius, 2 * Radius);
}
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
Assert.AreEqual(FindDistance(joints[0], new PointF(0, 0)), Manipulator.UpperArm);
Assert.AreEqual(FindDistance(joints[0], joints[1]), Manipulator.Forearm);
Assert.AreEqual(FindDistance(joints[1], joints[2]), Manipulator.Palm);
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
Assert.AreEqual(Manipulator.UpperArm, CalcJointsLength(joints[0]));
Assert.AreEqual(Manipulator.Forearm, CalcJointsLength(GetVectorCoord(joints[0], joints[1])));
Assert.AreEqual(Manipulator.Palm, CalcJointsLength(GetVectorCoord(joints[1], joints[2])));
}
public void TestMoveManipulatorTo()
{
var p = Generate();
var angles = ManipulatorTask.MoveManipulatorTo(p.x, p.y, p.angle);
var points = AnglesToCoordinatesTask.GetJointPositions(angles[0], angles[1], angles[2]);
var generatePoint = new PointF((float)p.x, (float)p.y);
Assert.AreEqual(generatePoint.X, points[2].X, 1e-4);
Assert.AreEqual(generatePoint.Y, points[2].Y, 1e-4);
}
public static void UpdateManipulator()
{
// Вызовите ManipulatorTask.MoveManipulatorTo и обновите значения полей Shoulder, Elbow и Wrist,
// если они не NaN. Это понадобится для последней задачи.
var angle = Math.Acos(Math.Abs(X - AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist)[2].X) / Manipulator.Palm);
if (!double.IsNaN(Shoulder) && !double.IsNaN(Elbow) && !double.IsNaN(Wrist))
{
ManipulatorTask.MoveManipulatorTo(X, Y, angle);
}
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
if (Math.Abs(GetFullDistanse(joints) -
(Manipulator.UpperArm + Manipulator.Forearm +
Manipulator.Palm)) > 1e-5)
{
Assert.Fail("TODO: проверить, что расстояния между суставами равны длинам сегментов манипулятора!");
}
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY, double forearmDist, double palmDist)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
var distance1 = Math.Sqrt(joints[0].X * joints[0].X + joints[0].Y * joints[0].Y);
var distance2 = Math.Round(Math.Sqrt(Math.Pow(joints[1].X - joints[0].X, 2) + Math.Pow(joints[1].Y - joints[0].Y, 2)));
var distance3 = Math.Sqrt(Math.Pow(joints[2].X - joints[1].X, 2) + Math.Pow(joints[2].Y - joints[1].Y, 2));
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
Assert.AreEqual(palmEndY, distance1);
Assert.AreEqual(forearmDist, distance2);
Assert.AreEqual(palmDist, distance3);
Assert.Fail("TODO: проверить, что расстояния между суставами равны длинам сегментов манипулятора!");
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double upperArm, double forearm, double palm)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(forearm + palm, joints[2].X, 1e-9, "palm endX");
Assert.AreEqual(upperArm, joints[2].Y, 1e-9, "palm endY");
double upperArmActual = hypotenuse(joints[0], new Point(0, 0));
double forearmActual = hypotenuse(joints[1], joints[0]);
double palmActual = hypotenuse(joints[2], joints[1]);
Assert.AreEqual(upperArm, upperArmActual, 1e-9, "UpperArm length");
Assert.AreEqual(forearm, forearmActual, 1e-9, "Forearm length");
Assert.AreEqual(palm, palmActual, 1e-9, "Palm length");
}
public void TestMoveManipulatorTo()
{
var random = new Random();
for (int i = 0; i < 10; ++i)
{
var x = random.Next(200);
var y = random.Next(200);
var angle = Math.PI * random.NextDouble();
var result = ManipulatorTask.MoveManipulatorTo(x, y, angle);
var joints = AnglesToCoordinatesTask.GetJointPositions(result[0],
result[1], result[2]);
Assert.AreEqual(x, joints[2].X, 1e-4);
Assert.AreEqual(y, joints[2].Y, 1e-4);
}
}
public void RndTestMoveManipulatorTo()
{
var random = new Random();
var x = random.NextDouble() * random.Next(0, 100);
var y = random.NextDouble() * random.Next(0, 100);
var getRandomAngle = random.Next(0, 100);
var angles = ManipulatorTask.MoveManipulatorTo(x, y, getRandomAngle);
var joints = AnglesToCoordinatesTask.GetJointPositions(
angles[0],
angles[1],
angles[2]);
var palm = joints[2];
Assert.AreEqual(x, palm.X, 1e-5);
Assert.AreEqual(y, palm.Y, 1e-5);
}
public void TestMoveManipulatorTo()
{
Random random = new Random();
for (int i = 0; i < 10; i++)
{
var x = random.Next();
var y = random.Next();
var angle = 2 * Math.PI * random.NextDouble();
var angles = ManipulatorTask.MoveManipulatorTo(x, y, angle);
if (!Double.IsNaN(angles[0]))
{
var coordinates = AnglesToCoordinatesTask.GetJointPositions(angles[0], angles[1], angles[2]);
Assert.AreEqual(x, coordinates[2].X, 1e-5);
Assert.AreEqual(y, coordinates[2].Y, 1e-5);
}
}
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist, double palmEndX, double palmEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
var expectedUpperarmLength = Math.Sqrt(joints[0].X * joints[0].X + joints[0].Y * joints[0].Y);
Assert.AreEqual(Manipulator.UpperArm, expectedUpperarmLength, 1e-5, "UpperArm Length");
var expectedForearmLength = Math.Sqrt((joints[1].X - joints[0].X) * (joints[1].X - joints[0].X)
+ (joints[1].Y - joints[0].Y) * (joints[1].Y - joints[0].Y));
Assert.AreEqual(Manipulator.Forearm, expectedForearmLength, 1e-4, "Forearm Length");
var expectedPalmLength = Math.Sqrt((joints[2].X - joints[1].X) * (joints[2].X - joints[1].X)
+ (joints[2].Y - joints[1].Y) * (joints[2].Y - joints[1].Y));
Assert.AreEqual(Manipulator.Palm, expectedPalmLength, 1e-4, "Palm Length");
}
public void TestMoveManipulatorTo()
{
var rnd = new Random();
for (int i = 0; i < 1000; i++)
{
var x = rnd.Next(-300, 300);
var y = rnd.Next(-300, 300);
var al = rnd.NextDouble() * Math.PI;
var angles = ManipulatorTask.MoveManipulatorTo(x, y, al);
if (!double.IsNaN(angles[0]))
{
var coord = AnglesToCoordinatesTask.GetJointPositions(angles[0], angles[1], angles[2]);
Assert.AreEqual(x, coord[2].X, 0.001);
Assert.AreEqual(y, coord[2].Y, 0.001);
}
}
}
public void TestMoveManipulatorTo()
{
var random = new Random();
for (var i = 0; i < 1000; i++)
{
var x = random.Next(100) + random.NextDouble();
var y = random.Next(100) + random.NextDouble();
var alpha = random.Next(100) + random.NextDouble();
var actualAngles = ManipulatorTask.MoveManipulatorTo(x, y, alpha);
if (!actualAngles.Contains(double.NaN))
{
var actualCoordinates =
AnglesToCoordinatesTask.GetJointPositions(actualAngles[0], actualAngles[1], actualAngles[2]);
Assert.AreEqual(x, actualCoordinates[2].X, 1e-3, "X");
Assert.AreEqual(y, actualCoordinates[2].Y, 1e-3, "Y");
}
}
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
var jointsPos = new PointF[] { ConvertMathToWindow(joints[0], shoulderPos),
ConvertMathToWindow(joints[1], shoulderPos),
ConvertMathToWindow(joints[2], shoulderPos) };
for (int i = 0; i < jointsPos.Length - 1; i++)
{
DrawJoint(graphics, jointsPos[i], jointsPos[i + 1]);
}
}
public void TestMoveManipulatorTo()
{
var rng = new Random(TestSeed);
for (var testNo = 0; testNo < NumTests; ++testNo)
{
var x = rng.NextDouble() * 2 * FullSize - FullSize;
var y = rng.NextDouble() * 2 * FullSize - FullSize;
var a = rng.NextDouble() * 2 * Math.PI;
var angles = ManipulatorTask.MoveManipulatorTo(x, y, a);
Assert.AreEqual(3, angles.Length);
if (!Double.IsNaN(angles[0]))
{
var joints = AnglesToCoordinatesTask.GetJointPositions(
angles[0], angles[1], angles[2]);
Assert.AreEqual(3, joints.Length);
Assert.AreEqual(joints[2].X, x, 0.001);
Assert.AreEqual(joints[2].Y, y, 0.001);
}
}
}
public void TestGetJointPositions(double shoulder, double elbow, double wrist,
double palmEndX, double palmEndY,
double wristEndX, double wristEndY,
double elbowEndX, double elbowEndY)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(shoulder, elbow, wrist);
Assert.AreEqual(palmEndX, joints[2].X, 1e-5, "palm endX");
Assert.AreEqual(palmEndY, joints[2].Y, 1e-5, "palm endY");
Assert.AreEqual(wristEndX, joints[1].X, 1e-5, "wrist endX");
Assert.AreEqual(wristEndY, joints[1].Y, 1e-5, "wrist endY");
Assert.AreEqual(elbowEndX, joints[0].X, 1e-5, "elbow endX");
Assert.AreEqual(elbowEndY, joints[0].Y, 1e-5, "elbow endY");
const int shoulderX = 0;
const int shoulderY = 0;
Assert.AreEqual(GetDistance(palmEndX, palmEndY, wristEndX, wristEndY), Manipulator.Palm);
Assert.AreEqual(GetDistance(elbowEndX, elbowEndY, wristEndX, wristEndY), Manipulator.Forearm);
Assert.AreEqual(GetDistance(elbowEndX, elbowEndY, shoulderX, shoulderY), Manipulator.UpperArm);
}
public void TestMoveManipulatorTo()
{
var rng = new Random(TestSeed);
for (var testNo = 0; testNo < NumTests; ++testNo)
{ // выбираем какую-то точку. не факт, что мы до неё сможем дотянуться.
var x = rng.NextDouble() * 2 * FullSize - FullSize;
var y = rng.NextDouble() * 2 * FullSize - FullSize;
var a = rng.NextDouble() * 2 * Math.PI;
var angles = ManipulatorTask.MoveManipulatorTo(x, y, a);
Assert.AreEqual(3, angles.Length);
if (!Double.IsNaN(angles[0]))
{ // кажется, дотянулись! проверим, в нужную ли точку
var joints = AnglesToCoordinatesTask.GetJointPositions(
angles[0], angles[1], angles[2]);
Assert.AreEqual(3, joints.Length);
Assert.AreEqual(joints[2].X, x, 0.001);
Assert.AreEqual(joints[2].Y, y, 0.001);
}
}
}
public static void DrawManipulator(Graphics graphics, PointF shoulderPos)
{
var joints = AnglesToCoordinatesTask.GetJointPositions(Shoulder, Elbow, Wrist);
var windowJoints = joints.Select(x => ConvertMathToWindow(x, shoulderPos)).ToList();
windowJoints.Insert(0, shoulderPos);
graphics.DrawString(
$"X={X:0}, Y={Y:0}, Alpha={Alpha:0.00}",
new Font(SystemFonts.DefaultFont.FontFamily, 12),
Brushes.DarkRed,
10,
10);
DrawReachableZone(graphics, ReachableAreaBrush, UnreachableAreaBrush, shoulderPos, joints);
for (var i = 0; i < windowJoints.Count - 1; i++)
{
var currentPoint = windowJoints[i];
var nextPoint = windowJoints[i + 1];
graphics.DrawLine(ManipulatorPen, currentPoint, nextPoint);
graphics.FillEllipse(JointBrush, currentPoint.X + RectSize, currentPoint.Y + RectSize,
-2 * RectSize, -2 * RectSize);
}
}
/// <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 });
}
public void TestMoveManipulatorTo()
{
Random rand = new Random();
double x, y, angle;
x = Convert.ToDouble(rand.Next(1234567890)) / 300;
y = Convert.ToDouble(rand.Next(1234567890)) / 400;
angle = Convert.ToDouble(rand.Next(1234567890)) / 500;
var joints = ManipulatorTask.MoveManipulatorTo(x, y, angle);
var actual = AnglesToCoordinatesTask.GetJointPositions(joints[0], joints[1], joints[2])[2];
if (x * x + y * y <= Math.Pow(Manipulator.UpperArm + Manipulator.Forearm + Manipulator.Palm, 2))
{
Assert.AreEqual(x, actual.X, 1e-5);
Assert.AreEqual(y, actual.Y, 1e-5);
}
else
{
Assert.AreEqual(Double.NaN, joints[0]);
Assert.AreEqual(Double.NaN, joints[1]);
Assert.AreEqual(Double.NaN, joints[2]);
}
}
