private void SolveInverseKinematics(Vector3 targetPosition)
{
var robotJoints = _kinematicsSolver.Joints;
var jointOffsets = robotJoints.Select(x => x.StartOffset).ToArray();
var jointAxis = robotJoints.Select(x => x.Axis).ToArray();
float[] jointAngles = new float[_kinematicsSolver.Joints.Length];
_kinematicProblem = new KinematicProblem(robotJoints[0].transform.position,
jointOffsets, jointAxis, targetPosition, jointAngles);
_kinnematicsSolverFinished = false;
Thread kinematicThread = new Thread(solveKinematics);
kinematicThread.Start(_kinematicProblem);
}
public static Vector3 ForwardKinematics(KinematicProblem problem)
{
Vector3 prevPoint = problem.firstJointPosition;
Quaternion rotation = Quaternion.identity;
for (int i = 1; i < problem.angles.Length; i++)
{
// Rotates around a new axis
rotation *= Quaternion.AngleAxis(problem.angles[i - 1], problem.jointAxis[i - 1]);
Vector3 nextPoint = prevPoint + rotation * problem.jointOffsets[i];
prevPoint = nextPoint;
}
return(prevPoint);
}
public static float PartialGradient(KinematicProblem problem, int i)
{
// Saves the angle,
// it will be restored later
float angle = problem.angles[i];
// Gradient : [F(x+SamplingDistance) - F(x)] / h
float f_x = DistanceFromTarget(problem);
problem.angles[i] += SamplingDistance;
float f_x_plus_d = DistanceFromTarget(problem);
float gradient = (f_x_plus_d - f_x) / SamplingDistance;
// Restores
problem.angles[i] = angle;
return(gradient);
}
public static void InverseKinematics(KinematicProblem problem)
{
Debug.Log("------ Begin IK ------");
Debug.Log("Original distance: " + DistanceFromTarget(problem));
Debug.Log("------------------------");
var step = 0;
// var maxSteps = 50000;
while (step < maxSteps && !(DistanceFromTarget(problem) < DistanceThreshold))
{
for (int i = problem.angles.Length - 3; i >= 0; i--)
{
// Gradient descent
// Update : Solution -= LearningRate * Gradient
float gradient = PartialGradient(problem, i);
problem.angles[i] -= LearningRate * gradient;
// Early termination
if (DistanceFromTarget(problem) < DistanceThreshold)
{
Debug.Log("------ Early End IK ------");
Debug.Log("Final distance (success true): " + DistanceFromTarget(problem).ToString("F4"));
return;
}
}
step++;
}
var finalDistance = DistanceFromTarget(problem);
Debug.Log("------ End IK ------");
Debug.Log("Final distance (success " + (finalDistance < DistanceThreshold) + "): " + finalDistance.ToString("F4"));
}
private void interactWithLoadedRobot()
{
if (Input.touchCount > 0)
{
// If clicking on one of UI element, no raycast or robot interaction possible
if (IsPointerOverUIObject())
{
return;
}
var screenTouchPosition = new Vector2(Input.GetTouch(0).position.x, Input.GetTouch(0).position.y);
if (!_arRaycastManager.Raycast(screenTouchPosition, s_Hits, TrackableType.PlaneWithinPolygon))
{
return;
}
var hitPose = s_Hits[0].pose;
_positionMarker = Instantiate(positionPrefab, hitPose.position, Quaternion.identity);
StartCoroutine(waitSeconds(1.0f));
UpdateJointsToPosition(hitPose.position);
}
if (!_robotMovedFinished)
{
var robotJoints = _kinematicsSolver.Joints;
int numberOfMovableJoint = robotJoints.Length - 2;
var jointOffsets = robotJoints.Select(x => x.StartOffset).ToArray();
var jointAxis = new Vector3[]
{
new Vector3(1, 0, 0),
new Vector3(0, 0, 1),
new Vector3(0, 0, 1),
new Vector3(1, 0, 0),
new Vector3(0, 0, 1),
new Vector3(1, 0, 0),
new Vector3(0, 0, 0)
};
float[] jointAngles = new float[_kinematicsSolver.Joints.Length];
var pb = new KinematicProblem(robotJoints[0].transform.position,
jointOffsets, jointAxis, _kinematicProblem.targetPosition, jointAngles);
Vector3 point = KinematicsSolver.ForwardKinematics(pb);
screenLogger.text = "Target: " + _kinematicProblem.targetPosition.ToString() + "\n";
screenLogger.text += "Curr: " + point.ToString() + "\n";
var distanceFromTarget = KinematicsSolver.DistanceFromTarget(pb);
screenLogger.text += "Dist: " + distanceFromTarget + "\n";
Dictionary <int, Vector3> _jointEditorAxis = new Dictionary <int, Vector3>(numberOfMovableJoint);
_jointEditorAxis.Add(0, new Vector3(1, 0, 0));
_jointEditorAxis.Add(1, new Vector3(0, 0, 1));
_jointEditorAxis.Add(2, new Vector3(0, 0, 1));
_jointEditorAxis.Add(3, new Vector3(1, 0, 0));
_jointEditorAxis.Add(4, new Vector3(0, 0, 1));
printArray(_jointAngles);
float[] currAngles = new float[numberOfMovableJoint];
for (int i = 0; i < numberOfMovableJoint; i++)
{
robotJoints[i].transform.rotation.ToAngleAxis(out float angle, out Vector3 axis);
currAngles[i] = angle;
var angleIncrement = _jointAngles[i] - angle;
robotJoints[i].transform.Rotate(_jointEditorAxis[i], _jointAngles[i]);
}
printArray(currAngles);
float[] updatedAngles = new float[numberOfMovableJoint];
for (int i = 0; i < numberOfMovableJoint; i++)
{
robotJoints[i].transform.rotation.ToAngleAxis(out float angle, out Vector3 axis);
updatedAngles[i] = angle;
}
pb = new KinematicProblem(robotJoints[0].transform.position,
jointOffsets, jointAxis, _kinematicProblem.targetPosition, jointAngles);
point = KinematicsSolver.ForwardKinematics(pb);
printArray(updatedAngles);
screenLogger.text += "Updated pos: " + point.ToString() + "\n";
_robotMovedFinished = true;
Destroy(_positionMarker);
}
}
public static float DistanceFromTarget(KinematicProblem problem)
{
Vector3 point = ForwardKinematics(problem);
return(Vector3.Distance(point, problem.targetPosition));
}