Adds the expressions for writing planners for a humanoid robot

[S-Dafarra]

This PR adds all the functions I needed to write planners for a humanoid robot. In particular:

• a set of variables from which it is possible to start when writing the optimal control problems
• a set of functions (that return casadi functions) to have prebuilt expressions useful when writing costs and constraints
• a generic terrain definition as done by @rob-mau
• interpolation utilities
• visualization utilities

[DanielePucci]

CC @ami-iit/artificial-mechanical-intelligence

[diegoferigo]

It looks really good @S-Dafarra! I have mainly comments on patterns plus some other minor ones. I skipped the visualization files, and went quickly on the terrains since probably @rob-mau had a better look at them.

The pattern using variable names seems working fine. However, it might be quite difficult to extend from the user point of view. Probably on the long term it would be helpful to have a sort of database (like a dict or a enum) containing all the names already used internally as casadi variables (and using them as default function parameters instead of using plain strings - quite helpful also for refactoring).

Nothing blocking though, approving regardless.

[S-Dafarra]

The pattern using variable names seems working fine. However, it might be quite difficult to extend from the user point of view. Probably on the long term it would be helpful to have a sort of database (like a dict or a enum) containing all the names already used internally as casadi variables (and using them as default function parameters instead of using plain strings - quite helpful also for refactoring).

Right now, I used a dictionary (will be added in the next PR, see

hippopt/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py

Lines 158 to 188 in 6a2f529

	function_inputs = {
	"mass_name": sym.mass.name(),
	"momentum_name": sym.system.centroidal_momentum.name(),
	"com_name": sym.system.com.name(),
	"quaternion_xyzw_name": "q",
	"gravity_name": sym.gravity.name(),
	"point_position_names": [],
	"point_force_names": [],
	"point_position_in_frame_name": "p_parent",
	"base_position_name": "pb",
	"base_quaternion_xyzw_name": "qb",
	"joint_positions_name": "s",
	"base_position_derivative_name": "pb_dot",
	"base_quaternion_xyzw_derivative_name": "qb_dot",
	"joint_velocities_name": "s_dot",
	"point_position_name": "p",
	"point_force_name": "f",
	"point_velocity_name": "v",
	"point_force_derivative_name": "f_dot",
	"point_position_control_name": "u_p",
	"height_multiplier_name": "kt",
	"dcc_gain_name": "k_bs",
	"dcc_epsilon_name": "eps",
	"static_friction_name": "mu_s",
	"desired_quaternion_xyzw_name": "qd",
	"first_point_name": "p_0",
	"second_point_name": "p_1",
	"desired_yaw_name": "yd",
	"desired_height_name": "hd",
	"options": self.settings.casadi_function_options,
	}

). I thought a bit about using a more structured way, but the functions might need different types of inputs and I did not manage to find a way to unify in a clean way.

In any case, the user can also avoid defining custom names. Nonetheless, when specifying a dynamics, the name of the variables used in the function is important. The integrators automatically call the dynamics' functions with the input variables matching the name used in the functions. This is why I needed to keep this possibility open. Let me know if it is not clear 😉

@diegoferigo I should also have dealt with your comments, if you are ok with it, I would merge 😉