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); }