def lp_ineq_4D(K, k, ones_range=None):
if (ones_range == None):
ones_range = (0, K.shape[0])
K, k = __normalize(K, k)
K_1 = __compute_K_1(K, ones_range)
cost = array([0., 0., 0., -1.])
lb = array([-100000000. for _ in range(4)])
ub = array([100000000. for _ in range(4)])
Alb = array([-100000000. for _ in range(k.shape[0])])
#~ K_1, k = __normalize(K_1, k)
solver = solver_LP_abstract.getNewSolver('qpoases',
"dyn_eq",
maxIter=10000,
maxTime=10000.0,
useWarmStart=False,
verb=0)
(status, res, rest) = solver.solve(cost,
lb=lb,
ub=ub,
A_in=K_1,
Alb=Alb,
Aub=-k,
A_eq=None,
b=None)
#problem solved or unfeasible
status_ok = status == solver_LP_abstract.LP_status.OPTIMAL
p_solved = status_ok and res[3] >= 0.
return status_ok, p_solved, res
def __init__ (self, name, contact_points, contact_normals, mu, g, mass, a, a0, maxIter=100, verb=0, solver='cvxopt'):
self.name = name;
self.solver = optim.getNewSolver(solver, name, maxIter=maxIter, verb=verb);
self.maxIter = maxIter;
self.verb = verb;
self.m_in = 6;
self.a = np.asarray(a).squeeze().copy();
self.a0 = np.asarray(a0).squeeze().copy();
# compute matrix A, which maps the force generator coefficients into the centroidal wrench
(self.A, G4) = compute_centroidal_cone_generators(contact_points, contact_normals, mu);
self.D = np.zeros((6,3));
self.d = np.zeros(6);
self.D[3:,:] = -mass*crossMatrix(g);
self.d[:3] = mass*g;
self.n = self.A.shape[1]+1;
self.grad = np.zeros(self.n);
self.grad[0] = -1.0;
self.constrMat = np.zeros((self.m_in,self.n));
self.constrMat[:,1:] = self.A;
self.constrMat[:,0] = -np.dot(self.D, self.a);
self.constrB = self.d + np.dot(self.D, self.a0);
self.lb = np.zeros(self.n);
self.lb[0] = -1e10;
self.ub = np.array(self.n*[1e10,]);
self.x = np.zeros(self.n);
def __init__(self,
name,
contact_points,
contact_normals,
mu,
g,
mass,
contact_tangents=None,
maxIter=100,
verb=0,
solver='cvxopt'):
g = np.asarray(g).squeeze()
self.solver = optim.getNewSolver(solver,
name,
maxIter=maxIter,
verb=verb)
self.D = np.zeros((6, 3))
self.d = np.zeros(6)
self.D[3:, :] = -mass * crossMatrix(g)
self.d[:3] = mass * g
(G, tmp) = compute_centroidal_cone_generators(contact_points,
contact_normals, mu,
contact_tangents)
self.name = name
self.maxIter = maxIter
self.verb = verb
self.m_in = G.shape[1]
self.n = 6
# self.iter = 0;
# self.initialized = False;
# self.Hess = np.zeros((self.n,self.n));
self.grad = np.zeros(self.n)
self.constrInMat = np.zeros((self.m_in, self.n))
self.constrInMat[:, :] = G.T
self.constrInUB = np.zeros(self.m_in) + 1e100
self.constrInLB = np.zeros(self.m_in)
self.constrEqMat = np.dot(np.ones(G.shape[1]), G.T).reshape((1, 6))
self.constrEqB = np.ones(1)
self.lb = np.array(self.n * [
-1e10,
])
self.ub = np.array(self.n * [
1e10,
])
self.x = np.zeros(self.n)
def __init__(self,
name,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=10000,
verb=0,
regularization=1e-5,
solver='qpoases'):
''' Constructor
@param c0 Initial CoM position
@param v Opposite of the direction in which you want to maximize the CoM acceleration (typically that would be
the CoM velocity direction)
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
@param solver Specify which LP solver to use (qpoases, cvxopt or scipy)
'''
self.name = name
self.verb = verb
self.m_in = 6
self.solver = optim.getNewSolver(solver,
name,
maxIter=maxIter,
verb=verb)
self.hessian_regularization = INITIAL_HESSIAN_REGULARIZATION
self.b = np.zeros(6)
self.d = np.empty(6)
self.c0 = np.empty(3)
self.v = np.empty(3)
self.constrUB = np.zeros(self.m_in) + 1e100
# self.constrLB = np.zeros(self.m_in)-1e100;
self.set_problem_data(c0, v, contact_points, contact_normals, mu, g,
mass, regularization)
def __init__(self,
name,
c0,
dc0,
contact_points,
contact_normals,
mu,
g,
mass,
kinematic_constraints=None,
angular_momentum_constraints=None,
contactTangents=None,
maxIter=1000,
verb=0,
regularization=1e-5,
solver='qpoases'):
''' Constructor
@param c0 Initial CoM position
@param dc0 Initial CoM velocity
@param contact points A matrix containing the contact points
@param contact normals A matrix containing the contact normals
@param mu Friction coefficient (either a scalar or an array)
@param g Gravity vector
@param mass The robot mass
@param kinematic constraints couple [A,b] such that the com is constrained by A x <= b
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
assert mass > 0.0, "Mass is not positive"
assert mu > 0.0, "Friction coefficient is not positive"
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(
dc0).squeeze().shape[0] == 3, "Com velocity vector has not size 3"
assert np.asarray(
contact_points).shape[1] == 3, "Contact points have not size 3"
assert np.asarray(
contact_normals).shape[1] == 3, "Contact normals have not size 3"
assert np.asarray(contact_points).shape[0] == np.asarray(
contact_normals
).shape[
0], "Number of contact points do not match number of contact normals"
self._name = name
self._maxIter = maxIter
self._verb = verb
self._c0 = np.asarray(c0).squeeze().copy()
self._dc0 = np.asarray(dc0).squeeze().copy()
self._mass = mass
self._g = np.asarray(g).squeeze().copy()
self._gX = skew(self._g)
# self._regularization = regularization;
self.set_contacts(contact_points, contact_normals, mu, contactTangents)
if kinematic_constraints != None:
self._kinematic_constraints = kinematic_constraints[:]
else:
self._kinematic_constraints = None
if angular_momentum_constraints != None:
self._angular_momentum_constraints = angular_momentum_constraints[:]
else:
self._angular_momentum_constraints = None
self._lp_solver = getNewSolver('qpoases',
"name",
useWarmStart=False,
verb=0)
self._qp_solver = qp_solver(3,
0,
solver='qpoases',
accuracy=1e-6,
maxIter=100,
verb=0)