Expose position of all the model points for use in marker tracking/comparison

[moorepants]

We can start with providing the positions of every point that is currently in the model: mass centers, joint centers, and feet bottoms.

If we want to have a point that represents specific marker locations, for example Ton's set that I used in https://peerj.com/articles/918/ then we can do that too.

[moorepants]

From:

eqs = derive_equations_of_motion()
trunk, rthigh, rshank, rfoot, lthigh, lshank, lfoot = eqs.segments

The points on the body segments are then:

rshank.joint,  # right ankle
rfoot.toe,
rfoot.heel,
rthigh.joint,  # right knee
trunk.joint,  # hip
trunk.mass_center,
lthigh.joint,  # left knee
lshank.joint,  # left ankle
lfoot.heel,
lfoot.toe,

So we need to make equations such as these available to provide to opty:

right_ankle_x, right_ankle_y = dynamicsymbols('ank_rx, ank_ry', real=True)
pos_eq0 = right_angle_x - rshank.joint.pos_from(eqs.origin).dot(eqs.inertial_frame.x)
pos_eq1 = right_angle_y - rshank.joint.pos_from(eqs.origin).dot(eqs.inertial_frame.y)

Then these can be added to the equations of motion:

eom = eqs.equations_of_motion
eom = eom.col_join(sympy.Matrix([pos_eq0, pos_eq1])
specifieds = eqs.specifieds
specifieds += [right_ankle_x, right_ankle_y]

p = Problem(eom, ...)

The new variables for the position measure numbers will then become unknown input trajectories for opty and available in the optimization vector for use in the objective function.

I don't have to add this as an output to gait2d (the animation uses these equations internally but the user never needs to deal with them). Or we can just add this to the gait-closed-loop-id code, which is probably the simplest first step.