def is_positive_combination(b, A):
"""
Check if b can be written as a positive combination of lines from A.
Parameters
----------
b : array
Test vector.
A : array
Matrix of line vectors to combine.
Returns
-------
is_positive_combination : bool
Whether :math:`b = A^T x` for some positive `x`.
Notes
-----
As an alternative implementation, one could try solving a QP minimizing
:math:`\\|A x - b\\|^2` (and no equality constraint), however I found that
the precision of the output is then quite low (~1e-1).
"""
try:
m = A.shape[0]
P, q, G, h = eye(m), zeros(m), -eye(m), zeros(m)
x = solve_qp(P, q, G, h, A.T, b)
if x is None: # optimum not found
return False
except ValueError:
return False
return norm(dot(A.T, x) - b) < 1e-10 and min(x) > -1e-10
def compute_control(self):
"""
Compute the stacked control vector ``U`` minimizing the preview QP.
"""
assert self.psi_last is not None, "Call compute_dynamics() first"
# Cost 1: sum_k u_k^2
P1 = eye(self.U_dim)
q1 = zeros(self.U_dim)
w1 = 1.
# Cost 2: |x_N - x_goal|^2 = |A * x - b|^2
A = self.psi_last
b = self.x_goal - dot(self.phi_last, self.x_init)
P2 = dot(A.T, A)
q2 = -dot(b.T, A)
w2 = 1000.
# Weighted combination of both costs
P = w1 * P1 + w2 * P2
q = w1 * q1 + w2 * q2
# Inequality constraints
G = self.G if self.E is None else vstack([self.G] + self.G_state)
h = self.h if self.E is None else hstack([self.h] + self.h_state)
self.U = solve_qp(P, q, G, h)
def find_supporting_wrenches(self,
wrench,
point,
friction_weight=1e-2,
cop_weight=1.,
yaw_weight=1e-4,
solver='quadprog'):
"""
Find supporting contact wrenches for a given net contact wrench.
Parameters
----------
wrench : array, shape=(6,)
Resultant contact wrench :math:`w_P` to be realized.
point : array, shape=(3,)
Point `P` where the wrench is expressed.
friction_weight : scalar, optional
Weight on friction minimization.
cop_weight : scalar, optional
Weight on COP deviations from the center of the contact patch.
solver : string, optional
Name of the QP solver to use. Default is 'quadprog' (fastest). If
you are planning on using extremal wrenches, 'cvxopt' is slower but
works better on corner cases.
Returns
-------
support : list of (Contact, array) pairs
Mapping between each contact `i` and a supporting contact wrench
:math:`w^i_{C_i}`. All contact wrenches satisfy friction constraints
and sum up to the net wrench: :math:`\\sum_c w^i_P = w_P``.
Notes
-----
Wrench coordinates are returned in their respective contact frames
(:math:`w^i_{C_i}`), not at the point `P` where the net wrench
:math:`w_P` is given.
"""
n = 6 * self.nb_contacts
epsilon = min(friction_weight, cop_weight, yaw_weight) * 1e-3
W_f = diag([friction_weight, friction_weight, epsilon])
W_tau = diag([cop_weight, cop_weight, yaw_weight])
P = block_diag(*[
block_diag(dot(ct.R, dot(W_f, ct.R.T)),
dot(ct.R, dot(W_tau, ct.R.T))) for ct in self.contacts
])
q = zeros((n, ))
G = block_diag(*[ct.wrench_inequalities for ct in self.contacts])
h = zeros((G.shape[0], )) # G * x <= h
A = self.compute_grasp_matrix(point)
b = wrench
w_all = solve_qp(P, q, G, h, A, b, solver=solver)
if w_all is None:
return None
support = [(contact, w_all[6 * i:6 * (i + 1)])
for i, contact in enumerate(self.contacts)]
return support
def __solve_qp(self, qp_P, qp_q, qp_G, qp_h):
try:
qd_active = solve_qp(qp_P, qp_q, qp_G, qp_h)
self.qd[self.active_dofs] = qd_active
except ValueError as e:
if "matrix G is not positive definite" in e:
msg = "rank deficiency. Did you add a regularization task?"
raise IKError(msg)
raise
return self.qd
def compute_velocity_fast(self, dt):
"""
Compute a new velocity satisfying all tasks at best.
Parameters
----------
dt : scalar
Time step in [s].
Returns
-------
qd : array
Vector of active joint velocities.
Note
----
This QP formulation is the default for
:func:`pymanoid.ik.IKSolver.solve` (posture generation) as it converges
faster.
Notes
-----
The method implemented in this function is reasonably fast but may
become unstable when some tasks are widely infeasible and the optimum
saturates joint limits. In such situations, it is better to use
:func:`pymanoid.ik.IKSolver.compute_velocity_safe`.
The returned velocity minimizes squared residuals as in the weighted
cost function, which corresponds to the Gauss-Newton algorithm. Indeed,
expanding the square expression in ``cost(task, qd)`` yields
.. math::
\\mathrm{minimize} \\quad
\\dot{q} J^T J \\dot{q} - 2 r^T J \\dot{q}
Differentiating with respect to :math:`\\dot{q}` shows that the minimum
is attained for :math:`J^T J \\dot{q} = r`, where we recognize the
Gauss-Newton update rule.
"""
n = self.nb_active_dofs
P, v, qd_max, qd_min = self.__compute_qp_common(dt)
G = vstack([+eye(n), -eye(n)])
h = hstack([qd_max, -qd_min])
try:
x = solve_qp(P, v, G, h)
self.qd[self.active_dofs] = x
except ValueError as e:
if "matrix G is not positive definite" in e:
raise Exception(RANK_DEFICIENCY_MSG)
raise
return self.qd
def solve(self):
"""
Compute the series of controls that minimizes the preview QP.
Note
----
This function can only be called after ``build()``.
"""
assert self.P is not None, "you need to build() the MPC problem first"
t_solve_start = time()
U = solve_qp(self.P, self.q, self.G, self.h)
self.U = U.reshape((self.nb_steps, self.u_dim))
self.solve_time = time() - t_solve_start
def compute_velocity_safe(self, dt, margin_reg=1e-5, margin_lin=1e-3):
"""
Compute a new velocity satisfying all tasks at best, while trying to
stay away from kinematic constraints.
Parameters
----------
dt : scalar
Time step in [s].
margin_reg : scalar
Regularization term on margin variables.
margin_lin : scalar
Linear penalty term on margin variables.
Returns
-------
qd : array
Vector of active joint velocities.
Note
----
This QP formulation is the default for :func:`pymanoid.ik.IKSolver.step`
as it has a more numerically-stable behavior.
Notes
-----
This is a variation of the QP from
:func:`pymanoid.ik.IKSolver.compute_velocity_fast` that was reported in
Equation (10) of [Nozawa16]_. DOF limits are better taken care of by
margin variables, but the variable count doubles and the QP takes
roughly 50% more time to solve.
"""
n = self.nb_active_dofs
E, Z = eye(n), zeros((n, n))
P0, v0, qd_max, qd_min = self.__compute_qp_common(dt)
P = vstack([hstack([P0, Z]), hstack([Z, margin_reg * E])])
v = hstack([v0, -margin_lin * ones(n)])
G = vstack(
[hstack([+E, +E / dt]),
hstack([-E, +E / dt]),
hstack([Z, -E])])
h = hstack([qd_max, -qd_min, zeros(n)])
try:
x = solve_qp(P, v, G, h)
self.qd[self.active_dofs] = x[:n]
except ValueError as e:
if "matrix G is not positive definite" in e:
raise Exception(RANK_DEFICIENCY_MSG)
raise
return self.qd
def compute_velocity(self, dt):
"""
Compute a new velocity satisfying all tasks at best, while staying
within joint-velocity limits.
INPUT:
- ``dt`` -- time step
.. NOTE::
Minimizing squared residuals as in the weighted cost function
corresponds to the Gauss-Newton algorithm
<https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm>.
Indeed, expanding the square expression in cost(task, qd) yields
minimize qd * (J.T * J) * qd - 2 * (residual / dt) * J * qd
Differentiating with respect to ``qd`` shows that the minimum is
attained for (J.T * J) * qd == (residual / dt), and we recognize the
Gauss-Newton update rule.
"""
n = self.nb_active_dofs
qp_P = zeros((n, n))
qp_q = zeros(n)
with self.tasks_lock:
for task in self.tasks.itervalues():
J = task.jacobian()[:, self.active_dofs]
r = task.residual(dt)
qp_P += task.weight * dot(J.T, J)
qp_q += task.weight * dot(-r.T, J)
q = self.robot.q[self.active_dofs]
qd_max_doflim = self.doflim_gain * (self.q_max - q) / dt
qd_min_doflim = self.doflim_gain * (self.q_min - q) / dt
qd_max = minimum(self.qd_max, qd_max_doflim)
qd_min = maximum(self.qd_min, qd_min_doflim)
# qp_G = vstack([+eye(n), -eye(n)])
qp_G = self.qp_G # saved to avoid recomputations
qp_h = hstack([qd_max, -qd_min])
try:
qd_active = solve_qp(qp_P, qp_q, qp_G, qp_h)
self.qd[self.active_dofs] = qd_active
except ValueError as e:
if "matrix G is not positive definite" in e:
msg = "rank deficiency. Did you add a regularization task?"
raise IKError(msg)
raise
return self.qd
def compute_velocity(self, dt):
"""
Compute a new velocity satisfying all tasks at best, while staying
within joint-velocity limits.
INPUT:
- ``dt`` -- time step
.. NOTE::
Minimizing squared residuals as in the weighted cost function
corresponds to the Gauss-Newton algorithm
<https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm>.
Indeed, expanding the square expression in cost(task, qd) yields
minimize qd * (J.T * J) * qd - 2 * (residual / dt) * J * qd
Differentiating with respect to ``qd`` shows that the minimum is
attained for (J.T * J) * qd == (residual / dt), and we recognize the
Gauss-Newton update rule.
"""
n = self.robot.nb_active_dofs
q = self.robot.q_active
q_max = self.robot.q_max[self.robot.active_dofs]
q_min = self.robot.q_min[self.robot.active_dofs]
qd_max = self.robot.qd_max[self.robot.active_dofs]
qd_min = self.robot.qd_min[self.robot.active_dofs]
E = eye(n)
qp_P = zeros((n, n))
qp_q = zeros(n)
with self.tasks_lock:
for task in self.tasks.itervalues():
J = task.jacobian()[:, self.robot.active_dofs]
r = task.residual(dt)
qp_P += task.weight * dot(J.T, J)
qp_q += task.weight * dot(-r.T, J)
qd_max_doflim = self.doflim_gain * (q_max - q) / dt
qd_min_doflim = self.doflim_gain * (q_min - q) / dt
qd_max = minimum(qd_max, qd_max_doflim)
qd_min = maximum(qd_min, qd_min_doflim)
qp_G = vstack([+E, -E])
qp_h = hstack([qd_max, -qd_min])
return solve_qp(qp_P, qp_q, qp_G, qp_h)
def find_supporting_wrenches(self, wrench, point):
"""Find supporting contact wrenches for a given net contact wrench.
Parameters
----------
wrench : array, shape=(6,)
Resultant contact wrench :math:`w_P` to be realized.
point : array, shape=(3,)
Point `P` where the wrench is expressed.
Returns
-------
support : list of (Contact, array) pairs
Mapping between each contact `i` in the contact set and a supporting
contact wrench :math:`w^i_{C_i}`. All contact wrenches satisfy
friction constraints and sum up to the net wrench: :math:`\\sum_c
w^i_P = w_P``.
Note
----
Note that wrench coordinates are returned in their respective contact
frames (:math:`w^i_{C_i}`), not at the point `P` where the net wrench
:math:`w_P` is given.
"""
n = 6 * self.nb_contacts
P = eye(n)
q = zeros((n, ))
G = block_diag(*[c.wrench_face for c in self.contacts])
h = zeros((G.shape[0], )) # G * x <= h
A = self.compute_grasp_matrix(point)
b = wrench
w_all = solve_qp(P, q, G, h, A, b)
if w_all is None:
return None
support = [(contact, w_all[6 * i:6 * (i + 1)])
for i, contact in enumerate(self.contacts)]
return support
def is_positive_combination(b, A):
"""
Check if b can be written as a positive combination of lines from A.
INPUT:
- ``b`` -- test vector
- ``A`` -- matrix of line vectors to combine
OUTPUT:
True if and only if b = A.T * x for some x >= 0.
"""
m = A.shape[0]
P, q = eye(m), zeros(m)
#
# NB: one could try solving a QP minimizing |A * x - b|^2 (and no equality
# constraint), however the precision of the output is quite low (~1e-1).
#
G, h = -eye(m), zeros(m)
x = solve_qp(P, q, G, h, A.T, b)
if x is None: # optimum not found
return False
return norm(dot(A.T, x) - b) < 1e-10 and min(x) > -1e-10
