Практика «Решение манипулятора»

MoveManipulatorTo

В том же проекте реализуйте метод MoveManipulatorTo в классе ManipulatorTasks.

Он должен возвращать массив углов new[] {shoulder, elbow, wrist}, необходимых для приведения эффектора манипулятора в точку (x, y) относительно крепления манипулятора к столу, и с углом между последним суставом и горизонталью равному alpha в радианах.

Если это невозможно, то возвращайте массив из трех double.NaN.

Сверяйтесь с чертежом! Не перепутайте углы и их направления!

Рандомизированный тест

В том же файле напишите вместо нескольких тестов один рандомизированный.

Он должен в цикле генерировать случайные значения X, Y, Angle, вызывать MoveManipulatorTo, по углам вычислять позицию эффектора и проверять, что она совпадает с требуемой.

Среди сгенерированных X, Y, Angle иногда будут попадаться нерешаемые. Например, если требуемая точка (X, Y) расположена слишком далеко, то метод вернет NaN-ы. Однако несложно определить, для каких точек задача имеет решение, а для каких нет. Подсказку ищите в коде задачи о визуализации. Ваш рандомизированный тест должен проверять, что NaN возвращается только тогда, когда решения действительно нет.

Подсказка про геометрию

Для решения этой задачи достаточно школьных знаний геометрии, однако поскольку наш курс посвящён программированию, и учитывая нелюбовь студентов к этой прекрасной области знаний, ниже дана схема геометрического решения этой задачи.

1. Проще всего найти угол wrist. Сделать это можно по формуле wrist = –alpha – shoulder – elbow.
2. Найдите координаты сустава Wrist по координатам (x, y) и углу alpha — они понадобятся в следующих шагах.
3. Найдите угол elbow с помощью метода GetABAngle в треугольнике (shoulder, elbow, wrist), в котором все три стороны теперь известны.
4. Найдите угол shoulder, как сумму двух углов:
   • угла (wrist, shoulder, elbow), который опять же можно найти с помощью метода GetABAngle.
   • и угла между лучом (shoulder, wrist) и осью OX. Его можно найти с помощью функции Math.Atan2 и координат сустава wrist.

Стоит заметить, что это не единственный способ решить данную задачу. Вы можете изобрести свой собственный.