public void IkTest1() { var angles = IkSolver.GetAnglesForXYZ(new Point3(100, 70, 125), 148, 161); //double b2, b3; //Form1.xyControl_InverseKinematicsSolver(125, 70, 148, 161, out b2, out b3); //Assert.AreEqual(a2, b2, 0.0001); //Assert.AreEqual(a3, b3, 0.0001); }
private void mouse3dController1_PositionChanged(object sender, Point3 newPosition) { // DAVE: calculate time based on the distance. // Move this code to the client to a function MoveEffector(x, y, z, rx, ry, rz) (6 axis) // the function will do its best to approximate the required position with the robot axes. // This will make it work in future setups. The function may return a struct with the calculated angles. var angles = IkSolver.GetAnglesForXYZ(newPosition, 148, 161); //armIface.Arm.SetAngles(a1, a2, a3, GripperAngleTrackbar.Value); armIface.Arm.SetAngles(angles, 100); m3dScene.Angle1 = angles.A1; m3dScene.Angle2 = angles.A2; m3dScene.Angle3 = angles.A3; //label1.Text = string.Format("Location ({3:0},{4:0},{5:0}) Angles: {0:0.}, {1:0.}, {2:0.}", a1, a2, a3, newPosition.X, newPosition.Y, newPosition.Z); }