Bug in this kind of solution?

[Dimoo-POP]

Hello, maybe I found the bug of the FABRIK algorithm or your code, so I want to discuss this bug with you and get your advice if you want.
Based on Demo 7, I built a chain as shown below. The start and end positions of each bone are: [0, 0, 0] [0, 0, 40]; [0, 0, 40], [0, 0, 80]; [0, 0, 80], [0, 0, 120]. The target that I want to solve is [0, 0, 100]. I have tried to change the joint limit many times, but all the results were all problematic. This chain is always along the z-axis, and the end of the last bone is not placed at the target position. The ideal configuration of this chain should be like this:

------1-----End point (target)
------1-------
------1-------
------1-----third joint (local hinge)
--------1-----
----------1---
------------1- second joint (local hinge)
----------1---
--------1-----
-------1------
------1---base joint (ball)

---

Did I make a mistake? Looking forward to your reply！

Code:

	public class LocalHingesWithReferenceAxisConstraints extends CalikoDemoStructure3D {
		@Override
		public void setup() {
			this.structure = new FabrikStructure3D("Demo 7 - Local Hinges with Reference Axis Constraints");
			int numChains = 1; // 3
			
			Vec3f hingeRotationAxis  = new Vec3f();
			Vec3f hingeReferenceAxis = new Vec3f();
			Vec3f baseBoneConstraintAxis = new Vec3f(); 
			float baseBoneConstraintAngleDegs = 90.0f; 
			baseBoneConstraintAxis = X_AXIS;  
			
			float rotStep = 360.0f / (float)numChains;
			for (int loop = 0; loop < numChains; loop++)
			{	
				// Set colour and axes							
				Colour4f chainColour = new Colour4f();
				switch (loop % 3)
				{
					case 0:
						chainColour        = Utils.RED;
						hingeRotationAxis  = new Vec3f(X_AXIS);
						hingeReferenceAxis = new Vec3f(Y_AXIS);
						
						break;
					case 1:
						chainColour        = Utils.GREEN;
						hingeRotationAxis  = new Vec3f(Y_AXIS);
						hingeReferenceAxis = new Vec3f(X_AXIS);
						break;
					case 2:
						chainColour        = Utils.BLUE;
						hingeRotationAxis  = new Vec3f(Z_AXIS);
						hingeReferenceAxis = new Vec3f(Y_AXIS);
						break;
				}
				
				FabrikChain3D chain = new FabrikChain3D();
				
				Vec3f startLoc = new Vec3f(0.0f, 0.0f, 0.0f); 
				startLoc       = Vec3f.rotateYDegs(startLoc, rotStep * (float)loop);
				Vec3f endLoc   = startLoc.plus( defaultBoneDirection.times(base_BoneLength) );  // base_BoneLength = 40 

				FabrikBone3D basebone = new FabrikBone3D(startLoc, endLoc);
				basebone.setColour(chainColour);
				chain.addBone(basebone);
				chain.setRotorBaseboneConstraint(FabrikChain3D.BaseboneConstraintType3D.GLOBAL_ROTOR, baseBoneConstraintAxis, baseBoneConstraintAngleDegs); 
				
				float constraintAngleDegs_1 = 120.0f; 
				chain.addConsecutiveHingedBone(defaultBoneDirection, (float) 40, JointType.LOCAL_HINGE, hingeRotationAxis, constraintAngleDegs_1, constraintAngleDegs_1, hingeReferenceAxis, Utils.GREY);
				chain.addConsecutiveHingedBone(defaultBoneDirection, (float) 40, JointType.LOCAL_HINGE, hingeRotationAxis, constraintAngleDegs_1, constraintAngleDegs_1, hingeReferenceAxis, Utils.GREY);

				this.structure.addChain(chain);	
			}
		}

I think you found a bug!

Here's some cleaned-up "Demo 7" replacement code that demonstrates the bug so you can see the issue:

	@Override
	public void setup()
	{
		this.structure = new FabrikStructure3D("Dimoo-POP Test");
		
		Vec3f baseBoneConstraintAxis = Z_AXIS.negated(); // The basebone constraint axis is pointing directing INTO the screen		
		float baseBoneConstraintAngleDegs = 45.0f;       // Changes basebone rotor angle to 45 degress (from 90) for testing purposes
		float boneLength = 40f;                          // Our default bone length will be 40 units			
		FabrikChain3D chain = new FabrikChain3D();       // Create a new chain
			
		// ----- Basebone stuff -----
		
		// Specify start and end locations of the basebone then construct it
		Vec3f startLoc = new Vec3f(0.0f, 0.0f, 100f); // NOTE: I've moved the bone backwards a bit, i.e. along the positive Z-axis
		Vec3f endLoc   = startLoc.plus( defaultBoneDirection.times(40.0f) ); // Default bone direction is along the -Z axis (i.e. INTO the screen)
		
		// Create the basebone, set its colour, and add it to the chain 
		FabrikBone3D basebone = new FabrikBone3D(startLoc, endLoc);		
		basebone.setColour(Utils.YELLOW); 
		chain.addBone(basebone);
		
		// Set a rotor / ball constraint on the given (-Z) axis with an angle limit of 45 degrees
		chain.setRotorBaseboneConstraint(FabrikChain3D.BaseboneConstraintType3D.GLOBAL_ROTOR, baseBoneConstraintAxis, baseBoneConstraintAngleDegs); 
		
		// ----- Additional bone stuff -----
				
		float hingeConstraintAngleDegs = 120.0f;                   // The hinge can rotate +/- this angle
		
		// Add two consecutive hinged bones
		// GLITCH-OUT!: Local hinges go CRAZY when they're over constrained! They swap between going "this way round" and "that way round" =/
		/*
		for (int i = 0; i < 2; ++i)
		{
			chain.addConsecutiveHingedBone(defaultBoneDirection,         // Direction of the bone
					                       boneLength,                   // Length of the bone
					                       JointType.LOCAL_HINGE,        // Joint type of the bone
					                       hingeRotationAxis,            // Rotation axis of the bone
					                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle clockwise,
					                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle anti-clockwise,
					                       hingeConstraintReferenceAxis, // Hinge constraint reference axis
					                       Utils.GREY);
		}
		*/
		
		// Global hinge version (one X-axis hinge followed by one Y-axis hinge)
		// Bone 1 - rotates up and down in relation to negative Z-axis
		chain.addConsecutiveHingedBone(defaultBoneDirection,         // Direction of the bone
				                       boneLength,                   // Length of the bone
				                       JointType.GLOBAL_HINGE,       // Joint type of the bone
				                       X_AXIS           ,            // Rotation axis of the bone
				                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle clockwise,
				                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle anti-clockwise,
				                       Z_AXIS.negated(),             // Hinge constraint reference axis
				                       Utils.GREY);                  // Bone colour

		/***
		 * Here's how to demonstrate your bug:
		 * 
		 * Step 1 - Look at the demo as it is in this code.
		 * Step 2 - Change the bone 2 (below) joint type from GLOBAL_HINGE to LOCAL_HINGE
		 * Step 3 - Boom! The hinge constraint reference axis is now pointing the wrong way!
		 *          i.e. with a LOCAL hinge the reference axis is now pointing at us (+Z) instead of into the screen / away from us (-Z).
		 * 
		 *  Nice find! 
		 ***/  
		
		// Bone 2 - rotates left and right in relation to bone 1 or global Y-axis
		chain.addConsecutiveHingedBone(defaultBoneDirection,         // Direction of the bone
				                       boneLength,                   // Length of the bone
				                       JointType.GLOBAL_HINGE,       // Joint type of the bone
				                       Y_AXIS,                       // Rotation axis of the bone
				                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle clockwise,
				                       hingeConstraintAngleDegs,     // Hinge can rotate up to this angle anti-clockwise,
				                       Z_AXIS.negated(),             // Hinge constraint reference axis
				                       Utils.GREY);                  // Bone colour
		

		// Add the completed chain to the structure ready for solving
		this.structure.addChain(chain);	

I'll need to go through the code and find the right place to NOT negate the local hinge axis - bit busy atm so a cheeky workaround would just be to specify the opposite of the hinge constraint reference axis when using local hinges for the time being.

Thanks for taking the time to let me know & hopefully I can get it all cleaned up soon.

Cheers!

[Dimoo-POP]

Thank you for your detailed reply!

Is there no End Effector orientation constrain in your code?