def constraints(self, x):
#
# The callback for calculating the constraints
#
math = Math()
pos = x[0:3]
force = x[3:6]
tau_constraint = np.dot(math.skew(pos), force)
return np.array((tau_constraint))
def bilinear_formulation():
''' Definition the bilinear problem '''
math = Math()
pos0 = np.array([2.5, 2.5, 2.5])
force0 = np.array([-10.0, 10.0, 10.0])
posDesired = np.array([2.0, 2.0, 2.0])
forceDesired = np.array([0.0, 0.0, 10.0])
tauDesired = np.dot(math.skew(posDesired), forceDesired)
print "initial contact position: ", pos0
print "initial force: ", force0
print "Desired torque: ", tauDesired
x0 = np.hstack([pos0, force0])
lb = [1.0, 1.0, 1.0, -20.0, -20.0, 0.0]
ub = [3.0, 3.0, 3.0, 20.0, 20.0, 20.0]
tau_x_constraints_lb = np.array([20.0])
tau_x_constraints_ub = np.array([20.0])
tau_y_constraints_lb = np.array([-20.0])
tau_y_constraints_ub = np.array([-20.0])
tau_z_constraints_lb = np.array([0.0])
tau_z_constraints_ub = np.array([0.0])
cl = np.hstack(
[tau_x_constraints_lb, tau_y_constraints_lb, tau_z_constraints_lb])
cu = np.hstack(
[tau_x_constraints_ub, tau_y_constraints_ub, tau_z_constraints_ub])
nlpConvex = ipopt.problem(n=len(x0),
m=len(cl),
problem_obj=buildBilinearFormulation(x0),
lb=lb,
ub=ub,
cl=cl,
cu=cu)
#
# Set solver options
#
#nlp.addOption('derivative_test', 'second-order')
nlpConvex.addOption('mu_strategy', 'adaptive')
nlpConvex.addOption('jacobian_approximation', 'finite-difference-values')
nlpConvex.addOption('hessian_approximation', 'limited-memory')
nlpConvex.addOption('tol', 1e-2)
#
# Scale the problem (Just for demonstration purposes)
#
nlpConvex.setProblemScaling(obj_scaling=1,
x_scaling=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
nlpConvex.addOption('nlp_scaling_method', 'user-scaling')
#
# Solve the problem
#
sol, info = nlpConvex.solve(x0)
print "Solution of the primal variables: x=%s\n" % repr(sol)
print "Solution of the dual variables: lambda=%s\n" % repr(info['mult_g'])
print "Objective=%s\n" % repr(info['obj_val'])
l = sol[0:3]
f = sol[3:6]
tau = np.dot(math.skew(l), f)
print colored('-----------> Results check: ', 'red')
print colored('desired torque: ',
'blue'), tauDesired[0], tauDesired[1], tauDesired[2]
print colored('actual torque: ', 'green'), tau
print colored('torque error: ', 'red'), np.subtract(tau, tauDesired)
print colored('foot pos: ', 'green'), l
print colored('force: ', 'green'), f