Created utility function for adding many priors simultaneously

[scharalambous3]

Created a utility function in initialize_solution_utils that accepts multiple keys and state terms to resolve copy/pasta in the examples

Checked the generated trajectories of the affected examples with csvdiff ✓
Unit test ✓

Cc: @dellaert (resolves review comment in #100) @yetongumich @gchenfc

[dellaert]

Several comments which you can generalize to the entire PR:

1. this won't work as you're not adding but replacing the graph
2. It won't generalize to cases where we have > 2 joints
3. I propose renaming to AddSoftKinematicsConstraint
4. I propose to do the key business inside the function so the four lines become:

AddSoftKinematicsConstraint(&graph, dynamics_model, {X_i[0], X_i[1]}, j0_id); // angle and vel for joint 0
AddSoftKinematicsConstraint(&graph, dynamics_model. {X_i[2], X_i[3]}, j1_id); // angle and vel for joint 1

Note t should the last argument and by default 0, as in values.h

[gchenfc]

Is the word "Soft" appropriate since the caller can pass in a Constrained noise model if they choose?

[scharalambous3]

Thanks Frank for the review. I will make changes accordingly. Please correct me if I'm wrong but I will have to add the key function as an input parameter, too
Gerry , my understanding is that "Soft" is appropriate because the conditions are only enforced as soft constraints

[dellaert]

No, the keys are constructed inside from the joint ID and the time "t".

[dellaert]

@gchenfc you are right. But what word to use if not "SoftConstraint" ? Constraint implies hard. Factor is too generic.

[dellaert]

Then again, we do talk about soft and hard constraints so maybe the best naming, unless something better comes along, is just omitting the "Soft"

[scharalambous3]

I'd hold off working on this until the spider-walking branch is merged because it may not be necessary

[scharalambous3]

Closing this because ObjectiveFactors.h in #148 fulfilled this requirement!