Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using System;
- using System.Drawing;
- using NUnit.Framework;
- namespace Manipulation
- {
- public static class ManipulatorTask
- {
- public static double[] MoveManipulatorTo(double x, double y, double alpha)
- {
- var wx = 0.0;
- var wy = 0.0;
- wx = x - Manipulator.Palm * Math.Cos(alpha);
- wy = y + Manipulator.Palm * Math.Sin(alpha);
- var line = Math.Sqrt(Math.Pow(wx, 2) + Math.Pow(wy, 2));
- var shoulder = TriangleTask.GetABAngle(Manipulator.UpperArm, line, Manipulator.Forearm) + Math.Atan2(wy, wx);
- var elbow = TriangleTask.GetABAngle(Manipulator.UpperArm, Manipulator.Forearm, line);
- var wrist = -alpha - shoulder - elbow;
- if (shoulder == double.NaN || elbow == double.NaN || wrist == double.NaN)
- return new[] {
- double.NaN, double.NaN, double.NaN
- };
- else
- return new[] {
- shoulder, elbow, wrist
- };
- }
- }
- [TestFixture]
- public class ManipulatorTask_Tests
- {
- [Test]
- public void TestMoveManipulatorTo()
- {
- var tests = new Random();
- var alphaAngle = tests.NextDouble();
- var positionX = tests.Next();
- var positionY = tests.Next();
- var angles = ManipulatorTask.MoveManipulatorTo(positionX, positionY, alphaAngle);
- if (angles[0] == double.NaN && angles[1] == double.NaN && angles[2] == double.NaN)
- {
- var coords = AnglesToCoordinatesTask.GetJointPositions(angles[0], angles[1], angles[2]);
- Assert.AreEqual(positionY, coords[2].Y, 1e-5);
- Assert.AreEqual(positionX, coords[2].X, 1e-5);
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement