Implicit midpoint

[bjack205]

Closes #19.

Defines discrete_dynamics, discrete_dynamics, and jacobian! for DiscretizedDynamics models that use implicit integrators. Currently only ImplicitMidpoint is defined. These methods use Newton's method and the implicit function theorem to emulate the behavior expected by an explicit integrator.

All methods are allocation-free for both static and in-place vector arguments / function signatures.

Each implicit integrator must store an instance of ImplicitNewtonCache, which provides the cache variables needed to compute the needed matrices / vectors. In order to calculate the Jacobian of a DiscretizedModel using an implicit integrator, the ImplicitFunctionTheorem DiffMethod must be used, which also specifies the diff method to be used to evaluate the
Jacobian of implicit dynamics residual function, e.g. ImplicitFunctionTheorem{ForwardAD}, created as ImplicitFunctionTheorem(ForwardAD())

All methods assume the control at the next time step is the same as the control at the current time step (i.e. zero-order-hold).

An example of using the API:

using RobotDynamics
const RD = RobotDynamics

model = Cartpole()
integrator = RD.ImplicitMidpoint(model)
dmodel = RD.DiscretizedDynamics(model, integrator)
# OR 
dmodel = RD.DiscretizedDynamics{RD.ImplicitMidpoint}(model)

n,m = RD.dims(model)
x1,u1 = rand(model)   # generate some random state and control vectors
dt = 0.01
z1 = KnotPoint{n,m}(x1,u1,0.0,dt)
z1_static = KnotPoint{n,m}(SVector{n}(x1), SVector{m}(u1), 0.0, dt)
x2 = copy(x1)

# Solve for next state using Newton's method
RD.discrete_dynamics!(dmodel, x2, z1)
x2_static = RD.discrete_dynamics(dmodel, z1_static)

# Get the dynamics Jacobians
J = zeros(n, n+m)
diff = RD.default_diffmethod(dmodel)  
# OR 
diff = RD.ImplicitFunctionTheorem(ForwardAD())
RD.jacobian!(RD.InPlace(), diff, dmodel, J, xn, z1)
RD.jacobian!(RD.StaticReturn(), diff, dmodel, J, xn, z1_static)