Add parameters to Models allowing robot joints to be parameterized

[joaomoura24]

Changes for adding option of optimizing a selection of the joints by specifying the joints that are parameterized when getting the robot model.

I tested all the changes for Inverse Kinematics, Inverse Differential Kinematics, and configuration planning.
I copied the figure_eight_plan.py file and made the necessary changes to keep one of the joints fixed, so the optimizer only optimizes 6 joints, here: link

The naming of functions might not be ideal, so please provide changes to the naming or the way it is done, as long as it doesn't break the goal functionality.

But also bare in mind that this functionality will not be used a lot by all the users, so it's ok to be a bit more cumbersome.
The added changes do not break the normal working of optas for the typical case that you optimize all of the joints.

[joaomoura24]

Actually, lets hold the merge until beginning of January, cause just before leaving I found a bug which doesn't seem to come from my latest changes (it looks like it's from a new model I am using), but still I want to check.

[joaomoura24]

@cmower, I rebased twice this branch.
It is now working, though i see that you made lots of changes in the functions naming.
Please, lets finalize the merge of this changes before making other changes to the master branch, or discuss a better way to achieve the same functionality.
I can meet you tomorrow to discuss this if needed.

[cmower]

This is moving in the right direction. However, I am not keen how the user is required to piece together the model optimized variables and parameters - the information on the ordering in the model array is all contained within the RobotModel class so this merging should be automatic.

This functionality should be abstracted to the Model class so TaskModel can take advantage of it too. I mention this in the comments that this we can leave as functionality only implemented, for now, in the RobotModel class. Please could you create an issue so we can implement this abstraction later.

Please see comments for specific changes needed.

[cmower]

Looking good, just some fairly minor points. I was thinking, could you double check the helper methods in the OptimizationBuilder class. I think some of these methods might break given this feature. E.g. enforce_model_limits may need to have the split/extract method called inside to make sure constraints are only enforced on optimized joints.

[cmower]

Some more changes please.

[joaomoura24]

Don't merge this yet.
I am fixing the examples to run with the new 'q/x'.

[joaomoura24]

I updated the examples and ran them.
Note that i got errors in the examples to do with the issues #77 and #78.
I ran those two examples in the master branch as well just to check if the problem was still there.

[joaomoura24]

I think this is ready to merge.
It still has the Changes requested on for some reason.
Could you take a look at it and see if you can merge?

[cmower]

Thanks @joaomoura24!