def quadratize_obstacle_cost_hinge(self, x, q, Q): QObs = np.zeros((DIM, DIM)) qObs = np.zeros(DIM) # Obstacles for obs in self.obstacles: d = x[:DIM] - obs.pos distr = np.linalg.norm(d) dist = distr - obs.radius - self.robot_radius if dist > 0: # Hinge cost is 0, so nothing to do here pass else: # Hinge cost is self.obstacle_factor * -self.scale_factor * dist a0 = -self.obstacle_factor * self.scale_factor a1 = a0 / (distr**2) a2 = 1.0 / distr qObs += a0 * (d / distr) QObs += a1 * (distr * np.eye(DIM) - a2*np.outer(d, d)) # Bottom and left boundaries for i in range(DIM): dist = (x[i] - self.bottom_left[i]) - self.robot_radius d = np.zeros(DIM) d[i] = 1.0 if dist > 0: # Hinge cost is 0, so nothing to do here pass else: # Hinge cost is self.obstacle_factor * -self.scale_factor * dist a0 = -self.obstacle_factor * self.scale_factor qObs += a0 * d # Hessian is zero # Top and right boundaries for i in range(DIM): dist = (self.top_right[i] - x[i]) - self.robot_radius d = np.zeros(DIM) d[i] = -1.0 if dist > 0: # Hinge cost is 0, so nothing to do here pass else: # Hinge cost is self.obstacle_factor * -self.scale_factor * dist a0 = -self.obstacle_factor * self.scale_factor qObs += a0 * d # Hessian is zero QObs = regularize(QObs) Q[:DIM, :DIM] = QObs + Q[:DIM, :DIM] q[:DIM] = qObs - QObs.dot(x[:DIM]) + q[:DIM] return q, Q
def quadratize_cost(self, x, u, t, it): Qt = self.Q qt = np.zeros(SX_DIM) qsp, Qsp = self.quadratize_sparse_control_cost(u, t) Qt = regularize(Qt) Rt = Qsp + self.R rt = qsp Pt = np.zeros((SU_DIM, SX_DIM)) return Pt, qt, Qt, rt, Rt
def quadratize_obstacle_cost(self, x, q, Q): ''' Quadratize obstacle cost around x ''' QObs = np.zeros((DIM, DIM)) qObs = np.zeros(DIM) # Obstacles for obs in self.obstacles: d = x[:DIM] - obs.pos distr = np.linalg.norm(d) d /= distr dist = distr - obs.radius - self.robot_radius d_ortho = np.array([d[1], -d[0]]) a0 = self.obstacle_factor * min(np.exp(-self.scale_factor * dist), OVERFLOW_CAP) a1 = -self.scale_factor * a0 a2 = -self.scale_factor * a1 b2 = a1 / distr QObs += a2*(np.outer(d, d)) + b2*(np.outer(d_ortho, d_ortho)) qObs += a1*d # Bottom and left boundaries for i in range(DIM): dist = (x[i] - self.bottom_left[i]) - self.robot_radius d = np.zeros(DIM) d[i] = 1.0 a0 = self.obstacle_factor * min(np.exp(-self.scale_factor*dist), OVERFLOW_CAP) a1 = -self.scale_factor*a0 a2 = -self.scale_factor*a1 QObs += a2*(np.outer(d, d)) qObs += a1*d # Right and top boundaries for i in range(DIM): dist = (self.top_right[i] - x[i]) - self.robot_radius d = np.zeros(DIM) d[i] = -1.0 a0 = self.obstacle_factor * min(np.exp(-self.scale_factor*dist), OVERFLOW_CAP) a1 = -self.scale_factor*a0 a2 = -self.scale_factor*a1 QObs += a2*(np.outer(d, d)) qObs += a1*d QObs = regularize(QObs) Q[:DIM, :DIM] = QObs + Q[:DIM, :DIM] q[:DIM] = qObs - QObs.dot(x[:DIM]) + q[:DIM] return q, Q
def quadratize_obstacle_cost(self, x, q, Q): QObs = np.zeros((NDIM, NDIM)) qObs = np.zeros(NDIM) for obs in self.obstacles: d = x[:NDIM] - obs.pos d[obs.dim] = 0 distr = np.linalg.norm(d) dist = distr - self.robot_radius - obs.radius d /= distr n = np.zeros(3) n[obs.dim] = 1 d_ortho = np.dot(skewSymmetric(n), d) a0 = self.exp_obstacle_cost(dist) a1 = -self.scale_factor * a0 a2 = -self.scale_factor * a1 b2 = a1 / distr QObs += a2 * (np.outer(d, d)) + b2 * np.outer(d_ortho, d_ortho) qObs += a1 * d for i in range(NDIM): dist = (x[i] - self.bottom_left[i]) - self.robot_radius d = np.zeros(NDIM) d[i] = 1.0 a0 = self.exp_obstacle_cost(dist) a1 = -self.scale_factor * a0 a2 = -self.scale_factor * a1 QObs += a2 * (np.outer(d, d)) qObs += a1 * d for i in range(NDIM): dist = (self.top_right[i] - x[i]) - self.robot_radius d = np.zeros(NDIM) d[i] = -1.0 a0 = self.exp_obstacle_cost(dist) a1 = -self.scale_factor * a0 a2 = -self.scale_factor * a1 QObs += a2 * (np.outer(d, d)) qObs += a1 * d QObs = regularize(QObs) Q[:NDIM, :NDIM] = QObs + Q[:NDIM, :NDIM] q[:NDIM] = qObs - QObs.dot(x[:NDIM]) + q[:NDIM] return q, Q
def quadratize_cost(self, x, u, t, it): if t == 0: Qt = self.Q qt = -self.Q.dot(self.x_start) else: Qt = np.zeros((NX_DIM, NX_DIM)) qt = np.zeros(NX_DIM) if it < 1: Qt[3:, 3:] = self.rot_cost * np.eye(3) qt, Qt = self.quadratize_obstacle_cost(x, qt, Qt) qsp, Qsp = self.quadratize_sparse_control_cost(u, t) Qt = regularize(Qt) Rt = self.R + Qsp rt = qsp - self.R.dot(self.u_nominal) Pt = np.zeros((NU_DIM, NX_DIM)) # qscalart = self.cost(x, u, t) + 0.5 * x.T.dot(Qt.dot(x)) + 0.5 * \ # u.T.dot(Rt.dot(u)) - x.T.dot(qt + Qt.dot(x)) - \ # u.T.dot(rt + Rt.dot(u)) return Pt, qt, Qt, rt, Rt
def quadratize_obstacle_cost_aula(self, x, q, Q, t): QObs = np.zeros((DIM, DIM)) qObs = np.zeros(DIM) scaleobstacle = self.obstacle_factor * self.scale_factor ind = 0 # Obstacles for obs in self.obstacles: d = x[:DIM] - obs.pos distr = np.linalg.norm(d) dist = distr - obs.radius - self.robot_radius #a0 = -self.obstacle_factor * self.scale_factor * self.theta[t, ind, 1] #a1 = a0 / (distr**2) #a2 = 1.0 / distr thetaprod = self.theta[t, ind, 0] * self.theta[t, ind, 1] a0 = (scaleobstacle * dist) / self.eta a1 = (scaleobstacle * d) / distr expa0 = min(np.exp(a0), OVERFLOW_CAP) denom = self.theta[t, ind, 1] + self.theta[t, ind, 0] * expa0 a2 = 1.0 / denom**2 a3 = -self.theta[t, ind, 1] * scaleobstacle * \ (distr * np.eye(DIM) - np.outer(d, d)/distr) * (1.0 / distr**2) a4 = ((-thetaprod * scaleobstacle**2) / (self.eta * distr**2)) * expa0 * np.outer(d, d) qObs += -self.theta[t, ind, 1] * a1 / denom QObs += a2 * (denom * a3 - a4) ind += 1 # Bottom and left boundaries for i in range(DIM): dist = (x[i] - self.bottom_left[i]) - self.robot_radius d = np.zeros(DIM) d[i] = 1.0 # a0 = -self.obstacle_factor * self.scale_factor * self.theta[t, ind, 1] thetaprod = self.theta[t, ind, 0] * self.theta[t, ind, 1] a0 = scaleobstacle * dist / self.eta expa0 = min(np.exp(a0), OVERFLOW_CAP) denom = self.theta[t, ind, 1] + self.theta[t, ind, 0] * expa0 a2 = (thetaprod * scaleobstacle**2) / (self.eta * denom**2) qObs += (-self.theta[t, ind, 1] * scaleobstacle / denom) * d QObs[i, i] += a2 * expa0 ind += 1 # Top and right boundaries for i in range(DIM): dist = (self.top_right[i] - x[i]) - self.robot_radius d = np.zeros(DIM) d[i] = -1.0 thetaprod = self.theta[t, ind, 0] * self.theta[t, ind, 1] a0 = scaleobstacle * dist / self.eta expa0 = min(np.exp(a0), OVERFLOW_CAP) denom = self.theta[t, ind, 1] + self.theta[t, ind, 0] * expa0 a2 = (thetaprod * scaleobstacle**2) / (self.eta * denom**2) qObs += (-self.theta[t, ind, 1] * scaleobstacle / denom) * d QObs -= a2 * expa0 ind += 1 QObs = regularize(QObs) Q[:DIM, :DIM] = QObs + Q[:DIM, :DIM] q[:DIM] = qObs - QObs.dot(x[:DIM]) + q[:DIM] return q, Q