contact_detector_1_point.py

import forcespro as fp
import numpy as np

NUM_FRICTION_CONE_BASIS_VECTORS = 4

stages = fp.MultistageProblem(1)
stages.dims[0]['n'] = NUM_FRICTION_CONE_BASIS_VECTORS  # dimension of decision variables
stages.dims[0]['r'] = 0  # number of eq constraints
stages.dims[0]['l'] = NUM_FRICTION_CONE_BASIS_VECTORS  # number of lower bounds
stages.dims[0]['u'] = 0  # number of upper bounds
stages.dims[0]['p'] = 0  # number of polytopic constraints
stages.dims[0]['q'] = 0  # number of quadratic constraints

stages.cost[0]['H'] = np.zeros((NUM_FRICTION_CONE_BASIS_VECTORS, NUM_FRICTION_CONE_BASIS_VECTORS))  # quadratic cost term (will get filled in later as a parameter)
stages.cost[0]['f'] = np.zeros((NUM_FRICTION_CONE_BASIS_VECTORS,))  # linear cost term (will get filled in later as a parameter)

stages.ineq[0]['b']['lbidx'] = range(1, NUM_FRICTION_CONE_BASIS_VECTORS + 1)
stages.ineq[0]['b']['lb'] = np.zeros((NUM_FRICTION_CONE_BASIS_VECTORS,))

stages.newParam("GTransposeSigmaInverseG", [1], 'cost.H')
stages.newParam("minusTwoGammaTransposeSigmaInverseG", [1], 'cost.f')

stages.newOutput("alpha", 1, range(1, NUM_FRICTION_CONE_BASIS_VECTORS+1))

# solver settings
stages.codeoptions['name'] = 'contact_detector_1_point_solver'
stages.codeoptions['printlevel'] = 0
stages.codeoptions['optlevel'] = 3

# generate code
import get_userid
stages.generateCode(get_userid.userid)

# demonstrate using the solver, and run some timing tests
import contact_detector_1_point_solver_py
problem = contact_detector_1_point_solver_py.contact_detector_1_point_solver_params
gamma = np.array([0,0,1])
sigma_inv = np.eye(3)
F = np.vstack(([1,0,1], [0,1,1], [-1,0,1], [0,-1,1])).T
Jc = np.eye(3)
G = Jc.dot(F)
problem["GTransposeSigmaInverseG"] = G.T.dot(sigma_inv).dot(G)
problem["minusTwoGammaTransposeSigmaInverseG"] = -2 * gamma.T.dot(sigma_inv).dot(G)
[solverout, exitflag, info] = contact_detector_1_point_solver_py.contact_detector_1_point_solver_solve(problem)
print solverout, exitflag, info
print "solvetime:", info.solvetime
alpha = solverout["alpha"]
print "alpha:", alpha

# Run the solver 1000 times and measure timing
import time
start = time.time()
num_tests = 1000
for i in range(num_tests):
    problem["GTransposeSigmaInverseG"] = G.T.dot(sigma_inv).dot(G)
    problem["minusTwoGammaTransposeSigmaInverseG"] = -2 * gamma.T.dot(sigma_inv).dot(G)
    [solverout, exitflag, info] = contact_detector_1_point_solver_py.contact_detector_1_point_solver_solve(problem)
    alpha = solverout["alpha"]

elapsed = time.time() - start
print "average time per solve:", elapsed / num_tests