private void JTIterate()
{
var jacobian = ComputeJacobian();
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var deltaE = SafeDeltaE(goal, baseJoint.EndEffector);
var deltaEVec = CreateVector.Dense(new float[] { deltaE.X, deltaE.Y });
var denom = (jacobianMat * jacobianMat.Transpose() * deltaEVec).L2Norm();
var lambda = (deltaEVec * jacobianMat * jacobianMat.Transpose() * deltaEVec) / (denom * denom);
var deltaPhiVec = (float)lambda * jacobianMat.Transpose() * deltaEVec;
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
baseJoint.ApplyDofDeltas(deltaPhi);
}
public void IterateDLS(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var lambda = DLSLambda;
var lambdaMatrix = lambda * lambda * CreateMatrix.DenseIdentity <float>(jacobianMat.RowCount);
var deltaPhiVec =
jacobianMat.Transpose() *
(jacobianMat * jacobianMat.Transpose() + (lambdaMatrix)).Inverse() *
CreateVector.DenseOfArray(new float[] { deltaE.X, deltaE.Y });
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
joint.ApplyDofDeltas(deltaPhi);
}
public void IterateJI(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var inverse = jacobianMat.GeneralizedInverse();
var pInverse = jacobianMat.SVDPseudoInverse();
var deltaPhi = new LinkedList <float>();
foreach (var row in pInverse.ToRowArrays())
{
deltaPhi.AddLast((deltaE.X * row[0]) + (deltaE.Y * row[1]));
}
joint.ApplyDofDeltas(deltaPhi);
}
public void IterateJT(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
if (deltaE.LengthSquared() < 0.0001)
{
return;
}
var deltaEVec = CreateVector.Dense(new float[] { deltaE.X, deltaE.Y });
var denom = (jacobianMat * jacobianMat.Transpose() * deltaEVec).L2Norm();
var lambda = (deltaEVec * jacobianMat * jacobianMat.Transpose() * deltaEVec) / (denom * denom);
var deltaPhiVec = (float)lambda * jacobianMat.Transpose() * deltaEVec;
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
joint.ApplyDofDeltas(deltaPhi);
}