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