public ArmManipulator(ArmController armController, ArmPresenter presenter, InverseKinematicsCalculator inverseKinematicsCalculator,
KinematicChain kinematicChain)
{
KinematicChain = kinematicChain;
InverseKinematicsCalculator = inverseKinematicsCalculator;
ArmController = armController;
Presenter = presenter;
CurrentPosition = new Vector3D
{
X = 0.061,
Y = 0.0,
Z = 0.1
};
CurrentServoPositions = new Dictionary <int, int>();
Recording = new List <ArmState>();
}
public void AdjustKinematicChainForPosition(KinematicChain kinematicChain, Vector3D targetPosition)
{
RandomizeCount = 0;
TargetPosition = targetPosition;
KinematicChain = kinematicChain;
SetCurrentThetasFromChain();
CurrentDelta = 30;
NumberOfThetas = kinematicChain.InputLinks.Select(link => link.Theta).ToArray().Length;
LastDistanceToTarget = kinematicChain.CalculateResultantPosition().EuclidianDistanceTo(targetPosition);
while (true)
{
var thisDeltaIsAxhausted = true;
for (var i = NumberOfThetas - 1; i >= 0; i--)
{
var positionWasImproved = TryToImproveThetaByUsingDelta(i);
if (positionWasImproved)
{
thisDeltaIsAxhausted = false;
}
}
if (thisDeltaIsAxhausted)
{
CurrentDelta /= 2;
}
if (LastDistanceToTarget < 0.00001)
{
ApplyThetas();
return; //Reached goal
}
if (CurrentDelta < 0.00001)
{
RandomizeThetas();
}
}
}