def find_supporting_wrenches(self, wrench, point, friction_weight=.1,
pressure_weight=10.):
n = 6 * self.nb_contacts
P = eye(n)
q = zeros((n,))
G = self.compute_stacked_wrench_faces()
h = zeros((G.shape[0],)) # G * x <= h
A = self.compute_grasp_matrix(point)
b = wrench
w_all = solve_relaxed_qp(P, q, G, h, A, b, tol=1e-2)
if w_all is None:
return None
output, next_index = [], 0
for i, contact in enumerate(self.contacts):
for j, p in enumerate(contact.vertices):
output.append((p, w_all[next_index:next_index + 6]))
next_index += 6
return output
def find_supporting_forces(self, wrench, point, friction_weight=.1,
pressure_weight=10.):
"""
Find a set of contact forces supporting a given wrench.
If the resultant wrench ``wrench`` (expressed at ``point``) can be
supported by the contact set, output a set of supporting contact
forces that minimizes the cost
sum_{contact i} w_t * |f_{i,t}|^2 + w_z * |f_{i,z}|^2
where |f_{i,t}| (resp. f_{i,z}) is the norm of the i-th friction (resp.
pressure) force.
INPUT:
- ``wrench`` -- the resultant wrench to be realized
- ``point`` -- point where the wrench is expressed
- ``friction_weight`` -- weight for friction term in optim. objective
- ``pressure_weight`` -- weight for pressure term in optim. objective
OUTPUT:
A list of couples (contact point, contact force) expressed in the world
frame.
.. NOTE::
Physically, contact results in continuous distributions of friction
and pressure forces. However, one can model them without loss of
generality (in terms of the resultant wrench) by considering only
point contact forces applied at the vertices of the contact area.
See [CPN]_ for details.
REFERENCES:
.. [CPN] Caron, Pham, Nakamura, "Stability of surface contacts for
humanoid robots: Closed-form formulae of the contact wrench cone for
rectangular support areas." 2015 IEEE International Conference on
Robotics and Automation (ICRA).
"""
n = 12 * self.nb_contacts
nb_forces = n / 3
P_fric = block_diag(*[
array([
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 0.]])
for _ in xrange(nb_forces)])
P_press = block_diag(*[
array([
[0., 0., 0.],
[0., 0., 0.],
[0., 0., 1.]])
for _ in xrange(nb_forces)])
o_z = hstack([
[0, 0, 1. / n]
for _ in xrange(nb_forces)])
P_press -= dot(o_z.reshape((n, 1)), o_z.reshape((1, n)))
P_local = friction_weight * P_fric + pressure_weight * P_press
RT_diag = block_diag(*[
contact.R.T
for contact in self.contacts for _ in xrange(4)])
P = dot(RT_diag.T, dot(P_local, RT_diag))
q = zeros((n,))
G = self.compute_stacked_force_faces()
h = zeros((G.shape[0],)) # G * x <= h
A = self.compute_grasp_matrix_from_forces(point)
b = wrench
# f_all = cvxopt_solve_qp(P, q, G, h, A, b) # useful for debugging
f_all = solve_relaxed_qp(P, q, G, h, A, b, tol=1e-2)
if f_all is None:
return None
output, next_index = [], 0
for i, contact in enumerate(self.contacts):
for j, p in enumerate(contact.vertices):
output.append((p, f_all[next_index:next_index + 3]))
next_index += 3
return output
