Optimization (NLP formulation and solving)

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

from robotic import ry
import numpy as np

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)

<robotic.ry.NLP_Solver at 0x7fd5e888f9b0>

solver.solve(True)

<robotic.ry.SolverReturn at 0x7fd5e428b6f0>

solver.getTrace_x()

array([[ 0.82853794, -1.65277958, -0.98424911],
       [ 0.84928627, -1.45277958, -0.86514683],
       [ 0.8700346 , -1.25277958, -0.74604455],
       [ 0.89078292, -1.05277958, -0.62694226],
       [ 0.91153125, -0.85277958, -0.50783998],
       [ 0.93227958, -0.65277958, -0.38873769],
       [ 0.95302791, -0.45277958, -0.26963541],
       [ 0.97377623, -0.25277958, -0.15053313],
       [ 0.99125874, -0.08425986, -0.05017771],
       [ 0.99708625, -0.02808662, -0.0167259 ],
       [ 0.99902875, -0.00936221, -0.0055753 ],
       [ 0.99967625, -0.00312074, -0.00185843]])

solver.getTrace_costs()

array([[3.72982590e+00, 0.00000000e+00, 0.00000000e+00],
       [2.88176217e+00, 0.00000000e+00, 0.00000000e+00],
       [2.14293015e+00, 0.00000000e+00, 0.00000000e+00],
       [1.51332981e+00, 0.00000000e+00, 0.00000000e+00],
       [9.92961174e-01, 0.00000000e+00, 0.00000000e+00],
       [5.81824230e-01, 0.00000000e+00, 0.00000000e+00],
       [2.79918980e-01, 0.00000000e+00, 0.00000000e+00],
       [8.72454237e-02, 0.00000000e+00, 0.00000000e+00],
       [9.69393597e-03, 0.00000000e+00, 0.00000000e+00],
       [1.07710400e-03, 0.00000000e+00, 0.00000000e+00],
       [1.19678222e-04, 0.00000000e+00, 0.00000000e+00],
       [1.32975802e-05, 0.00000000e+00, 0.00000000e+00]])