Advertisement
Guest User

Untitled

a guest
Nov 19th, 2019
95
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.82 KB | None | 0 0
  1. using System;
  2. using System.Drawing;
  3. using NUnit.Framework;
  4.  
  5. namespace Manipulation
  6. {
  7. public static class ManipulatorTask
  8. {
  9. public static double[] MoveManipulatorTo(double x, double y, double alpha)
  10. {
  11. var wx = 0.0;
  12. var wy = 0.0;
  13. wx = x - Manipulator.Palm * Math.Cos(alpha);
  14. wy = y + Manipulator.Palm * Math.Sin(alpha);
  15. var line = Math.Sqrt(Math.Pow(wx, 2) + Math.Pow(wy, 2));
  16. var shoulder = TriangleTask.GetABAngle(Manipulator.UpperArm, line, Manipulator.Forearm) + Math.Atan2(wy, wx);
  17. var elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, line);
  18. var wrist = -alpha - shoulder - elbow;
  19. if (shoulder == double.NaN || elbow == double.NaN || wrist == double.NaN)
  20. return new[] {
  21. double.NaN, double.NaN, double.NaN
  22. };
  23. else
  24. return new[] {
  25. shoulder, elbow, wrist
  26. };
  27. }
  28. }
  29.  
  30. [TestFixture]
  31. public class ManipulatorTask_Tests
  32. {
  33. [Test]
  34. public void TestMoveManipulatorTo()
  35. {
  36. var tests = new Random();
  37. var alphaAngle = tests.NextDouble();
  38. var positionX = tests.Next();
  39. var positionY = tests.Next();
  40. var angles = ManipulatorTask.MoveManipulatorTo(positionX, positionY, alphaAngle);
  41. if (angles[0] == double.NaN && angles[1] == double.NaN && angles[2] == double.NaN)
  42. {
  43. var coords = AnglesToCoordinatesTask.GetJointPositions(angles[0], angles[1], angles[2]);
  44. Assert.AreEqual(positionY, coords[2].Y, 1e-5);
  45. Assert.AreEqual(positionX, coords[2].X, 1e-5);
  46. }
  47. }
  48. }
  49. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement