Unity Fabrik IK Script

2024-05-27 12:27 UTC gpt-4o Open in ChatGPT ↗

You

Can you figure out the inverse kinematics of this system, and implement them via the FABRIK algorithm in a Unity script?

ChatGPT

To implement the inverse kinematics of this system using the FABRIK (Forward And Backward Reaching Inverse Kinematics) algorithm in Unity, we’ll need to understand the structure and constraints of the robotic arm depicted in the image. Here’s a step-by-step outline and example Unity script for implementing FABRIK:

Understanding the System

From the image, we have:

• Four joints: Joint 01 (Base), Joint 02 (Main), Joint 03 (Secondary), Joint 04 (Speaker Swivel).
• Minimum and maximum reach constraints.
• The robot will ignore values below the ground line.

Steps to Implement FABRIK in Unity

1. Set Up the Scene in Unity:

   • Create empty GameObjects for each joint.
   • Position the GameObjects to match the initial configuration shown in the image.
   • Attach the script to a main controller GameObject.

2. FABRIK Algorithm Implementation:

   • The algorithm iteratively adjusts the positions of the joints to reach the target.

Here’s a sample Unity C# script implementing the FABRIK algorithm:

using System.Collections.Generic;
using UnityEngine;

public class FabrikIK : MonoBehaviour
{
    public Transform target;
    public Transform[] joints;
    public float[] segmentLengths;
    public float tolerance = 0.01f;
    public int maxIterations = 10;

    void Start()
    {
        if (joints.Length != segmentLengths.Length + 1)
        {
            Debug.LogError("Number of joints should be one more than the number of segments.");
        }
    }

    void Update()
    {
        if (Vector3.Distance(joints[joints.Length - 1].position, target.position) > tolerance)
        {
            PerformFABRIK();
        }
    }

    void PerformFABRIK()
    {
        int iterations = 0;
        while (Vector3.Distance(joints[joints.Length - 1].position, target.position) > tolerance && iterations < maxIterations)
        {
            ForwardPass();
            BackwardPass();
            iterations++;
        }
    }

    void ForwardPass()
    {
        joints[joints.Length - 1].position = target.position;
        for (int i = joints.Length - 2; i >= 0; i--)
        {
            float distance = Vector3.Distance(joints[i + 1].position, joints[i].position);
            float ratio = segmentLengths[i] / distance;
            Vector3 newPosition = (1 - ratio) * joints[i + 1].position + ratio * joints[i].position;
            joints[i].position = newPosition;
        }
    }

    void BackwardPass()
    {
        joints[0].position = Vector3.zero; // Assuming the base joint is fixed at origin
        for (int i = 1; i < joints.Length; i++)
        {
            float distance = Vector3.Distance(joints[i].position, joints[i - 1].position);
            float ratio = segmentLengths[i - 1] / distance;
            Vector3 newPosition = (1 - ratio) * joints[i - 1].position + ratio * joints[i].position;
            joints[i].position = newPosition;
        }
    }
}

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FabrikIK script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths based on the distances between the joints.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths and tolerance to get the desired behavior.

This script provides a basic implementation of the FABRIK algorithm for a robotic arm with four joints. Depending on the specific constraints and additional requirements (like rotational limits or specific reach zones), you might need to extend this script further.

You

but can you see the “scissor-like” structure? the angle of joint3 affects the position of joint4

ChatGPT

I see the scissor-like structure in the image. To account for the dependency of Joint 04 on the angle of Joint 03, we need to adjust the FABRIK algorithm to consider this relationship. Here’s an updated approach to implement the FABRIK algorithm considering the scissor mechanism.

Steps to Implement FABRIK with Scissor Mechanism in Unity

1. Set Up the Scene in Unity:

   • Create empty GameObjects for each joint.
   • Position the GameObjects to match the initial configuration shown in the image.
   • Attach the script to a main controller GameObject.

2. Adjust the FABRIK Algorithm to Include Scissor Mechanism:

   • Calculate the position of Joint 04 based on the angle of Joint 03.
   • Implement the forward and backward passes considering the scissor-like dependency.

Here’s a sample Unity C# script implementing the FABRIK algorithm with the scissor mechanism:

using System.Collections.Generic;
using UnityEngine;

public class FabrikScissorIK : MonoBehaviour
{
    public Transform target;
    public Transform[] joints;
    public float[] segmentLengths;
    public float tolerance = 0.01f;
    public int maxIterations = 10;

    void Start()
    {
        if (joints.Length != segmentLengths.Length + 1)
        {
            Debug.LogError("Number of joints should be one more than the number of segments.");
        }
    }

    void Update()
    {
        if (Vector3.Distance(joints[joints.Length - 1].position, target.position) > tolerance)
        {
            PerformFABRIK();
        }
    }

    void PerformFABRIK()
    {
        int iterations = 0;
        while (Vector3.Distance(joints[joints.Length - 1].position, target.position) > tolerance && iterations < maxIterations)
        {
            ForwardPass();
            BackwardPass();
            iterations++;
        }
    }

    void ForwardPass()
    {
        joints[joints.Length - 1].position = target.position;
        for (int i = joints.Length - 2; i >= 0; i--)
        {
            float distance = Vector3.Distance(joints[i + 1].position, joints[i].position);
            float ratio = segmentLengths[i] / distance;
            Vector3 newPosition = (1 - ratio) * joints[i + 1].position + ratio * joints[i].position;
            joints[i].position = newPosition;

            // Update the position of Joint 04 based on the angle of Joint 03
            if (i == 2)
            {
                UpdateJoint04Position();
            }
        }
    }

    void BackwardPass()
    {
        joints[0].position = Vector3.zero; // Assuming the base joint is fixed at origin
        for (int i = 1; i < joints.Length; i++)
        {
            float distance = Vector3.Distance(joints[i].position, joints[i - 1].position);
            float ratio = segmentLengths[i - 1] / distance;
            Vector3 newPosition = (1 - ratio) * joints[i - 1].position + ratio * joints[i].position;
            joints[i].position = newPosition;

            // Update the position of Joint 04 based on the angle of Joint 03
            if (i == 3)
            {
                UpdateJoint04Position();
            }
        }
    }

    void UpdateJoint04Position()
    {
        // Calculate the angle of Joint 03
        Vector3 direction = (joints[2].position - joints[1].position).normalized;
        float angle = Vector3.Angle(Vector3.right, direction);

        // Calculate the position of Joint 04 based on the angle of Joint 03
        float offsetX = segmentLengths[2] * Mathf.Cos(angle * Mathf.Deg2Rad);
        float offsetY = segmentLengths[2] * Mathf.Sin(angle * Mathf.Deg2Rad);
        joints[3].position = joints[2].position + new Vector3(offsetX, offsetY, 0);
    }
}

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FabrikScissorIK script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths based on the distances between the joints.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths and tolerance to get the desired behavior.

This script provides a basic implementation of the FABRIK algorithm with consideration for the scissor mechanism. You may need to adjust the script further based on the exact geometry and constraints of your robotic arm.

You

given this IK code, and this image, adjust the code so that we can apply arbitrary angle constraints on joint P1

private static void FABRIK_Pass_2(float3 P0, float3 P1, float3 P2, float3 E, float L1, float L2, float L3, float L3_short, out float3 P1_out, out float3 P2_out, out float3 E_out) { //------------------------------- // BACKWARD PASS //------------------------------- // End Effector to P2 direction var v1 = math.normalize(P2 - E);

    // New P2 = End Effector + L3 Length
    P2 = E + v1 * L3;

    // Back Extension using L3_short
    var P2_Ext = P2 + v1 * L3_short;

    // New P2 to old P1 direction
    var v2 = math.normalize(P1 - P2);
    // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
    P1 = P2 + v2 * L2;

    // Building ABC triangle, to find exact position of P1 after constraint
    // We use the temp position of P1 and the desired P0 to find the corner C
    var C = P1 + math.normalize(P0-P1)*L3_short;
    var B = P2;
    var A = P2_Ext;
    P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy),0);

    //-------------------------------
    // FORWARD PASS
    //-------------------------------
    v1 = math.normalize(P1 - P0);
    P1 = P0 + v1 * L1;
    var P1B = P0 + v1 * (L1-L3_short);

    v2 = math.normalize(P2 - P1);
    P2 = P1 + v2 * L2;

    B = P1B;
    A = P1;
    C = P2;

    var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy),0);

    var v3 =  math.normalize(C-D);
    var NEXT_JOINT = D + v3 * (L3 + L3_short);


    P1_out = P1;
    P2_out = P2;
    E_out = NEXT_JOINT;
}

ChatGPT

To adjust the code to apply arbitrary angle constraints on joint P1, we will need to modify the FABRIK algorithm implementation to include angle constraints during the position adjustments. Below is an updated version of the code with angle constraints applied to joint P1.

Updated FABRIK Algorithm with Angle Constraints

We will add a method to apply angle constraints and update the FABRIK_Pass_2 method accordingly.

using Unity.Mathematics;

public static class FABRIKWithConstraints
{
    public static void FABRIK_Pass_2(
        float3 P0, float3 P1, float3 P2, float3 E,
        float L1, float L2, float L3, float L3_short,
        float minAngleP1, float maxAngleP1, // Angle constraints for P1
        out float3 P1_out, out float3 P2_out, out float3 E_out)
    {
        //-------------------------------
        // BACKWARD PASS
        //-------------------------------
        // End Effector to P2 direction
        var v1 = math.normalize(P2 - E);

        // New P2 = End Effector + L3 Length
        P2 = E + v1 * L3;

        // Back Extension using L3_short
        var P2_Ext = P2 + v1 * L3_short;

        // New P2 to old P1 direction
        var v2 = math.normalize(P1 - P2);
        // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
        P1 = P2 + v2 * L2;

        // Building ABC triangle, to find exact position of P1 after constraint
        // We use the temp position of P1 and the desired P0 to find the corner C
        var C = P1 + math.normalize(P0 - P1) * L3_short;
        var B = P2;
        var A = P2_Ext;
        P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        // Apply angle constraints to P1
        P1 = ApplyAngleConstraint(P0, P1, minAngleP1, maxAngleP1);

        //-------------------------------
        // FORWARD PASS
        //-------------------------------
        v1 = math.normalize(P1 - P0);
        P1 = P0 + v1 * L1;
        var P1B = P0 + v1 * (L1 - L3_short);

        v2 = math.normalize(P2 - P1);
        P2 = P1 + v2 * L2;

        B = P1B;
        A = P1;
        C = P2;

        var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        var v3 = math.normalize(C - D);
        var NEXT_JOINT = D + v3 * (L3 + L3_short);

        P1_out = P1;
        P2_out = P2;
        E_out = NEXT_JOINT;
    }

    private static float2 FindMissingScissorPoint(float2 A, float2 B, float2 C)
    {
        // Implement the logic to find the missing point in the scissor mechanism
        // This will depend on the specific geometry and constraints of your mechanism
        return new float2();
    }

    private static float3 ApplyAngleConstraint(float3 origin, float3 point, float minAngle, float maxAngle)
    {
        // Calculate the current angle
        float currentAngle = math.degrees(math.atan2(point.y - origin.y, point.x - origin.x));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the new position based on the constrained angle
        float distance = math.distance(origin, point);
        float constrainedX = origin.x + distance * math.cos(math.radians(constrainedAngle));
        float constrainedY = origin.y + distance * math.sin(math.radians(constrainedAngle));

        return new float3(constrainedX, constrainedY, point.z);
    }
}

Explanation

1. Angle Constraint Method:

   • ApplyAngleConstraint is a method that constrains the angle of a point relative to an origin within specified minimum and maximum angles.
   • It calculates the current angle, clamps it within the specified range, and calculates the new position based on the constrained angle.

