Extension - NLP interface: Low-level NLP formulation and solving

This is completely independent from all robotics code. Provides a generic interface to NLP formulation and calling various solvers.

import robotic as ry
import numpy as np
import matplotlib.pylab as plt

Define a function to compute differentiable features

#the function needs to have the signature (array) -> (array, array) with dimensionalities (n) -> (d, d-times-n)
def sqrPot(x):
    y = np.array(x)
    y[0] = y[0] - 1.
    J = np.eye(y.size)
    return y,J

Define a NLP (non-linear mathematical program)

nlp = ry.NLP_Factory()
nlp.setDimension(3)
nlp.setBounds([-2,-2,-2],[2,2,2])
nlp.setFeatureTypes([ry.OT.sos, ry.OT.sos, ry.OT.sos])
nlp.setEvalCallback(sqrPot)

Define a solver

solver = ry.NLP_Solver()
solver.setProblem(nlp)
solver.setSolver(ry.NLP_SolverID.newton)

solver.solve(True)

solver.getTrace_costs()

import robotic as ry
from robotic import nlp
import numpy as np
import matplotlib.pylab as plt

class MyNLP(nlp.NLP):
    b = 3
    def evaluate(self, x):
        phi = np.array([ x[0]-1,
                         self.b*(x[1]-x[0]**2) ])
        J = np.array([[ 1, 0 ],
                      [ -2*self.b*x[0], self.b ]])
        return phi, J

    def f(self, x):
        phi, _ = self.evaluate(x)
        return np.sum(phi**2)

    def getDimension(self):
        return 2
    def getFeatureTypes(self):
        return [ry.nlp.OT.sos] * 2
    def getBounds(self):
        return np.array([[-2,-1], [2,3]])

p = MyNLP()

solver = ry.NLP_Solver()
solver.setPyProblem(p)
solver.setOptions(maxStep=.5, damping=1e-4)

for i in range(20):
    ret = solver.solve(1)
    print(ret)
x = solver.getTrace_x()
z = solver.getTrace_costs()
plt.plot(x[:,0], x[:,1], 'o-')

X = np.arange(-2, 2, 0.15)
Y = np.arange(-1, 3, 0.15)
X, Y = np.meshgrid(X, Y)
xy = np.hstack((X.reshape(-1,1),Y.reshape(-1,1)))
Z = np.apply_along_axis(p.f, 1, xy).reshape(X.shape)

fig, ax = plt.subplots()
ax.contour(X,Y,Z, 200)
ax.plot(x[:,0], x[:,1], 'o-r', ms=3)

for i in range(20):
    x = solver.getProblem().getInitializationSample()
    r = solver.getProblem().checkJacobian(x, 1e-6)
    print(r, x)