/
dashytka
/
MoveManipulatorTo
Обзор
Документация
Войти
/
dashytka
/
MoveManipulatorTo
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
CI/CD
Аналитика
Безопасность
master
c
34 строки
1 KB
dashytka
create c#
11 дек 2025, 22:47
11 дек 2025, 22:47
4973d08
Код
Авторство
О чём код?
using System; using static Manipulation.Manipulator; namespace Manipulation; public static class ManipulatorTask { public static double[] MoveManipulatorTo(double x, double y, double alpha) { var l1 = UpperArm; var l2 = Forearm; var l3 = Palm; var wristX = x - l3 * Math.Cos(alpha); var wristY = y - l3 * Math.Sin(alpha); var d = Math.Sqrt(wristX * wristX + wristY * wristY); if (d > l1 + l2 || d < Math.Abs(l1 - l2)) return new[] { double.NaN, double.NaN, double.NaN }; var cosElbow = (l1 * l1 + l2 * l2 - d * d) / (2 * l1 * l2); cosElbow = Math.Max(-1.0, Math.Min(1.0, cosElbow)); var elbow = Math.PI - Math.Acos(cosElbow); var theta = Math.Atan2(wristY, wristX); var phi = Math.Acos((l1 * l1 + d * d - l2 * l2) / (2 * l1 * d)); var shoulder = theta - phi; var wrist = -alpha - shoulder - elbow; return new[] { shoulder, elbow, wrist }; } }