2. Updated FABRIK_Pass_2 Method:

   • The method now accepts minimum and maximum angle constraints for P1.
   • During the backward pass, after calculating the new position for P1, it applies the angle constraint using the ApplyAngleConstraint method.
   • The forward pass remains unchanged but will work with the newly constrained position of P1.

3. Usage:

   • To use this updated FABRIK algorithm, call FABRIK_Pass_2 with the additional angle constraint parameters.

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FabrikWithConstraints script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths and angle constraints based on your specific requirements.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths, angle constraints, and tolerance to get the desired behavior.

This updated script provides the necessary functionality to apply angle constraints on joint P1 while performing the FABRIK inverse kinematics. Adjustments may be needed based on the exact geometry and constraints of your robotic arm.

You

we need to calculate the angle between p2-P1 and p1-p0

ChatGPT

To calculate the angle between vectors $P2 - P1$ and $P1 - P0$, we can use the dot product formula. This will help us enforce the angle constraints during the FABRIK algorithm.

Here’s the updated FABRIK_Pass_2 method with the calculation of the angle and the enforcement of the angle constraints:

using Unity.Mathematics;
using UnityEngine;

public static class FABRIKWithConstraints
{
    public static void FABRIK_Pass_2(
        float3 P0, float3 P1, float3 P2, float3 E,
        float L1, float L2, float L3, float L3_short,
        float minAngleP1, float maxAngleP1, // Angle constraints for P1
        out float3 P1_out, out float3 P2_out, out float3 E_out)
    {
        //-------------------------------
        // BACKWARD PASS
        //-------------------------------
        // End Effector to P2 direction
        var v1 = math.normalize(P2 - E);

        // New P2 = End Effector + L3 Length
        P2 = E + v1 * L3;

        // Back Extension using L3_short
        var P2_Ext = P2 + v1 * L3_short;

        // New P2 to old P1 direction
        var v2 = math.normalize(P1 - P2);
        // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
        P1 = P2 + v2 * L2;

        // Building ABC triangle, to find exact position of P1 after constraint
        // We use the temp position of P1 and the desired P0 to find the corner C
        var C = P1 + math.normalize(P0 - P1) * L3_short;
        var B = P2;
        var A = P2_Ext;
        P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        // Apply angle constraints to P1
        P1 = ApplyAngleConstraint(P0, P1, P2, minAngleP1, maxAngleP1);

        //-------------------------------
        // FORWARD PASS
        //-------------------------------
        v1 = math.normalize(P1 - P0);
        P1 = P0 + v1 * L1;
        var P1B = P0 + v1 * (L1 - L3_short);

        v2 = math.normalize(P2 - P1);
        P2 = P1 + v2 * L2;

        B = P1B;
        A = P1;
        C = P2;

        var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        var v3 = math.normalize(C - D);
        var NEXT_JOINT = D + v3 * (L3 + L3_short);

        P1_out = P1;
        P2_out = P2;
        E_out = NEXT_JOINT;
    }

    private static float2 FindMissingScissorPoint(float2 A, float2 B, float2 C)
    {
        // Implement the logic to find the missing point in the scissor mechanism
        // This will depend on the specific geometry and constraints of your mechanism
        return new float2();
    }

    private static float3 ApplyAngleConstraint(float3 P0, float3 P1, float3 P2, float minAngle, float maxAngle)
    {
        // Calculate vectors
        float3 v1 = math.normalize(P1 - P0);
        float3 v2 = math.normalize(P2 - P1);

        // Calculate the current angle between the vectors
        float dotProduct = math.dot(v1, v2);
        float currentAngle = math.degrees(math.acos(dotProduct));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the rotation required to get the constrained angle
        float angleDifference = constrainedAngle - currentAngle;
        float3 axis = math.cross(v1, v2);
        quaternion rotation = quaternion.AxisAngle(axis, math.radians(angleDifference));

        // Apply the rotation to P1 to get the constrained position
        float3 newP1 = P0 + math.rotate(rotation, v1) * math.distance(P0, P1);

        return newP1;
    }
}

Explanation

