Add add_cost method in the inverse kinematics

[Giulero]

This pull request enhances the inverse kinematics solver in src/adam/casadi/inverse_kinematics.py by introducing new features and improving code clarity. The key changes include the addition of a homogeneous transformation matrix for the base, a new method to retrieve optimization variables, and functionality to add custom cost terms to the optimization problem.

New features:

• Homogeneous transformation matrix:

  • Added self.base_homogeneous, which computes the homogeneous transformation matrix of the base using its position and quaternion. ([src/adam/casadi/inverse_kinematics.pyR64-R66](https://github.com/ami-iit/adam/pull/130/files#diff-92636529c4bff705bc52b6921d13c06319f84c74610c40fc35d376e6a589d8a7R64-R66))

• Retrieve optimization variables:

  • Introduced the get_opti_variables method, which returns a dictionary of all optimization variables, including the new homogeneous transformation matrix. ([src/adam/casadi/inverse_kinematics.pyR88-R106](https://github.com/ami-iit/adam/pull/130/files#diff-92636529c4bff705bc52b6921d13c06319f84c74610c40fc35d376e6a589d8a7R88-R106))

• Custom cost terms:

  • Added the add_cost method to allow users to add custom cost terms to the optimization problem, enhancing flexibility. ([src/adam/casadi/inverse_kinematics.pyR581-R589](https://github.com/ami-iit/adam/pull/130/files#diff-92636529c4bff705bc52b6921d13c06319f84c74610c40fc35d376e6a589d8a7R581-R589))

---

📚 Documentation preview 📚: https://adam-docs--130.org.readthedocs.build/en/130/

[Copilot]

Pull Request Overview

This PR adds extensibility features to the inverse kinematics solver by introducing methods to retrieve optimization variables and add custom cost terms. The changes also include a precomputed homogeneous transformation matrix for the base.

• Addition of get_opti_variables() method to expose optimization variables to users
• Implementation of add_cost() method for adding custom cost terms to the optimization problem
• Precomputation of base homogeneous transformation matrix from position and quaternion

[Copilot]

There is an extra backtick at the end of the docstring line. It should end with a period instead.

Suggested change

	- "base_homogeneous": Homogeneous transformation matrix of the base (4x4 matrix, from position and quaternion).`
	- "base_homogeneous": Homogeneous transformation matrix of the base (4x4 matrix, from position and quaternion).

[Copilot]

The add_cost method lacks documentation explaining how the cost term will be used in the optimization (e.g., whether it's added to existing costs or how it affects the objective function).

Suggested change

	This method appends the provided cost term to the `cost_terms` list.
	During the `_finalize_problem` step, all cost terms in this list are
	combined using `cs.sum(cs.vertcat(*self.cost_terms))` and minimized
	as part of the objective function.
	Note: This method ensures that the optimization graph is still modifiable
	before adding the cost term. Once the problem is finalized (after the
	first call to `solve()`), no new cost terms can be added.

[traversaro]

I do not remember how fixed joints are handled in adam, but I am not sure if this sentence is correct.

[traversaro]

Also, I am not sure about the "joint variables" name. Perhaps "joint positions" is more clear?