def __init__(self, hrp, taumax, support_state, active_dofs): self.hrp = hrp self.contact = DistributedContactPoints(hrp, support_state) self.taumax = taumax self.active_dofs = active_dofs
class ForceConstraint(TOPPConstraint): def __init__(self, hrp, taumax, support_state, active_dofs): self.hrp = hrp self.contact = DistributedContactPoints(hrp, support_state) self.taumax = taumax self.active_dofs = active_dofs def compute_abc(self, q, qs, qss, sd0=1., sdd0=0.): taumax = self.taumax[:50] JT = self.contact.compute_jacobian(q).T cdim = JT.shape[1] # == 3 * nb_contact_points pinvJP = linalg.pinv(JT[50:, :]) nullspace_proj = eye(cdim) - dot(pinvJP, JT[50:, :]) a0, b0, c0 = self.compute_phase_components(q, qs, qss) Sa0, Sb0, Sc0 = map(lambda v: v[:50], [a0, b0, c0]) fa0, fb0, fc0 = map(lambda v: dot(pinvJP, v[50:]), [a0, b0, c0]) f0 = fa0 * sdd0 + fb0 * sd0 ** 2 + fc0 T, fc = self.contact.get_friction_matrix() # minimize |z| s.t. (T * f0 + nullspace_proj(z) <= fc) cvx_P = cvxopt.matrix(eye(cdim)) cvx_q = cvxopt.matrix(zeros(cdim)) cvx_G = cvxopt.matrix(dot(T, nullspace_proj)) cvx_h = cvxopt.matrix(fc - dot(T, f0)) cvx_z = cvxopt.solvers.qp(cvx_P, cvx_q, cvx_G, cvx_h)['x'] z = array(cvx_z).reshape((cdim,)) z0 = zeros(cdim) z1 = z z2 = zeros(cdim) a_qp = fa0 + dot(nullspace_proj, z0) b_qp = fb0 + dot(nullspace_proj, z1) c_qp = fc0 + dot(nullspace_proj, z2) check = dot(T, a_qp * sdd0 + b_qp * sd0 ** 2 + c_qp) - fc # assert (check <= 11e-5).all(), "Error with:" + str(check) if not all(check <= 11e-5): print "no solution! returning best shot" assert allclose(z1 + z2, z) # dot(T, f) <= fc a_fric = dot(T, a_qp) b_fric = dot(T, b_qp) c_fric = dot(T, c_qp) - fc a, b, c = map(list, [a_fric, b_fric, c_fric]) # tau <= +taumax a_act, b_act, c_act = {}, {}, {} a_act['right'] = Sa0 - dot(JT, a_qp)[:50] b_act['right'] = Sb0 - dot(JT, b_qp)[:50] c_act['right'] = Sc0 - dot(JT, c_qp)[:50] - taumax # tau >= -taumax a_act['left'] = -a_act['right'] b_act['left'] = -b_act['right'] c_act['left'] = -Sc0 + dot(JT, c_qp)[:50] - taumax for hs in ['left', 'right']: for dof in self.active_dofs: if dof.torque_limit is not None: a.append(a_act[hs][dof.index]) b.append(b_act[hs][dof.index]) c.append(c_act[hs][dof.index]) return a, b, c