def distance_squared(self, u):
# Function `distance` can be applied to CasADi symbols and
# lists of numbers. However, if `u` is a symbol, we need to
# use appropriate CasADi functions like cs.sign and cs.norm_2
if fn.is_symbolic(u):
# Case I: `u` is a CasADi SX symbol
nu = u.size(1)
v = u if self.__center is None else u - self.__center
elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \
or isinstance(u, np.ndarray):
# Case II: `u` is an array of numbers or an np.ndarray
nu = len(u)
if self.__center is None:
v = u
else:
# Note: self.__center is np.ndarray (`u` might be a list)
z = self.__center.reshape(nu)
u = np.array(u).reshape(nu)
v = np.subtract(u, z)
else:
raise Exception("u is of invalid type")
# Compute distance to Ball infinity:
# dist^2(u) = norm(v)^2
# + SUM_i [
# min{vi^2, r^2}
# - 2*min{vi^2, r*|vi|}
# ]
# where v = u - xc
squared_distance = fn.norm2(v)**2
for i in range(nu):
squared_distance += fn.fmin(v[i]**2, self.radius**2) \
- 2.0 * fn.fmin(v[i]**2, self.radius * fn.fabs(v[i]))
return squared_distance
def distance_squared(self, u):
"""Computes the squared distance between a given point `u` and this
second-order cone
:param u: given point; can be a list of float, a numpy
n-dim array (`ndarray`) or a CasADi SX/MX symbol
:return: distance from set as a float or a CasADi symbol
"""
if isinstance(u, cs.SX):
warnings.warn(
"This function does not accept casadi.SX; use casadi.MX instead"
)
if fn.is_symbolic(u):
nu = u.size(1)
elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \
or isinstance(u, np.ndarray):
nu = len(u)
else:
raise Exception("Illegal Argument, `u`")
# Partition `u = (x, r)`, where `r` is the last element of `u`
a = self.__a
x = u[0:nu - 1]
r = u[nu - 1]
eps = 1e-16
norm_x = fn.norm2(x) # norm of x
sq_norm_x = cs.dot(x, x) # squared norm of x
gamma = (a * norm_x + r) / (a**2 + 1)
fun1 = 0
fun2 = sq_norm_x + r**2
fun3 = sq_norm_x * (1 - gamma * a / norm_x)**2 + (r - gamma)**2
condition0 = norm_x + cs.fabs(r) < eps
condition1 = norm_x <= a * r
condition2 = a * norm_x <= -r
f = cs.if_else(
condition0, 0,
cs.if_else(condition1, fun1,
cs.if_else(condition2, fun2, fun3, True), True), True)
return f
def distance_squared(self, u):
"""Computes the squared distance between a given point `u` and this ball
:param u: given point; can be a list of float, a numpy
n-dim array (`ndarray`) or a CasADi SX/MX symbol
:return: distance from set as a float or a CasADi symbol
"""
if fn.is_symbolic(u):
# Case I: `u` is a CasADi SX symbol
v = u if self.__center is None else u - self.__center
elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u))\
or isinstance(u, np.ndarray):
# Case II: `u` is an array of numbers or an np.ndarray
if self.__center is None:
v = u
else:
# Note: self.__center is np.ndarray (`u` might be a list)
z = self.__center.reshape(len(u))
u = np.array(u).reshape(len(u))
v = np.subtract(u, z)
else:
raise Exception("u is of invalid type")
# Compute squared distance
# Let B = B(xc, r) be a Euclidean ball centered at xc with radius r
#
# dist_B^2(u) = max(0, sign(t(u))*t(u)^2), where
# t(u) = ||u - x_c|| - r
#
# Note: there is another way to take squared distances:
# d_B^2(u) = ||u||^2 * (1 - 1 / max{r, ||u||})^2,
# but this leads to slightly lengthier CasADi symbols for
# the Jacobian of the squared distance, so this approach was
# abandoned
t = fn.norm2(v) - self.radius
return fn.fmax(0.0, fn.sign(t) * t ** 2)
