def __init__(self, hrp, taumax, support_state, active_dofs):
self.hrp = hrp
self.taumax = taumax[:50]
self.active_dofs = active_dofs
self.contact = SurfaceContactWrench(hrp, support_state)
self.last_jacobian = 100000 * ones((6, 56))
self.last_jacnorm = 1.
self.last_projectors = None
self.nb_recomp = 0
class WrenchConstraint(TOPPConstraint):
def __init__(self, hrp, taumax, support_state, active_dofs):
self.hrp = hrp
self.taumax = taumax[:50]
self.active_dofs = active_dofs
self.contact = SurfaceContactWrench(hrp, support_state)
self.last_jacobian = 100000 * ones((6, 56))
self.last_jacnorm = 1.
self.last_projectors = None
self.nb_recomp = 0
def compute_actuated_projector_righetti(self, q):
"""This function implements the result from
Righetti, Ludovic, et al. "Optimal distribution of contact forces
with inverse-dynamics control." The International Journal of
Robotics Research 32.3 (2013): 280-298.
with a custom weight matrix on constraint variables.
"""
weight_mat = eye(6)
weight_mat[2, 2] = -X ** 2 - Y ** 2 - 2 * mu ** 2 # fz^2
weight_mat[5, 5] = 0
J = self.contact.compute_jacobian(q)
if matnorm(J - self.last_jacobian) < 5e-2 * self.last_jacnorm:
return self.last_projectors
elif DEBUG_WRENCH:
print "recomputing QR decomposition...",
print matnorm(J - self.last_jacobian),
print self.nb_recomp,
self.nb_recomp += 1
Q, R0 = qr(J.T)
Qc = Q[:, :6] # constrained
Qu = Q[:, 6:] # unconstrained
R = R0[:6, :]
nullmat = R0[6:, :]
assert weight_mat.shape == (6, 6)
assert allclose(dot(Q.T, Q), eye(len(q)))
assert Q.shape == (len(q), len(q))
assert R0.shape == (len(q), 6)
assert R.shape == (6, 6)
assert allclose(nullmat, 0)
assert allclose(dot(Qc, R), J.T)
n_act = len(q) - 6 # number of actuated DOF
I_act = eye(n_act)
S = hstack([I_act, zeros((n_act, 6))])
R_inv = inv(R)
Wc_inside = vstack([
hstack([alldot(inv(R.T), weight_mat, R_inv), zeros((6, n_act))]),
hstack([zeros((n_act, 6)), I_act])])
Wc = alldot(Q, Wc_inside, Q.T)
W = alldot(S, Wc, S.T)
W_inv = inv(W)
QuTSTinv = inv(dot(Qu.T, S.T))
# Unconstrained-space projector
#
# tau = Pu * (M * qdd + h)
# Pu.shape = (n_act, n)
#
Pu = dot(QuTSTinv, Qu.T) \
+ alldot((I_act - alldot(QuTSTinv, Qu.T, S.T)), W_inv, S, Wc)
# Constrained-space projector
#
# lambda = Pc * (M * qdd + h)
# Pc.shape = (6, n)
#
Pc = alldot(R_inv, Qc.T, eye(len(q)) - dot(S.T, Pu))
if self.last_projectors:
print matnorm(Pc - self.last_projectors[0]),
print matnorm(Pu - self.last_projectors[1])
self.last_jacobian = J
self.last_jacnorm = matnorm(J)
self.last_projectors = (Pc, Pu)
return (Pc, Pu)
def compute_actuated_projector(self, q):
J = self.contact.compute_jacobian(q)
P, S = zeros((6, 56)), zeros((50, 56))
P[:, 50:] = eye(6)
S[:, :50] = eye(50)
Pc = dot(inv(dot(P, J.T)), P)
Pu = S - alldot(S, J.T, Pc)
return (Pc, Pu)
def compute_abc(self, q, qs, qss):
a, b, c = [], [], []
a0, b0, c0 = self.compute_phase_components(q, qs, qss)
Pc, Pu = self.compute_actuated_projector(q)
# tau - taumax <= 0
a_taumax = dot(Pu, a0)
b_taumax = dot(Pu, b0)
c_taumax = dot(Pu, c0) - self.taumax
for dof in self.active_dofs:
if dof.torque_limit is not None:
a.append(a_taumax[dof.index])
b.append(b_taumax[dof.index])
c.append(c_taumax[dof.index])
# -tau - taumax <= 0
a_taumin = -dot(Pu, a0)
b_taumin = -dot(Pu, b0)
c_taumin = -dot(Pu, c0) - self.taumax
for dof in self.active_dofs:
if dof.torque_limit is not None:
a.append(a_taumin[dof.index])
b.append(b_taumin[dof.index])
c.append(c_taumin[dof.index])
C_local = array([
[+1, 0, -mu, 0, 0, 0],
[-1, 0, -mu, 0, 0, 0],
[0, +1, -mu, 0, 0, 0],
[0, -1, -mu, 0, 0, 0],
[0, 0, -1, 0, 0, 0],
[0, 0, -Y, +1, 0, 0],
[0, 0, -Y, -1, 0, 0],
[0, 0, -X, 0, +1, 0],
[0, 0, -X, 0, -1, 0],
[+Y, +X, -mu * (X + Y), -mu, -mu, -1],
[+Y, -X, -mu * (X + Y), -mu, +mu, -1],
[-Y, +X, -mu * (X + Y), +mu, -mu, -1],
[-Y, -X, -mu * (X + Y), +mu, +mu, -1],
[+Y, +X, -mu * (X + Y), +mu, +mu, +1],
[+Y, -X, -mu * (X + Y), +mu, -mu, +1],
[-Y, +X, -mu * (X + Y), -mu, +mu, +1],
[-Y, -X, -mu * (X + Y), -mu, -mu, +1]])
R = self.contact.get_link_rotation(q)
import pylab
assert pylab.norm(R - eye(3)) <= 1e-1 # tmp
O = zeros((3, 3))
wrench_rotation = vstack([
hstack([R, O]),
hstack([O, R])])
C = dot(C_local, wrench_rotation)
# C * wrench <= 0
a_wrench = alldot(C, Pc, a0)
b_wrench = alldot(C, Pc, b0)
c_wrench = alldot(C, Pc, c0)
a.extend(a_wrench)
b.extend(b_wrench)
c.extend(c_wrench)
return a, b, c
def test_abc(self, q, qs, qss, sd, sdd=0):
"""Compare (a, b, c) with the DCP model."""
a, b, c = self.compute_abc(q, qs, qss)
a, b, c = map(array, [a, b, c])
xW = a * sdd + b * sd ** 2 + c
print "SWC: (a sdd + b sd^2 + c <= 0)?", all(xW < 0)
from dcp_cons import ForceConstraint
fcons = ForceConstraint(self.hrp, self.hrp.torque_limits,
self.hrp.Support.LEFT, self.active_dofs)
a2, b2, c2 = fcons.compute_abc(q, qs, qss, sd0=sd, sdd0=sdd)
a2, b2, c2 = map(array, [a2, b2, c2])
xf = a2 * sdd + b2 * sd ** 2 + c2
print "DCP: (a sdd + b sd^2 + c <= 0)?", all(xf < 0)
