Extracting Angles

[supertick]

Hello again !
I was wondering if there is a way to easily extract angles between the bones ?
I've been playing around with Caliko for a little bit, and can see how to iterate through the bones ..

    for (FabrikBone3D bone : chains.get("default").getChain()) {
      bone.getStartLocation().getGlobalPitchDegs();
      bone.getStartLocation().getGlobalYawDegs();
      Vec3f.getDirectionUV(bone.getStartLocation(), bone.getEndLocation());

      System.out.println("Bone X: " + bone.getStartLocation().toString());
    }

but didn't find any methods for computed angles.
Would I need to get the start and end vector of each bone then compute it again ?
Digging deeper I saw this for local hinges, and tried adding the calculation for the local angle

					case LOCAL_HINGE:
						// Local hinges get constrained to the hinge rotation axis, but not the reference axis within the hinge plane
						
						// Construct a rotation matrix based on the previous bones inner-to-to-inner direction...
						Mat3f m = Mat3f.createRotationMatrix( mChain.get(loop-1).getDirectionUV() );
						
						// ...and transform the hinge rotation axis into the previous bones frame of reference.
						Vec3f relativeHingeRotationAxis = m.times( thisBoneJoint.getHingeRotationAxis() ).normalise();
											
						// Project this bone's outer-to-inner direction onto the plane described by the relative hinge rotation axis
						// Note: The returned vector is normalised.					
						thisBoneOuterToInnerUV = thisBoneOuterToInnerUV.projectOntoPlane(relativeHingeRotationAxis);
						
						
 					        // Calculate the dot product between the two vectors
						float dotProduct = Vec3f.dotProduct(thisBoneOuterToInnerUV, relativeHingeRotationAxis);

						// Use the dot product to calculate the angle (in radians) between the vectors
						float localHingeAngle = (float)Math.acos(dotProduct);

						// If you want the angle in degrees, you can convert it
						float localHingeAngleDegrees = (float)Math.toDegrees(localHingeAngle);
						
						System.out.println(String.format("angle: %f", localHingeAngleDegrees));
						
						break;

Heh, it gave me nothing but 90 degrees 🙄

angle: 90.000008
angle: 89.999992
angle: 90.000000
angle: 90.000000
angle: 90.000000
angle: 90.000000
angle: 90.000008
angle: 89.999992
angle: 90.000000
angle: 90.000000
angle: 90.000000
angle: 90.000000
angle: 90.000008
Bone X: x: 0.000, y: 10.000, z: 0.000
Bone X: x: -14.142, y: 10.000, z: 0.000
Bone X: x: -19.066, y: 9.132, z: 0.000
Bone X: x: -15.811, y: -9.328, z: 6.974

The big picture is, I would like an IK solver to solve servo positions for a robot project called InMoov.


I've roughed out the right arm.
To begin with I'd like to control 4 servos at 4 joints, but I need the local angles.
The joints in order from top to bottom are:

• omoplate
• shoulder
• rotate
• bicep
  All of them are local hinges, except for "rotate" which rotates the upper arm in the shoulder socket. For that one, I create a freely rotating hinge bone.

chain.addConsecutiveFreelyRotatingHingedBone(getAxis(directionUV), length, JointType.LOCAL_HINGE, getAxis(hingeRotationAxis), getColor(color));

Appreciate any help.
Regards,
GroG

[alansley]

So if I've understood you right, you're looking to find the euler angles (i.e., pitch/yaw/roll) of each bone with respect to the previous bone in the chain, yeah? This should be doable.

Roll does not work well in Caliko unfortunately, for a variety of reasons, but that's something we can come back to later if needed - however the pitch (x-axis rotation) and yaw (y-axis rotation) should make for a good start. Also, roll can be addressed separately if required (i.e., handled outside of Caliko).

I'll look into it and get back to you on the weekend.

[supertick]

Yes, precisely.
One "roll" occurs in a joint/servo called rotate, but all the rest are pitch/yaw in relation to the previous bone.

• (1) omoplate
• (2) shoulder
• (3) bicep
• (4) rotate (the roll one)
  That would be great. Thanks for your help :)

[alansley]

Try calling this from the draw method & let me know if it's what you were after:

	private void printAngles(FabrikStructure3D structure)
	{
		var chain = structure.getChain(0); // Grab the first chain in the structure	
		var bones = chain.getChain();      // Get the list of bones in the chain (terrible method name - it should have been `getBoneList` or similar!)
		var numBones = bones.size();       // Find out how many bones exist in the chain
		
		System.out.println("----- Global Pitch / Yaw -----");
		
		// Loop over all the bones..
		for (int i = 0; i < numBones; ++i)
		{
			// ..getting & printing the global pitch and yaw of each bone.
			var bone = bones.get(i);
			var globalPitchDegs = bone.getGlobalPitchDegs(); // Pitch is around the X-axis
			var globalYawDegs   = bone.getGlobalYawDegs();   // Yaw is around the Y-axis
			
			System.out.println("Bone " + i + ": Global pitch is: " + globalPitchDegs + " degs, Global yaw is: " + globalYawDegs);			
		}		
		
		System.out.println("----- Relative Pitch / Yaw -----");
		
		// Loop over all the bones..
		float previousGlobalPitchDegs = 0f;
		float previousGlobalYawDegs = 0f;
		for (int i = 0; i < numBones; ++i)
		{
			// If it's the very first bone in the chain then just grab & store the details..
			if (i == 0)
			{				
				var bone = bones.get(i);
				previousGlobalPitchDegs = bone.getGlobalPitchDegs();
				previousGlobalYawDegs   = bone.getGlobalYawDegs();
				
				// ..then skip to the next iteration.
				continue;
			}
			
			// Otherwise grab the current bone & it's global pitch/yaw
			var bone = bones.get(i);
			var currentGlobalPitchDegs = bone.getGlobalPitchDegs();
			var currentGlobalYawDegs   = bone.getGlobalYawDegs();
		
			// Get the relative pitch/yaw (Note: Swap the order of subtraction if required, that is, if you're working in the opposite direction)
			var relativePitchDegs = currentGlobalPitchDegs - previousGlobalPitchDegs;
			var relativeYawDegs   = currentGlobalYawDegs   - previousGlobalYawDegs;
			
			System.out.println("Bone " + i + " relative to bone " + (i - 1) + " - Pitch: " + relativePitchDegs + " degs, Yaw: " + relativeYawDegs + " degs.");
			
			// Update the "previous" pitch/yaw for the next iteration (so the next bone we look at is compared relative to THIS bone)
			previousGlobalPitchDegs = currentGlobalPitchDegs;
			previousGlobalYawDegs = currentGlobalYawDegs;					
		}		
	}

[supertick]

Great, thank you Al.
I'll try and report back.