Add function to generate x,y coordinates of various points.

[moorepants]

TODO:

• Add new equations to equations of motion.
• Decide on what points should be included for tracking.
• Create initial guess for marker trajectories (will probably converge faster if not zero).
• Generate marker data to track.
• Add marker tracking to objective.
• Consider only calculating the marker trajectories in the objective function.

[moorepants]

@tvdbogert this runs an generates the marker trajectories as unknown trajectory inputs. Did you have an idea on generating the marker data to track? Or should I build out a data set from some measurement data?

[moorepants]

I have basic ankle tracking running but I'm getting this error:

 785  3.0041439e+02 3.69e-08 1.77e-01  -6.9 6.93e-01    -  5.56e-01 2.91e-14h 40
Cannot call restoration phase at point that is almost feasible (violation 3.685287e-08).
Abort in line search due to no other fall back.

Number of Iterations....: 785

                                   (scaled)                 (unscaled)
Objective...............:   3.0041438539933364e+02    3.0041438539933364e+02
Dual infeasibility......:   1.7653921335754885e-01    1.7653921335754885e-01
Constraint violation....:   3.6852866647230087e-08    3.6852866647230087e-08
Variable bound violation:   9.9999981774773241e-09    9.9999981774773241e-09
Complementarity.........:   2.0935469082948100e-04    2.0935469082948100e-04
Overall NLP error.......:   1.7653921335754885e-01    1.7653921335754885e-01


Number of objective function evaluations             = 3085
Number of objective gradient evaluations             = 759
Number of equality constraint evaluations            = 3085
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 793
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 0
Total seconds in IPOPT                               = 31.967

EXIT: Error in step computation!
IPOPT was not successful.
   obj: 300.414 = 1.047(torque) + 20.647(track)

[moorepants]

With some adjustments I still get the error in computation on some solves but did manage to get an optimal solution found on the final motion.

solution-ankle-track-50nodes-1.2mps.mp4

The walker is leaned back, so I wonder if the x=0 position of the mocap system needs to be addressed.

[moorepants]

For the above this is the ankle tracking:

So, not quite right.

[moorepants]

Weighting ankle more (got error in step calc on final solution but many optimal solutions in the process):

solution-ankle-track-50nodes-1.2mps-more-ankle-track-weight.mp4

Not much different that prior solution.

[moorepants]

If I shift the by -5 cm in both the x and y, I get this:

The joint angles fit a bit better too.

So maybe this has to do with scaling the model to the dimensions of the measured person.

[moorepants]

With the -0.05 m offset in the data and adding periodicity instance constraints to the ankle positions, i get faster no error solutions:

solution-ankle-track-50nodes-1.2mps-with-periodicity.mp4

[moorepants]

From gait cycle 45:

solution-ankle-track-50nodes-1.2mps.mp4

[moorepants]

Check that ground reaction force considers belt speed in the animation plots.

[moorepants]

If tracking markers we do not want to set the qax to equal zero at the start and end of the half gait cycle.

[tvdbogert]

When solving one gait cycle, we would still want periodicity: qax(T)-qax(0) = 0 (like we do for all states)

[moorepants]

This runs I'll merge and it can be tweaked in the master branch.