def check_contact_forces(self, q, qd, qdd): import cvxopt.solvers cvxopt.solvers.options['show_progress'] = False # disable output with self.hrp.rave: self.hrp.rave.SetDOFValues(q) self.hrp.rave.SetDOFVelocities(qd) tau = self.rave.ComputeInverseDynamics(qdd) J, contacts = self.compute_contact_jacobian( q, qd, self.support_state, returncontacts=True) ncontacts = len(contacts) T, fc = self.get_friction_cones(ncontacts) PJT, Ptau = J.T[50:, :], tau[50:] # QP: minimize norm(dot(PJT, f) - Ptau) # s.t. dot(T, f) <= fc # qp_P = cvxopt.matrix(dot(PJT.T, PJT)) qp_q = cvxopt.matrix(-dot(Ptau, PJT)) qp_G = cvxopt.matrix(T) qp_h = cvxopt.matrix(fc) qp_x = cvxopt.solvers.qp(qp_P, qp_q, qp_G, qp_h)['x'] f = array(qp_x).reshape((3 * ncontacts,)) error = norm(dot(PJT, f) - Ptau) thres = 300 * 1e-2 # norm(Ptau) is around 300 N.m if error > thres / 2: # more than half the allowed error print "danger zone: (%f > %f) " % (error, thres / 2) assert error < thres, \ "contact forces cannot realize the motion " \ "(%f > %f) " % (error, thres) return contacts, f
def connect(q0, q1, support): """Bezier curve with as-linear-as-possible velocity.""" assert not hrp.self_collides(q0) assert not hrp.self_collides(q1) qd_ref = (q1 - q0) / norm(q1 - q0) qd0 = project_foot_velocity(q0, qd_ref) qd1 = project_foot_velocity(q1, qd_ref) traj = active_bezier_interpolate( q0, qd0, q1, qd1, ACTUATED_DOFS, support) return traj
def _enforce(self, q_warm): self.converged = False self.robot.rave.SetDOFValues(q_warm) self.robot.rave.SetActiveDOFs(self.active_indexes) q_max = array([DOF_SCALE * dof.ulim for dof in self.active_dofs]) q_min = array([DOF_SCALE * dof.llim for dof in self.active_dofs]) I = eye(self.nb_active_dof) q = full_to_active(q_warm, self.active_dofs) self.robot.rave.SetActiveDOFValues(q) for itnum in xrange(self.max_iter): conv_vect = array([norm(task.f()) for task in self.tasks]) if numpy.all(conv_vect < self.conv_thres): self.converged = True break if DEBUG_IK: conv = ["%10.8f" % x for x in conv_vect] print " %4d: %s" % (itnum, ' '.join(conv)) ker_proj = eye(self.nb_active_dof) dq = zeros(self.nb_active_dof) qd_max_reg = self.gain * (q_max - q) qd_min_reg = self.gain * (q - q_min) for i, task in enumerate(self.tasks): J = task.J() Jn = dot(J, ker_proj) b = -self.gain * task.f() - dot(J, dq) In = eye(Jn.shape[0]) sr_inv = dot(Jn.T, linalg.inv(dot(Jn, Jn.T) + 1e-8 * In)) dq += dot(sr_inv, b) ker_proj = dot(ker_proj, I - dot(linalg.pinv(Jn), Jn)) qd_max_reg = self.gain * (q_max - q) qd_min_reg = self.gain * (q - q_min) q += solve_ineq(I, dq, I, qd_max_reg, qd_min_reg) self.robot.rave.SetActiveDOFValues(q) return self.robot.rave.GetDOFValues()