SHARE
TWEET

Untitled

a guest Nov 19th, 2019 81 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top