private void SearchAngles_Click(object sender, RoutedEventArgs e)
{
double[] target = { targetEdit.PointX, targetEdit.PointY, targetEdit.PointZ };
var dht = GetMDHParameters();
bool success = false;
double[] angles = RoboticMath.InverseKinematics(dht, target, ref success);
searchedAngles.Children.Clear();
if (success)
{
for (int i = 0; i < angles.Length; i++)
{
var text = new TextBlock();
text.Text = String.Format("Angle {0}: {1:0.00}", i, (float)RadiansToDegrees(angles[i]));
searchedAngles.Children.Add(text);
}
}
else
{
searchedAngles.Children.Add(new TextBlock()
{
Text = "Angles Not Found"
});
}
}
private void LinkControl_OnValueChanged(object sender, RoutedEventArgs e)
{
var source = e.Source as LinkControl;
var arm = DataContext as RoboticArm;
var jointControlValue = DegreesToRadians(source.JointControl.Value);
double initValue;
double currentValue;
if (arm.GetLink(source.LinkNum).Parameters.LinkType == LinkType.REVOLUTE)
{
initValue = (linksList.Children[source.LinkNum] as LinkEdit).Theta;
currentValue = arm.GetLink(source.LinkNum).Parameters.Theta;
}
else
{
initValue = (linksList.Children[source.LinkNum] as LinkEdit).D;
currentValue = arm.GetLink(source.LinkNum).Parameters.D;
}
arm.Displace(source.LinkNum, -currentValue + initValue + jointControlValue);
arm.Render();
var tipPose = RoboticMath.GetPositionVector(RoboticMath.GetTransformMatrix(arm.GetParameters()));
tipPosition.Text = String.Format("{0:0.00}; {1:0.00}; {2:0.00};", tipPose[0], tipPose[1], tipPose[2]);
}
private void MakeRoboticArm()
{
RoboticArm arm = DataContext as RoboticArm;
arm.Clear();
var parameters = GetMDHParameters();
foreach (var parameter in parameters)
{
arm.AddLink(new Link(parameter));
}
var tipPose = RoboticMath.GetPositionVector(RoboticMath.GetTransformMatrix(arm.GetParameters()));
tipPosition.Text = String.Format("{0:0.00}; {1:0.00}; {2:0.00};", tipPose[0], tipPose[1], tipPose[2]);
}
