Inverse Kinematics - Improvements

[danic85]

Congratulations @KeahiXie, you discovered my secret! The IK algorithm in the arduino sketch is not my finest hour, and here's why:

• It is based of an example sketch that I 'adapted' in to fit the project.
• It isn't full IK, and actually just adjusts the leg height based on the input. This was really all I needed since it can't take a step anyway.
• It's very slow to calculate and (I believe) is not capable of calculating the full range of motion that the legs actually have
• I pulled it together in an afternoon about 18 months ago (check out the full commit) and haven't been back to it since.

In truth it hasn't been a priority (yet), but I have been hoping that someone with a good understanding of IK would like the challenge of improving it and contributing some much needed changes to the project.

Do you know anyone who fits that description? (hint hint 😉 )

I'm more than happy (excited, even) to collaborate on it, if that's something you'd be up for.

[EngineerJoe2016]

Yes, absolutely. I mentioned this on Discord as well, but I would be excited to help with KI. I have plans to add feedback and PI control as well!

[KeahiXie]

Okay, got it! I've been working on learning IK and getting a handle on how all the files work together for Arduino. The process has been a bit slow, but I'm doing my best. Once I fully understand IK, I'll start making some improvements. Anyway, thanks again for all the effort you've put into it 🙂

[danic85]

Thanks both. It would be good to collaborate on this so please keep posting your learnings, any changes you're working on, and any questions you have.

GitHub allows you to 'fork' the repository, make changes and commit them, then create a pull request (PR) back into this main repo. It means we can collaborate and review changes easily. Please feel free to make use of it for this discussion - you can even create 'draft' PRs if you just want to show the work to us rather than requesting a merge.

@EngineerJoe2016 you mentioned using the algebraic method. I'm not familiar with that - can you share any resources?

[SentryCoderDev]

I created a roadmap with ideas about changes we could make:

Issues with the Current IK Algorithm:

Simplified IK: The code uses a simplified approach based on leg height rather than a complete inverse kinematics solution. This prevents the robot from stepping or performing complex movements.

Slow Computation: TheinverseKinematics2D function may have high computational costs, which can slow down the robot’s movements.

Limited Range of Motion: The algorithm cannot use the full range of motion of the legs, which restricts the robot's potential movements.

Suggestions for Improvement:

Switch to Full IK Algorithm: Instead of the simplified approach, consider using a more advanced and efficient IK algorithm, such as FABRIK (Forward And Backward Reaching Inverse Kinematics). FABRIK is computationally efficient and can utilize the full range of leg movement. It also yields reasonable results even when the target is out of reach.

Increase Computation Speed: To optimize IK calculations, use lookup tables or precomputed values. Additionally, consider faster approximation methods in place of trigonometric functions. Reducing computational cost is essential due to the limited processing power of Arduino.

Motion Planning: To make the robot's movements smoother and more realistic, a motion planning algorithm can be added. This would allow the robot to plan its steps in advance and perform movements more coherently.

PI Control: As suggested by @KeahiXie, adding PI control can enhance the precision and stability of the robot's movements, which is particularly important for robustness against external factors.

Additional Notes on the Code:

• ServoEasing Library: This library is used to smooth servo movements, making the robot's motion more fluid, though it may add unnecessary complexity. If there are performance issues, consider reviewing its use.

• PiConnect Class: Handles communication with the Raspberry Pi. Optimizing this communication protocol could reduce latency.

• Use of #ifdef Directives: Certain features are disabled with#ifdef directives, which can reduce readability. Using a configuration file instead may be a better approach.

To proceed, start by working on implementing a full IK algorithm like FABRIK. This will significantly improve the robot's movements. Then, tackle other optimizations like performance improvements and motion planning.

InverseKinematics.h

2D IK: The inverseKinematics2D function calculates inverse kinematics only in a 2D plane, meaning the robot can only move forward and backward. A 3D IK solution is needed for side-to-side movement or turning.

Angle Limits: The anglesWithinLimits function checks the angle limits of servo motors, but these checks occur only within the inverseKinematics2D function. Ensuring limit checks in other motion functions can help prevent servo motor damage.

calculateOtherLeg Function: This function uses a simple symmetry assumption to calculate the angles of the other leg, which may not always be accurate. A more accurate approach is to perform separate IK calculations for each leg.

Trigonometric Functions: Functions like acos can be computationally expensive. If there are performance issues, consider faster approximation methods or using atan instead of atan2 if acceptable, though this may reduce accuracy.

Constants: Constants like HIP_ADJUSTMENT, KNEE_ADJUSTMENT, and ANKLE_ADJUSTMENT are defined directly in the code. Moving these values to Config.h could improve readability and editability.

Config.h:

Servo Calibration: The SERVO_CALIBRATION_ENABLED macro enables servo calibration, which can adjust servo motor zero points. Simplifying this calibration process could improve user experience, as the current approach might require technical knowledge.

Position Arrays: Arrays like PosMin, PosMax, PosRest, and PosStand are defined as fixed values. Storing these in EEPROM would allow the robot to remember its positions even after power loss.