1. Calculating the Angle:

   • v1 and v2 are the normalized vectors from $P0$ to $P1$ and from $P1$ to $P2$, respectively.
   • The dot product of v1 and v2 is used to find the angle between them.
   • The math.degrees(math.acos(dotProduct)) function calculates the current angle in degrees.

2. Constraining the Angle:

   • The angle is clamped between the minimum and maximum specified angles.
   • The difference between the constrained angle and the current angle (angleDifference) is calculated.
   • A quaternion rotation is computed using the axis of rotation (cross product of v1 and v2) and the angle difference.
   • The rotation is applied to the vector v1 to find the new constrained position of $P1$.

3. Applying the Angle Constraint in FABRIK:

   • The ApplyAngleConstraint method is called after calculating the unconstrained position of $P1$.
   • The method adjusts $P1$ to satisfy the angle constraints relative to $P0$ and $P2$.

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FABRIKWithConstraints script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths, angle constraints, and tolerance based on your specific requirements.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths, angle constraints, and tolerance to get the desired behavior.

This updated script provides the necessary functionality to apply angle constraints on joint P1 while performing the FABRIK inverse kinematics. Adjustments may be needed based on the exact geometry and constraints of your robotic arm.

You

why don’t you apply the angle constraint on both passes?

ChatGPT

You’re right, applying the angle constraint only in one pass might not fully enforce the constraints throughout the entire FABRIK process. We should apply the angle constraints during both the backward and forward passes to ensure that the constraints are maintained. Below is the updated code to apply the angle constraint in both passes.

Updated FABRIK Algorithm with Angle Constraints Applied in Both Passes

using Unity.Mathematics;
using UnityEngine;

public static class FABRIKWithConstraints
{
    public static void FABRIK_Pass_2(
        float3 P0, float3 P1, float3 P2, float3 E,
        float L1, float L2, float L3, float L3_short,
        float minAngleP1, float maxAngleP1, // Angle constraints for P1
        out float3 P1_out, out float3 P2_out, out float3 E_out)
    {
        //-------------------------------
        // BACKWARD PASS
        //-------------------------------
        // End Effector to P2 direction
        var v1 = math.normalize(P2 - E);

        // New P2 = End Effector + L3 Length
        P2 = E + v1 * L3;

        // Back Extension using L3_short
        var P2_Ext = P2 + v1 * L3_short;

        // New P2 to old P1 direction
        var v2 = math.normalize(P1 - P2);
        // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
        P1 = P2 + v2 * L2;

        // Building ABC triangle, to find exact position of P1 after constraint
        // We use the temp position of P1 and the desired P0 to find the corner C
        var C = P1 + math.normalize(P0 - P1) * L3_short;
        var B = P2;
        var A = P2_Ext;
        P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        // Apply angle constraints to P1 in backward pass
        P1 = ApplyAngleConstraint(P0, P1, P2, minAngleP1, maxAngleP1);

        //-------------------------------
        // FORWARD PASS
        //-------------------------------
        v1 = math.normalize(P1 - P0);
        P1 = P0 + v1 * L1;
        var P1B = P0 + v1 * (L1 - L3_short);

        v2 = math.normalize(P2 - P1);
        P2 = P1 + v2 * L2;

        B = P1B;
        A = P1;
        C = P2;

        var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        var v3 = math.normalize(C - D);
        var NEXT_JOINT = D + v3 * (L3 + L3_short);

        // Apply angle constraints to P1 in forward pass
        P1 = ApplyAngleConstraint(P0, P1, P2, minAngleP1, maxAngleP1);

        P1_out = P1;
        P2_out = P2;
        E_out = NEXT_JOINT;
    }

    private static float2 FindMissingScissorPoint(float2 A, float2 B, float2 C)
    {
        // Implement the logic to find the missing point in the scissor mechanism
        // This will depend on the specific geometry and constraints of your mechanism
        return new float2();
    }

