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);
}
