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));
    }
Example #2
0
    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");
    }
Example #4
0
    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);
    }