    private static float3 ApplyAngleConstraint(float3 P0, float3 P1, float3 P2, float minAngle, float maxAngle)
    {
        // Calculate vectors
        float3 v1 = math.normalize(P1 - P0);
        float3 v2 = math.normalize(P2 - P1);

        // Calculate the current angle between the vectors
        float dotProduct = math.dot(v1, v2);
        float currentAngle = math.degrees(math.acos(dotProduct));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the rotation required to get the constrained angle
        float angleDifference = constrainedAngle - currentAngle;
        float3 axis = math.cross(v1, v2);
        quaternion rotation = quaternion.AxisAngle(axis, math.radians(angleDifference));

        // Apply the rotation to P1 to get the constrained position
        float3 newP1 = P0 + math.rotate(rotation, v1) * math.distance(P0, P1);

        return newP1;
    }
}

Explanation of Changes

1. Angle Constraint in Both Passes:

   • The ApplyAngleConstraint method is now called after adjusting P1 during both the backward and forward passes to ensure the angle constraint is enforced throughout the entire process.

2. Consistency:

   • By applying the angle constraint in both passes, we ensure that P1 remains within the specified angle constraints relative to P0 and P2.

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FABRIKWithConstraints script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths, angle constraints, and tolerance based on your specific requirements.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths, angle constraints, and tolerance to get the desired behavior.

This updated script ensures that the angle constraints on joint P1 are applied consistently during both the backward and forward passes of the FABRIK algorithm. Adjustments may be needed based on the exact geometry and constraints of your robotic arm.

You

shouldn’t you calculate the new P1 differently, depenging on whether we are on the forward or backward pass?

ChatGPT

You are correct. The calculations for the new position of $ P1 $ should indeed differ depending on whether we are in the forward or backward pass, as the contexts of these passes are different. Let’s refine the approach by distinguishing between the backward and forward passes when applying the angle constraints.

Updated FABRIK Algorithm with Differentiated Angle Constraints

We will now differentiate between the two passes in the FABRIK_Pass_2 function and apply the angle constraints appropriately.

using Unity.Mathematics;
using UnityEngine;

public static class FABRIKWithConstraints
{
    public static void FABRIK_Pass_2(
        float3 P0, float3 P1, float3 P2, float3 E,
        float L1, float L2, float L3, float L3_short,
        float minAngleP1, float maxAngleP1, // Angle constraints for P1
        out float3 P1_out, out float3 P2_out, out float3 E_out)
    {
        //-------------------------------
        // BACKWARD PASS
        //-------------------------------
        // End Effector to P2 direction
        var v1 = math.normalize(P2 - E);

        // New P2 = End Effector + L3 Length
        P2 = E + v1 * L3;

        // Back Extension using L3_short
        var P2_Ext = P2 + v1 * L3_short;

        // New P2 to old P1 direction
        var v2 = math.normalize(P1 - P2);
        // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
        P1 = P2 + v2 * L2;

        // Building ABC triangle, to find exact position of P1 after constraint
        // We use the temp position of P1 and the desired P0 to find the corner C
        var C = P1 + math.normalize(P0 - P1) * L3_short;
        var B = P2;
        var A = P2_Ext;
        P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        // Apply angle constraints to P1 in backward pass
        P1 = ApplyAngleConstraintBackward(P0, P1, P2, minAngleP1, maxAngleP1);

        //-------------------------------
        // FORWARD PASS
        //-------------------------------
        v1 = math.normalize(P1 - P0);
        P1 = P0 + v1 * L1;
        var P1B = P0 + v1 * (L1 - L3_short);

        v2 = math.normalize(P2 - P1);
        P2 = P1 + v2 * L2;

        B = P1B;
        A = P1;
        C = P2;

        var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        var v3 = math.normalize(C - D);
        var NEXT_JOINT = D + v3 * (L3 + L3_short);

        // Apply angle constraints to P1 in forward pass
        P1 = ApplyAngleConstraintForward(P0, P1, P2, minAngleP1, maxAngleP1);

        P1_out = P1;
        P2_out = P2;
        E_out = NEXT_JOINT;
    }

    private static float2 FindMissingScissorPoint(float2 A, float2 B, float2 C)
    {
        // Implement the logic to find the missing point in the scissor mechanism
        // This will depend on the specific geometry and constraints of your mechanism
        return new float2();
    }

