public float DistanceFromTarget(Vector3 target, KinematicInfo info, int angleIndex)
{
Vector3 point = info.GetLastJoint().Position;
//We will use gradient descent here to work towards that.
return(Vector3.Distance(point, target));
}
public void InverseKinematics(Vector3 target, KinematicInfo info)
{
if (DistanceFromTarget(target, info) < DistanceThreshold)
{
return;
}
//Start at the end of the joints and work backwards (core part of INVERSE Kinematics)
for (int i = Joints.Length - 1; i >= 0; i--)
{
// Gradient descent
// Update : Solution -= LearningRate * Gradient
if (i < Joints.Length - 1)
{
float gradient = PartialGradient(target, info.Angles, i);
OfficialAngles[i] -= LearningRate * gradient;
if (ApplyJointClamping)
{
info.Angles[i] = Mathf.Clamp(info.Angles[i], Joints[i].MinAngle, Joints[i].MaxAngle);
}
// Early termination
if (DistanceFromTarget(target, info.Angles) < DistanceThreshold)
{
return;
}
}
}
}
public void HandleForwardKinematics()
{
CurrentResults = FwdKinematics(GetAngles());
//Debug.Log(CurrentResults.ToString());
ForwardKinematicsResult = ForwardKinematics(GetAngles());
LastTargetPosition = Joints[Joints.Length - 1].transform.position;
// Debug.Log("Position goes from: " + Joints[0].transform.position + " to " + result.ToString() + "\n");
}
public float PartialGradient(Vector3 target, float[] angles, int angleIndex)
{
// it will be restored later
float angle = angles[angleIndex];
//Evaluate before
KinematicInfo beforeInfo = FwdKinematics(angles);
float f_x = DistanceFromTarget(target, beforeInfo);
float f_y = CalculateOrientationAnglesForFullArm(Vector3.up, beforeInfo, angleIndex);
angles[angleIndex] += SamplingDistance;
KinematicInfo afterInfo = FwdKinematics(angles);
float f_x_plus_d = DistanceFromTarget(target, afterInfo);
float f_y_plus_d = CalculateOrientationAnglesForFullArm(Vector3.up, afterInfo, angleIndex);
// Gradient : [F(x+SamplingDistance) - F(x)] / h
//float f_x = DistanceFromTarget(target, angles);
//float f_y = CalculateOrientationAnglesForFullArm(Vector3.up, angles);
//angles[angleIndex] += SamplingDistance;
//float f_x_plus_d = DistanceFromTarget(target, angles);
//float f_y_plus_d = CalculateOrientationAnglesForFullArm(Vector3.up, angles);
float distanceGradient = (f_x_plus_d - f_x) / SamplingDistance;
float orientationGradient = (f_y_plus_d - f_y) / SamplingDistance;
float gradient = distanceGradient;
if (UseOrientationFitness)
{
float percentDistance = distanceGradient * (PositionalWeight / (PositionalWeight + OrientationWeight));
float percentOrientation = orientationGradient * (OrientationWeight / (PositionalWeight + OrientationWeight));
percentOrientation = percentOrientation / 100;
gradient = percentDistance + percentOrientation;
}
// Restores
angles[angleIndex] = angle;
//Debug.Log(gradient + "\n");
return(gradient);
}
private void DrawKinematicInfoGizmos()
{
Vector3 VerticalOffset = Vector3.up * 1.25f;
KinematicInfo info = CurrentResults;
if (enabled && ShouldDrawKinematicGizmos)
{
for (int i = 0; i < info.JointCount; i++)
{
JointInformation joint = info.GetJointInformation(i);
Gizmos.color = new Color(1, 1, 1, .5f);
Gizmos.DrawCube(VerticalOffset + joint.Position + Vector3.up * i * .05f, Vector3.one * .05f);
Gizmos.color = Color.green;
Gizmos.DrawRay(VerticalOffset + joint.Position + Vector3.up * i * .05f, joint.UpDirection.normalized);
Gizmos.color = Color.blue;
Gizmos.DrawRay(VerticalOffset + joint.Position + Vector3.up * i * .05f, joint.RightDirection.normalized);
}
}
}
public float CalculateOrientationAnglesForFullArm(Vector3 target, KinematicInfo info, int angleIndex)
{
return(info.SumAnglesFromComfortForEachJoint());
}
public KinematicInfo FwdKinematics(float[] angles, bool primaryCall = false, int depth = int.MaxValue)
{
KinematicInfo info = new KinematicInfo(angles, depth);
//Start with the position of the first joint
Vector3 prevPoint = Vector3.zero;
Vector3 prevUp = Vector3.up;
Vector3 prevRight = Vector3.right;
Vector3 nextUp = Vector3.up;
Vector3 nextRight = Vector3.right;
//Vector3 prevPoint = Joints[0].transform.position;
Quaternion rotation = Quaternion.identity;
string reportAngles = "";
//For each required joint
for (int i = 1; i < Joints.Length && i < depth; i++)
{
//Make sure we're using the CORRECT current index
int currentIndex = i - 1;
// Rotates around that joint's axis
rotation *= Quaternion.AngleAxis(angles[currentIndex], Joints[currentIndex].Axis);
//Calculate the point of this joint based on the previous point + the rotation offset start offset.
Vector3 ThisJointsRotatedOffset = rotation * Joints[i].StartOffset;
Vector3 nextPoint = prevPoint + ThisJointsRotatedOffset;
nextUp = rotation * prevUp;
nextRight = rotation * prevRight;
if (Offsets != null && Offsets.Length > i)
{
reportAngles += "[" + Joints[currentIndex].name + "] - assigned " + Joints[currentIndex].StartOffset + " " + Joints[currentIndex].transform.localPosition + "\n";
Offsets[currentIndex] = Joints[currentIndex].StartOffset;
}
prevPoint = nextPoint;
prevUp = nextUp;
prevRight = nextRight;
info.Set(i, nextPoint, nextUp, nextRight, Joints[i]);
if (RotationBase != null && UseRotationBase)
{
Joints[currentIndex].transform.rotation = RotationBase.transform.rotation * rotation;
}
else
{
Joints[currentIndex].transform.rotation = rotation;
}
}
if (ReportLogs)
{
Debug.Log(reportAngles + "\n");
}
//return Joints[0].transform.position + prevPoint;
return(info);
}
