def cone_span_to_face(S, eliminate_redundancies=False):
"""
Returns the face matrix S^F of the span matrix S,
that is, a matrix such that
{x = S z, z >= 0} if and only if {S^F x <= 0}.
"""
V = hstack([ones((S.shape[1], 1)), S.T])
# V-representation: first column is 0 for rays
V_cdd = Matrix(V, number_type=NUMBER_TYPE)
V_cdd.rep_type = RepType.GENERATOR
P = Polyhedron(V_cdd)
H_matrix = P.get_inequalities()
if (eliminate_redundancies):
H_matrix.canonicalize()
H = array(H_matrix)
if (H.shape[1] > 1):
b = H[:, 0]
A = H[:, 1:]
else:
b, A = H[:, 0], zeros((H.shape[0], S.shape[0]))
#~ for i in xrange(H.shape[0]):
#~ if b[i] != 0:
#~ raise NotConeSpan(S)
return -A, b
def eliminate_redundant_inequalities(A,b):
# H-representation: A x + b >= 0
A_plot = Matrix(hstack([b.reshape((A.shape[0],1)), -A]), number_type=NUMBER_TYPE)
A_plot.rep_type = RepType.INEQUALITY
A_plot.canonicalize()
A_plot = np.array(A_plot)
b_plot, A_plot = A_plot[:, 0], -A_plot[:, 1:]
return (A_plot, b_plot);
def poly_span_to_face(S):
"""
Returns the face matrix S^F of the span matrix S,
that is, a matrix such that
{x = S z, z >= 0, sum(z)=1} if and only if {S^F x <= s}.
"""
V = hstack([ones((S.shape[1], 1)), S.T])
# V-representation: first column is 0 for rays, 1 for vertices
V_cdd = Matrix(V, number_type=NUMBER_TYPE)
V_cdd.rep_type = RepType.GENERATOR
P = Polyhedron(V_cdd)
H = array(P.get_inequalities()) # H-representation: A x + b >= 0
b, A = H[:, 0], H[:, 1:]
return (-A, b)
def arbitrary_face_to_span(F, f):
"""
Compute the span matrix F^S of the face matrix F,
that is, a matrix such that
{F x <= f} if and only if {x = F^S z, z >= 0, sum(z)=1}.
Works for both polytopes and cones.
"""
b, A = f.reshape((F.shape[0], 1)), -F
# H-representation: A x + b >= 0
F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE)
F_cdd.rep_type = RepType.INEQUALITY
P = Polyhedron(F_cdd)
V = array(P.get_generators())
return (V[:, 1:].T, V[:, 0])
def theta_k(circumbody, k=0, i=0):
"""
This is from the Section 4 of the paper [Sadraddini and Tedrake, 2020].
Inputs:
* AH-polytope: ``circumbody``
* int ``k``: the number of columns added to the subspace of ``U``.
For more information, look at the paper.
*Default* is 0.
Outputs:
* 2D numpy array ``Theta``, which is defined in the paper
"""
circumbody_AH = pp.to_AH_polytope(circumbody)
H_y = circumbody_AH.P.H
q_y = H_y.shape[0]
if k < 0:
return np.eye(q_y)
Y = circumbody_AH.T
# First identify K=[H_y'^Y'+ ker(H_y')]
S_1 = np.dot(np.linalg.pinv(H_y.T), Y.T)
S_2 = spa.null_space(H_y.T)
if S_2.shape[1] > 0:
S = np.hstack((S_1, S_2))
else:
S = S_1
# phiw>=0. Now with the augmentation
S_complement = spa.null_space(S.T)
circumbody.dim_complement = S_complement.shape[1]
number_of_columns = min(k, S_complement.shape[1])
if k == 0:
psi = S
else:
psi = np.hstack((S, S_complement[:, i:number_of_columns + i]))
p_mat = Matrix(np.hstack((np.zeros((psi.shape[0], 1)), psi)))
p_mat.rep_type = RepType.INEQUALITY
poly = Polyhedron(p_mat)
R = np.array(poly.get_generators())
r = R[:, 1:].T
Theta = np.dot(psi, r)
assert np.all(Theta > -1e-5)
return Theta
def cone_span_to_face(S, eliminate_redundancies=False):
"""
Returns the face matrix S^F of the span matrix S,
that is, a matrix such that
{x = S z, z >= 0} if and only if {S^F x <= 0}.
"""
S = np.asarray(S).squeeze()
V = hstack([zeros((S.shape[1], 1)), S.T])
# V-representation: first column is 0 for rays
V_cdd = Matrix(V, number_type=NUMBER_TYPE)
V_cdd.rep_type = RepType.GENERATOR
P = Polyhedron(V_cdd)
H_matrix = P.get_inequalities()
if (eliminate_redundancies):
try:
H_matrix.canonicalize()
except:
print "RuntimeError: failed to canonicalize matrix"
H = array(H_matrix)
if (len(H.shape) < 2):
# warnings.warn("[cone_span_to_face] H is a vector rather than a matrix. S:\n"+str(S)+"\nH:\n"+str(H));
# S += 1e-6*np.random.rand(S.shape[0], S.shape[1]);
# V = hstack([zeros((S.shape[1], 1)), S.T])
# V_cdd = Matrix(V, number_type=NUMBER_TYPE)
# V_cdd.rep_type = RepType.GENERATOR
# P = Polyhedron(V_cdd)
# H_matrix = P.get_inequalities();
# H = array(H_matrix);
# if(len(H.shape)<2):
raise ValueError(
"[cone_span_to_face] Cddlib failed to convert cone span to face: H is a vector rather than a matrix. H: "
+ str(H))
if (H.shape[1] > 1):
b = H[:, 0]
A = H[:, 1:]
else:
b, A = H[:, 0], zeros((H.shape[0], S.shape[0]))
for i in xrange(H.shape[0]):
if b[i] != 0:
raise NotConeSpan(S)
return -A
def arbitrary_span_to_face(S, rv):
"""
Returns the face matrix S^F of the span matrix S,
that is, a matrix such that
{x = S z, z >= 0} if and only if {S^F x <= f}.
The vector rv specifies whether the corresponding column in S
is a vertex (1) or a ray (0).
"""
V = hstack([rv.reshape((S.shape[1], 1)), S.T])
# V-representation: first column is 0 for rays
V_cdd = Matrix(V, number_type=NUMBER_TYPE)
V_cdd.rep_type = RepType.GENERATOR
P = Polyhedron(V_cdd)
H = array(P.get_inequalities())
b, A = H[:, 0], H[:, 1:]
return (-A, b)
def cone_face_to_span(F):
"""
Compute the span matrix F^S of the face matrix F,
that is, a matrix such that
{F x <= 0} if and only if {x = F^S z, z >= 0}.
"""
b, A = zeros((F.shape[0], 1)), -F
# H-representation: A x + b >= 0
F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE)
F_cdd.rep_type = RepType.INEQUALITY
P = Polyhedron(F_cdd)
V = array(P.get_generators())
for i in xrange(V.shape[0]):
if V[i, 0] != 0: # 1 = vertex, 0 = ray
raise NotConeFace(F)
return V[:, 1:].T
def face_of_span(S):
"""
Returns the face matrix S^F of the span matrix S,
that is, a matrix such that
{x = S z, z >= 0} if and only if {S^F x <= 0}.
"""
V = hstack([zeros((S.shape[1], 1)), S.T])
# V-representation: first column is 0 for rays
V_cdd = Matrix(V, number_type=NUMBER_TYPE)
V_cdd.rep_type = RepType.GENERATOR
P = Polyhedron(V_cdd)
H = array(P.get_inequalities())
b, A = H[:, 0], H[:, 1:]
for i in xrange(H.shape[0]):
if b[i] != 0:
raise NotConeSpan(S)
return -A
def visualize_2D(list_of_polytopes, a=1.5):
"""
Given a polytope in its H-representation, plot it
"""
p_list = []
x_all = np.empty((0, 2))
for polytope in list_of_polytopes:
p_mat = Matrix(np.hstack((polytope.H, polytope.h)))
poly = Polyhedron(p_mat)
x = np.array(poly.get_generators())[:, 0:2]
x = x[ConvexHull(x).vertices, :]
x_all = np.vstack((x_all, x))
p = Polygon(x)
p_list.append(p)
p_patch = PatchCollection(p_list, color=[(np.random.random(),np.random.random(),np.tanh(np.random.random())) \
for polytope in list_of_polytopes],alpha=0.6)
fig, ax = plt.subplots()
ax.add_collection(p_patch)
ax.set_xlim([np.min(x_all[:, 0]) * a, a * np.max(x_all[:, 0])])
ax.set_ylim([np.min(x_all[:, 1]) * a, a * np.max(x_all[:, 1])])
ax.grid(color=(0, 0, 0), linestyle='--', linewidth=0.3)
def poly_face_to_span(F, f):
"""
Compute the span matrix F^S of the face matrix F,
that is, a matrix such that
{F x <= f} if and only if {x = F^S z, z >= 0, sum(z)=1}.
"""
b, A = f.reshape((F.shape[0], 1)), -F
# H-representation: A x + b >= 0
F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE)
F_cdd.rep_type = RepType.INEQUALITY
P = Polyhedron(F_cdd)
V = array(P.get_generators())
if (V.shape[0] == 0):
print "V.shape", V.shape, "F.shape", F.shape, "f.shape", f.shape
raise ValueError("This polytope seems to have no vertices")
for i in xrange(V.shape[0]):
if V[i, 0] != 1: # 1 = vertex, 0 = ray
raise NotPolyFace(F)
return V[:, 1:].T
def sample_knots(num_intervals: int,
knot_bounds: Union[np.ndarray, None] = None,
interval_sizes: Union[np.ndarray, None] = None,
num_samples: int = 1) -> Union[np.ndarray, None]:
"""Sample knots given a set of rules.
Args:
num_intervals
Number of intervals (number of knots minus 1).
knot_bounds
Bounds for the interior knots. Here we assume the domain span 0 to 1,
bound for a knot should be between 0 and 1, e.g. ``[0.1, 0.2]``.
``knot_bounds`` should have number of interior knots of rows, and each row
is a bound for corresponding knot, e.g.
``knot_bounds=np.array([[0.0, 0.2], [0.3, 0.4], [0.3, 1.0]])``,
for when we have three interior knots.
interval_sizes
Bounds for the distances between knots. For the same reason, we assume
elements in `interval_sizes` to be between 0 and 1. For example,
``interval_distances=np.array([[0.1, 0.2], [0.1, 0.3], [0.1, 0.5], [0.1, 0.5]])``
means that the distance between first (0) and second knot has to be between 0.1 and 0.2, etc.
And the number of rows for ``interval_sizes`` has to be same with ``num_intervals``.
num_samples
Number of knots samples.
Returns:
np.ndarray: Return knots sample as array, with `num_samples` rows and number of knots columns.
"""
# rename variables
k = num_intervals
b = knot_bounds
d = interval_sizes
N = num_samples
t0 = 0.0
tk = 1.0
# check input
assert t0 <= tk
assert k >= 2
if d is not None:
assert d.shape == (k, 2) and sum(d[:, 0]) <= 1.0 and\
np.all(d >= 0.0) and np.all(d <= 1.0)
else:
d = np.repeat(np.array([[0.0, 1.0]]), k, axis=0)
if b is not None:
assert b.shape == (k - 1, 2) and\
np.all(b[:, 0] <= b[:, 1]) and\
np.all(b[:-1, 1] <= b[1:, 1]) and\
np.all(b >= 0.0) and np.all(b <= 1.0)
else:
b = np.repeat(np.array([[0.0, 1.0]]), k - 1, axis=0)
d = d * (tk - t0)
b = b * (tk - t0) + t0
d[0] += t0
d[-1] -= tk
# find vertices of the polyhedron
D = -col_diff_mat(k - 1)
I = np.identity(k - 1)
A1 = np.vstack((-D, D))
A2 = np.vstack((-I, I))
b1 = np.hstack((-d[:, 0], d[:, 1]))
b2 = np.hstack((-b[:, 0], b[:, 1]))
A = np.vstack((A1, A2))
b = np.hstack((b1, b2))
mat = np.insert(-A, 0, b, axis=1)
mat = Matrix(mat)
mat.rep_type = RepType.INEQUALITY
poly = Polyhedron(mat)
ext = poly.get_generators()
vertices_and_rays = np.array(ext)
if vertices_and_rays.size == 0:
print('there is no feasible knots')
return None
if np.any(vertices_and_rays[:, 0] == 0.0):
print('polyhedron is not closed, something is wrong.')
return None
else:
vertices = vertices_and_rays[:, 1:]
# sample from the convex combination of the vertices
n = vertices.shape[0]
s_simplex = sample_simplex(n, N=N)
s = s_simplex.dot(vertices)
s = np.insert(s, 0, t0, axis=1)
s = np.insert(s, k, tk, axis=1)
return s