    private static float3 ApplyAngleConstraintBackward(float3 P0, float3 P1, float3 P2, float minAngle, float maxAngle)
    {
        // Calculate vectors
        float3 v1 = math.normalize(P1 - P0);
        float3 v2 = math.normalize(P2 - P1);

        // Calculate the current angle between the vectors
        float dotProduct = math.dot(v1, v2);
        float currentAngle = math.degrees(math.acos(dotProduct));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the new position for P1 after applying the angle constraint in the backward pass
        float angleDifference = constrainedAngle - currentAngle;
        float3 axis = math.cross(v1, v2);
        quaternion rotation = quaternion.AxisAngle(axis, math.radians(angleDifference));

        // Apply the rotation to v1 to get the constrained position of P1
        float3 newP1 = P2 - (math.rotate(rotation, v2) * math.distance(P1, P2));

        return newP1;
    }

    private static float3 ApplyAngleConstraintForward(float3 P0, float3 P1, float3 P2, float minAngle, float maxAngle)
    {
        // Calculate vectors
        float3 v1 = math.normalize(P1 - P0);
        float3 v2 = math.normalize(P2 - P1);

        // Calculate the current angle between the vectors
        float dotProduct = math.dot(v1, v2);
        float currentAngle = math.degrees(math.acos(dotProduct));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the new position for P1 after applying the angle constraint in the forward pass
        float angleDifference = constrainedAngle - currentAngle;
        float3 axis = math.cross(v1, v2);
        quaternion rotation = quaternion.AxisAngle(axis, math.radians(angleDifference));

        // Apply the rotation to v1 to get the constrained position of P1
        float3 newP1 = P0 + (math.rotate(rotation, v1) * math.distance(P0, P1));

        return newP1;
    }
}

Explanation of Changes

1. Separate Methods for Angle Constraints:

   • ApplyAngleConstraintBackward calculates the new position of P1 during the backward pass.
   • ApplyAngleConstraintForward calculates the new position of P1 during the forward pass.
   • Each method considers the context of the pass and applies the angle constraints accordingly.

2. Backward Pass:

   • The backward pass involves moving from the end effector back to the base, adjusting the positions to respect the constraints.
   • The angle constraint is applied after calculating the temporary position of P1 during the backward pass.

3. Forward Pass:

   • The forward pass involves moving from the base to the end effector, again adjusting the positions to respect the constraints.
   • The angle constraint is applied after calculating the temporary position of P1 during the forward pass.

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FABRIKWithConstraints script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths, angle constraints, and tolerance based on your specific requirements.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths, angle constraints, and tolerance to get the desired behavior.

This updated script ensures that the angle constraints on joint P1 are applied appropriately during both the backward and forward passes of the FABRIK algorithm. Adjustments may be needed based on the exact geometry and constraints of your robotic arm.

You

actually since P0 can’t go anywhere, let’s try to apply the angle on the forward pass, in order to constrain P2.

ChatGPT

I understand. We’ll focus on applying the angle constraint during the forward pass to constrain $ P2 $ relative to $ P1 $ and $ P0 $. Here’s how we can adjust the FABRIK_Pass_2 method to apply the angle constraint in the forward pass, specifically constraining $ P2 $ based on the angle formed at $ P1 $.

Updated FABRIK Algorithm with Angle Constraints in Forward Pass

using Unity.Mathematics;
using UnityEngine;