NOVAL Macro: TheNOVAL macro is used to indicate that the servo motor should not move, but the value used (1000) is out of the servo motor’s angle range. Using a more meaningful value, like -1, could improve readability.

Mpu6050.h

MPU6050 Initialization: The MPU6050 sensor initializes within the doInit function. If initialization fails, an error message is printed, but the program continues, which could cause unexpected behavior. Halting the program or implementing an alternative process upon failure would be safer.

General Suggestions:

Modularity: Breaking down functions into smaller, specialized parts would enhance code readability, maintenance, and reusability.

Comments: There are few comments in the code, and existing ones lack detail. Adding more descriptive comments explaining different sections of the code will make it easier to understand.

Implementing these improvements will enhance code quality and performance. Specifically, applying a full 3D IK solution will greatly improve the robot’s movement capabilities.

Roadmap for Implementation:

1. Implementing the FABRIK Algorithm:

• Find libraries or code examples of FABRIK, especially in C++, to explore implementations.

• Replace the current inverseKinematics2D function with a new function that uses FABRIK, taking advantage of FABRIK’s suitability for 3D movements to enhance the robot’s capabilities.

• Set leg segment lengths and joint limits required for the FABRIK algorithm accurately. These values can be defined in Config.h.

2. Performance Optimization:

• Trigonometric Functions: Use faster approximation methods or lookup tables instead of functions like acos, sin, and cos where possible.

• ServoEasing Library: Assess the performance of the ServoEasing library and, if necessary, consider a lighter alternative or direct servo control.

• Computation Load: Simplify and optimize IK calculations. Avoid unnecessary computations and optimize loops.

3. Code Enhancements:

• 3D Movement: Support 3D movements with the FABRIK algorithm, enabling the robot to perform more complex and realistic actions.

• Angle Limits: Check servo motor angle limits in all motion functions to protect the servos.

• calculateOtherLeg Function: Improve accuracy by performing separate IK calculations for each leg.

• Constants: Move constants like HIP_ADJUSTMENT, KNEE_ADJUSTMENT, and ANKLE_ADJUSTMENT to Config.h.

• Store Position Values in EEPROM: Storing position values in EEPROM will allow the robot to retain memory of different positions even after power interruptions.

• NOVAL Macro: Use a more meaningful value (e.g., -1) for the NOVAL macro.

• MPU6050 Initialization: Make the MPU6050 sensor initialization more robust by halting the program or implementing an alternative if initialization fails.

• Modularity: Make the code more modular by breaking down functions into smaller parts.

• Comments: Add more detailed comments explaining the functionality of different code sections.

[EngineerJoe2016]

@SentryCoderDev thanks for the details, it's good but I'm a little confused on the direction you defined. I do think that we can follow Dan's suggestion and create forks for this portion of the code and work to demonstrate it to the community.

@danic85, at least from my time studying robotics you have many different ways to calculate the end-effector positions. But in terms of math, normally it comes down to either the geometric method or the algebraic method. Now I guess there is a third method(ish) as well, FABRIK.

The geometric method is what a lot of people tend to use that are new to robotics, geometry is good for small-scale robot arms, but when you increase joint counts and complex dimensions, it can get nasty real fast.

The algebraic method uses linear algebra, specifically matrices and transforms as it translates well to 3D driven calculations. With this method, you can feed the end-effector coordinates into the matrix and it will back calculate your joint positions as needed. Plus you can build in limitations and specific paths for your robot to take; as I said above you can link multiple KI equations together as well.

I have some links below to help explain it a little more. I do plan on providing write ups for the equations I create.

Look at section 3.2.1 vs 3.2.2
Link: https://opentextbooks.clemson.edu/wangrobotics/chapter/inverse-kinematics/#:~:text=The%20algebraic%20approach%20to%20solving,writing%20down%20the%20DH%20table.

wiki: https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant

For the community, I am not trying to push one method over another. I just like linear algebra, as it's what I was taught and I want to see what I can do.

As a Note: EngineerJoe2016 <-> CriminialJoe (Discord)
I use one for my professional account and the other I use to play games. Just want to clear that up for anyone that travels between here and Discord.

[SentryCoderDev]

@EngineerJoe2016 , thanks for the feedback! Glad the direction is mostly clear, though I understand there might be some ambiguity with the approach I outlined. Following Dan's suggestion to create forks and demonstrate this part of the code for the community sounds like a solid plan.

As for the inverse kinematics (IK) methods you mentioned—I'm with you on the benefits of linear algebra for complex setups. The geometric method can indeed be helpful for simpler systems but often falls short with increased joint counts or complexity. The algebraic approach is robust for 3D systems, as you noted, thanks to matrix-based calculations. FABRIK is also an interesting alternative, especially for specific applications where iterative solutions are preferable.

I’ll check out sections 3.2.1 and 3.2.2 and start mapping some examples based on your links. And no worries on the account names—got it noted!"

Let me know if there’s anything specific you’d like to dive into!

[danic85]

Thanks @EngineerJoe2016 ! I'll take a look at those resources. Feel free to share your write ups etc here when you have it - and let me know if you need any support.