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