public static class FABRIKWithConstraints
{
    public static void FABRIK_Pass_2(
        float3 P0, float3 P1, float3 P2, float3 E,
        float L1, float L2, float L3, float L3_short,
        float minAngleP1, float maxAngleP1, // Angle constraints for P1
        out float3 P1_out, out float3 P2_out, out float3 E_out)
    {
        //-------------------------------
        // BACKWARD PASS
        //-------------------------------
        // End Effector to P2 direction
        var v1 = math.normalize(P2 - E);

        // New P2 = End Effector + L3 Length
        P2 = E + v1 * L3;

        // Back Extension using L3_short
        var P2_Ext = P2 + v1 * L3_short;

        // New P2 to old P1 direction
        var v2 = math.normalize(P1 - P2);
        // PRE-CONSTRAIN PASS New P1 = P2 + L2 Length.
        P1 = P2 + v2 * L2;

        // Building ABC triangle, to find exact position of P1 after constraint
        // We use the temp position of P1 and the desired P0 to find the corner C
        var C = P1 + math.normalize(P0 - P1) * L3_short;
        var B = P2;
        var A = P2_Ext;
        P1 = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        //-------------------------------
        // FORWARD PASS
        //-------------------------------
        v1 = math.normalize(P1 - P0);
        P1 = P0 + v1 * L1;
        var P1B = P0 + v1 * (L1 - L3_short);

        v2 = math.normalize(P2 - P1);
        P2 = P1 + v2 * L2;

        B = P1B;
        A = P1;
        C = P2;

        var D = new float3(FindMissingScissorPoint(A.xy, B.xy, C.xy), 0);

        var v3 = math.normalize(C - D);
        var NEXT_JOINT = D + v3 * (L3 + L3_short);

        // Apply angle constraints to P2 in forward pass
        P2 = ApplyAngleConstraint(P0, P1, P2, minAngleP1, maxAngleP1);

        P1_out = P1;
        P2_out = P2;
        E_out = NEXT_JOINT;
    }

    private static float2 FindMissingScissorPoint(float2 A, float2 B, float2 C)
    {
        // Implement the logic to find the missing point in the scissor mechanism
        // This will depend on the specific geometry and constraints of your mechanism
        return new float2();
    }

    private static float3 ApplyAngleConstraint(float3 P0, float3 P1, float3 P2, float minAngle, float maxAngle)
    {
        // Calculate vectors
        float3 v1 = math.normalize(P1 - P0);
        float3 v2 = math.normalize(P2 - P1);

        // Calculate the current angle between the vectors
        float dotProduct = math.dot(v1, v2);
        float currentAngle = math.degrees(math.acos(dotProduct));

        // Constrain the angle within the specified limits
        float constrainedAngle = math.clamp(currentAngle, minAngle, maxAngle);

        // Calculate the rotation required to get the constrained angle
        float angleDifference = constrainedAngle - currentAngle;
        float3 axis = math.cross(v1, v2);
        quaternion rotation = quaternion.AxisAngle(axis, math.radians(angleDifference));

        // Apply the rotation to v1 to get the constrained position of P2
        float3 newP2 = P1 + (math.rotate(rotation, v1) * math.distance(P1, P2));

        return newP2;
    }
}

Explanation of Changes

1. Forward Pass Angle Constraint:

   • During the forward pass, after computing the unconstrained position of $ P2 $, we apply the angle constraint to $ P2 $ based on the vectors $ P0 \to P1 $ and $ P1 \to P2 $.

2. Angle Constraint Calculation:

   • ApplyAngleConstraint method computes the angle between the vectors $ P0 \to P1 $ and $ P1 \to P2 $.
   • The method clamps the angle to the specified range and calculates the required rotation.
   • The position of $ P2 $ is adjusted based on the constrained angle.

Setting Up in Unity

1. Create GameObjects for Joints:

   • Create empty GameObjects for each joint (e.g., Joint01, Joint02, Joint03, Joint04).
   • Parent them appropriately to reflect the hierarchy shown in the image.
   • Position them in their initial locations as shown in the image.

2. Attach the Script:

   • Create an empty GameObject (e.g., IKController) and attach the FABRIKWithConstraints script to it.
   • Assign the target and joints in the inspector.
   • Set the segment lengths, angle constraints, and tolerance based on your specific requirements.

3. Test and Adjust:

   • Place a target GameObject in the scene and move it around to test the IK system.
   • Adjust the segment lengths, angle constraints, and tolerance to get the desired behavior.

This updated script ensures that the angle constraints on joint $ P1 $ are applied during the forward pass, constraining the position of $ P2 $. Adjustments may be needed based on the exact geometry and constraints of your robotic arm.