def intersect_line_cylinder(line, vertices):
"""
Intersect the line segment [p1, p2] with a vertical cylinder of polygonal
cross-section. If the intersection has two points, returns the one closest
to p1.
Parameters
----------
line : couple of (3,) arrays
End points of the 3D line segment.
vertices : list of (3,) arrays
Vertices of the polygon.
Returns
-------
inter_points : list of (3,) arrays
List of intersection points between the line segment and the cylinder.
"""
inter_points = []
inter_2d = intersect_line_polygon(line, vertices, apply_hull=True)
for p in inter_2d:
p1, p2 = array(line[0]), array(line[1])
alpha = norm(p - p1[:2]) / norm(p2[:2] - p1[:2])
z = p1[2] + alpha * (p2[2] - p1[2])
inter_points.append(array([p[0], p[1], z]))
return inter_points
def intersect_line_cylinder(p1, p2, points):
"""
Intersect the line segment [p1, p2] with a vertical cylinder of polygonal
cross-section. If the intersection has two points, returns the one closest
to p1.
Parameters
----------
p1 : array, shape=(3,)
End point of the line segment.
p2 : array, shape=(3,)
Other end point of the line segment.
points : list of arrays
2D vertices of the polygon.
Returns
-------
pi : array, shape=(3,), or None
Point closest to p1 if the intersection is not empty, None otherwise.
"""
p = intersect_line_polygon(p1, p2, points)
if p is None:
return None
p1 = array(p1)
p2 = array(p2)
alpha = norm(p - p1[:2]) / norm(p2[:2] - p1[:2])
z = p1[2] + alpha * (p2[2] - p1[2])
return array([p[0], p[1], z])
def draw_polygon(points,
normal,
combined='g-#',
color=None,
faces=None,
linewidth=1.,
pointsize=0.02):
"""
Draw a polygon defined as the convex hull of a set of points. The normal
vector n of the plane containing the polygon must also be supplied.
Parameters
----------
points : list of arrays
List of coplanar 3D points.
normal : array, shape=(3,)
Unit vector normal to the drawing plane.
combined : string, optional
Drawing spec in matplotlib fashion. Default: 'g-#'.
color : char or RGBA tuple
Color of the polygon.
faces : string
Faces of the polyhedron to draw. Use '.' for vertices, '-' for edges and
'#' for facets.
linewidth : scalar
Thickness of drawn line.
pointsize : scalar
Vertex size.
Returns
-------
handles : list of openravepy.GraphHandle
OpenRAVE graphical handles. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
assert abs(1. - norm(normal)) < 1e-10
n = normal
t = array([n[2] - n[1], n[0] - n[2], n[1] - n[0]], dtype=float)
t /= norm(t)
b = cross(n, t)
points2d = [[dot(t, x), dot(b, x)] for x in points]
try:
hull = ConvexHull(points2d)
except QhullError:
warn("QhullError: maybe polygon is empty?")
return []
except IndexError:
warn("Qhull raised an IndexError for points2d=%s" % repr(points2d))
return []
return draw_polyhedron(points,
combined,
color,
faces,
linewidth,
pointsize,
hull=hull)
def sqrtm(A, disp=True):
"""Matrix square root.
Parameters
----------
A : array, shape(M,M)
Matrix whose square root to evaluate
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
sgnA : array, shape(M,M)
Value of the sign function at A
(if disp == False)
errest : float
Frobenius norm of the estimated error, ||err||_F / ||A||_F
Notes
-----
Uses algorithm by Nicholas J. Higham
"""
A = asarray(A)
if len(A.shape)!=2:
raise ValueError("Non-matrix input to matrix function.")
T, Z = schur(A)
T, Z = rsf2csf(T,Z)
n,n = T.shape
R = np.zeros((n,n),T.dtype.char)
for j in range(n):
R[j,j] = sqrt(T[j,j])
for i in range(j-1,-1,-1):
s = 0
for k in range(i+1,j):
s = s + R[i,k]*R[k,j]
R[i,j] = (T[i,j] - s)/(R[i,i] + R[j,j])
R, Z = all_mat(R,Z)
X = (Z * R * Z.H)
if disp:
nzeig = np.any(diag(T)==0)
if nzeig:
print "Matrix is singular and may not have a square root."
return X.A
else:
arg2 = norm(X*X - A,'fro')**2 / norm(A,'fro')
return X.A, arg2
def sqrtm(A, disp=True):
"""Matrix square root.
Parameters
----------
A : array, shape(M,M)
Matrix whose square root to evaluate
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
sgnA : array, shape(M,M)
Value of the sign function at A
(if disp == False)
errest : float
Frobenius norm of the estimated error, ||err||_F / ||A||_F
Notes
-----
Uses algorithm by Nicholas J. Higham
"""
A = asarray(A)
if len(A.shape)!=2:
raise ValueError("Non-matrix input to matrix function.")
T, Z = schur(A)
T, Z = rsf2csf(T,Z)
n,n = T.shape
R = np.zeros((n,n),T.dtype.char)
for j in range(n):
R[j,j] = sqrt(T[j,j])
for i in range(j-1,-1,-1):
s = 0
for k in range(i+1,j):
s = s + R[i,k]*R[k,j]
R[i,j] = (T[i,j] - s)/(R[i,i] + R[j,j])
R, Z = all_mat(R,Z)
X = (Z * R * Z.H)
if disp:
nzeig = np.any(diag(T)==0)
if nzeig:
print "Matrix is singular and may not have a square root."
return X.A
else:
arg2 = norm(X*X - A,'fro')**2 / norm(A,'fro')
return X.A, arg2
def dist(self, other_point):
"""
Distance to another point.
Parameters
----------
other_point : array or Point
Point to compute the distance to.
"""
if isinstance(other_point, list):
other_point = array(other_point)
if isinstance(other_point, ndarray):
return norm(other_point - self.p)
return norm(other_point.p - self.p)
def expm(A, q=False):
"""Compute the matrix exponential using Pade approximation.
Parameters
----------
A : array, shape(M,M)
Matrix to be exponentiated
Returns
-------
expA : array, shape(M,M)
Matrix exponential of A
References
----------
N. J. Higham,
"The Scaling and Squaring Method for the Matrix Exponential Revisited",
SIAM. J. Matrix Anal. & Appl. 26, 1179 (2005).
"""
if q: warnings.warn("argument q=... in scipy.linalg.expm is deprecated.")
A = asarray(A)
A_L1 = norm(A,1)
n_squarings = 0
if A.dtype == 'float64' or A.dtype == 'complex128':
if A_L1 < 1.495585217958292e-002:
U,V = _pade3(A)
elif A_L1 < 2.539398330063230e-001:
U,V = _pade5(A)
elif A_L1 < 9.504178996162932e-001:
U,V = _pade7(A)
elif A_L1 < 2.097847961257068e+000:
U,V = _pade9(A)
else:
maxnorm = 5.371920351148152
n_squarings = max(0, int(ceil(log2(A_L1 / maxnorm))))
A = A / 2**n_squarings
U,V = _pade13(A)
elif A.dtype == 'float32' or A.dtype == 'complex64':
if A_L1 < 4.258730016922831e-001:
U,V = _pade3(A)
elif A_L1 < 1.880152677804762e+000:
U,V = _pade5(A)
else:
maxnorm = 3.925724783138660
n_squarings = max(0, int(ceil(log2(A_L1 / maxnorm))))
A = A / 2**n_squarings
U,V = _pade7(A)
else:
raise ValueError("invalid type: "+str(A.dtype))
P = U + V # p_m(A) : numerator
Q = -U + V # q_m(A) : denominator
R = solve(Q,P)
# squaring step to undo scaling
for i in range(n_squarings):
R = dot(R,R)
return R
def compute_chebyshev_center(A, b):
"""
Compute the Chebyshev center of a polyhedron, that is, the point furthest
away from all inequalities.
Parameters
----------
A : array, shape=(m, k)
Matrix of halfspace representation.
b : array, shape=(m,)
Vector of halfspace representation.
Returns
-------
z : array, shape=(k,)
Point further away from all inequalities.
Notes
-----
The Chebyshev center is discussed in [Boyd04]_, Section 4.3.1, p. 148.
"""
cost = zeros(A.shape[1] + 1)
cost[-1] = -1.
a_cheby = array([norm(A[i, :]) for i in xrange(A.shape[0])])
A_cheby = hstack([A, a_cheby.reshape((A.shape[0], 1))])
z = solve_lp(cost, A_cheby, b)
if z[-1] < -1e-1: # last coordinate is distance to boundaries
raise Exception("Polytope is empty (margin violation %.2f)" % z[-1])
return z[:-1]
def compute_chebyshev_center(A, b):
"""
Compute the Chebyshev center of a polyhedron, that is, the point
furthest away from all inequalities.
INPUT:
- ``A`` -- matrix of polytope H-representation
- ``b`` -- vector of polytope H-representation
OUTPUT:
A numpy array of shape ``(A.shape[1],)``.
REFERENCES:
Stephen Boyd and Lieven Vandenberghe, "Convex Optimization",
Section 4.3.1, p. 148.
"""
cost = zeros(A.shape[1] + 1)
cost[-1] = -1.
a_cheby = array([norm(A[i, :]) for i in xrange(A.shape[0])])
A_cheby = hstack([A, a_cheby.reshape((A.shape[0], 1))])
z = solve_lp(cost, A_cheby, b)
assert z[-1] > 0 # last coordinate is distance to boundaries
return z[:-1]
def face_of_span(S):
"""
Compute the face matrix F of the span matrix S, which is such that
{x = S z, z >= 0} if and only if {F x <= 0}.
"""
V = vstack([
hstack([zeros((S.shape[1], 1)), S.T]),
hstack([1, zeros(S.shape[0])])])
# V-representation: first column is 0 for rays
mat = cdd.Matrix(V, number_type='float')
mat.rep_type = cdd.RepType.GENERATOR
P = cdd.Polyhedron(mat)
ineq = P.get_inequalities()
H = array(ineq)
if H.shape == (0,): # H == []
return H
A = []
for i in xrange(H.shape[0]):
# H matrix is [b, -A] for A * x <= b
if norm(H[i, 1:]) < 1e-10:
continue
elif abs(H[i, 0]) > 1e-10: # b should be zero for a cone
raise Exception("Polyhedron is not a cone")
elif i not in ineq.lin_set:
A.append(-H[i, 1:])
return array(A)
def step(self, dt, unsafe=False):
"""
Apply velocities computed by inverse kinematics.
Parameters
----------
dt : scalar
Time step in [s].
unsafe : bool, optional
When set, use the faster but less numerically-stable method
implemented in :func:`pymanoid.ik.IKSolver.compute_velocity_fast`.
"""
q = self.robot.q
if unsafe or self.unsafe:
qd = self.compute_velocity_fast(dt)
else: # safe formulation is the default
qd = self.compute_velocity_safe(dt)
if self.verbosity >= 3:
print "\n TASK COST",
print "\n------------------------------"
for task in self.tasks.itervalues():
J = task.jacobian()
r = task.residual(dt)
print "%20s %.2e" % (task.name, norm(dot(J, qd) - r))
print ""
self.robot.set_dof_values(q + qd * dt, clamp=True)
self.robot.set_dof_velocities(qd)
def on_tick(self, sim):
if self.handle is None:
self.update_polygon()
for contact in self.contact_set.contacts:
if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10:
self.update_contact_poses()
self.update_polygon()
break
def rsf2csf(T, Z):
"""Convert real Schur form to complex Schur form.
Convert a quasi-diagonal real-valued Schur form to the upper triangular
complex-valued Schur form.
Parameters
----------
T : array, shape (M, M)
Real Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix
Returns
-------
T : array, shape (M, M)
Complex Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix corresponding to the complex form
See also
--------
schur : Schur decompose a matrix
"""
Z, T = map(asarray_chkfinite, (Z, T))
if len(Z.shape) != 2 or Z.shape[0] != Z.shape[1]:
raise ValueError("matrix must be square.")
if len(T.shape) != 2 or T.shape[0] != T.shape[1]:
raise ValueError("matrix must be square.")
if T.shape[0] != Z.shape[0]:
raise ValueError("matrices must be same dimension.")
N = T.shape[0]
arr = numpy.array
t = _commonType(Z, T, arr([3.0], 'F'))
Z, T = _castCopy(t, Z, T)
conj = numpy.conj
dot = numpy.dot
r_ = numpy.r_
transp = numpy.transpose
for m in range(N - 1, 0, -1):
if abs(T[m, m - 1]) > eps * (abs(T[m - 1, m - 1]) + abs(T[m, m])):
k = slice(m - 1, m + 1)
mu = eigvals(T[k, k]) - T[m, m]
r = misc.norm([mu[0], T[m, m - 1]])
c = mu[0] / r
s = T[m, m - 1] / r
G = r_[arr([[conj(c), s]], dtype=t), arr([[-s, c]], dtype=t)]
Gc = conj(transp(G))
j = slice(m - 1, N)
T[k, j] = dot(G, T[k, j])
i = slice(0, m + 1)
T[i, k] = dot(T[i, k], Gc)
i = slice(0, N)
Z[i, k] = dot(Z[i, k], Gc)
T[m, m - 1] = 0.0
return T, Z
def rsf2csf(T, Z):
"""Convert real Schur form to complex Schur form.
Convert a quasi-diagonal real-valued Schur form to the upper triangular
complex-valued Schur form.
Parameters
----------
T : array, shape (M, M)
Real Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix
Returns
-------
T : array, shape (M, M)
Complex Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix corresponding to the complex form
See also
--------
schur : Schur decompose a matrix
"""
Z, T = map(asarray_chkfinite, (Z, T))
if len(Z.shape) != 2 or Z.shape[0] != Z.shape[1]:
raise ValueError("matrix must be square.")
if len(T.shape) != 2 or T.shape[0] != T.shape[1]:
raise ValueError("matrix must be square.")
if T.shape[0] != Z.shape[0]:
raise ValueError("matrices must be same dimension.")
N = T.shape[0]
arr = numpy.array
t = _commonType(Z, T, arr([3.0],'F'))
Z, T = _castCopy(t, Z, T)
conj = numpy.conj
dot = numpy.dot
r_ = numpy.r_
transp = numpy.transpose
for m in range(N-1, 0, -1):
if abs(T[m,m-1]) > eps*(abs(T[m-1,m-1]) + abs(T[m,m])):
k = slice(m-1, m+1)
mu = eigvals(T[k,k]) - T[m,m]
r = misc.norm([mu[0], T[m,m-1]])
c = mu[0] / r
s = T[m,m-1] / r
G = r_[arr([[conj(c), s]], dtype=t), arr([[-s, c]], dtype=t)]
Gc = conj(transp(G))
j = slice(m-1, N)
T[k,j] = dot(G, T[k,j])
i = slice(0, m+1)
T[i,k] = dot(T[i,k], Gc)
i = slice(0, N)
Z[i,k] = dot(Z[i,k], Gc)
T[m,m-1] = 0.0;
return T, Z
def compute_static_equilibrium_polygon(self):
"""
Compute the halfspace and vertex representations of the
static-equilibrium polygon (SEP) of the stance.
"""
sep_vertices = super(Stance, self).compute_static_equilibrium_polygon()
self.sep_hrep = compute_polytope_hrep(sep_vertices)
self.sep_norm = array([norm(a) for a in self.sep_hrep[0]])
self.sep_vertices = sep_vertices
return sep_vertices
def draw_polygon(points, normal, combined='g-#', color=None, faces=None,
linewidth=1., pointsize=0.02):
"""
Draw a polygon defined as the convex hull of a set of points. The normal
vector n of the plane containing the polygon must also be supplied.
INPUT:
- ``points`` -- list of coplanar 3D points
- ``normal`` -- unit vector normal to the drawing plane
- ``combined`` -- (default: 'g-#') drawing spec in matplotlib fashion
- ``color`` -- color letter or RGBA tuple
- ``faces`` -- string indicating the faces of the polyhedron to draw
- ``linewidth`` -- (default: 1.) thickness of drawn line
- ``pointsize`` -- (default: 0.02) vertex size
OUTPUT:
And OpenRAVE handle. Must be stored in some variable, otherwise the drawn
object will vanish instantly.
"""
assert abs(1. - norm(normal)) < 1e-10
n = normal
t = array([n[2] - n[1], n[0] - n[2], n[1] - n[0]], dtype=float)
t /= norm(t)
b = cross(n, t)
points2d = [[dot(t, x), dot(b, x)] for x in points]
try:
hull = ConvexHull(points2d)
except QhullError:
warn("QhullError: maybe polygon is empty?")
return []
except IndexError:
warn("Qhull raised an IndexError for points2d=%s" % repr(points2d))
return []
return draw_polyhedron(
points, combined, color, faces, linewidth, pointsize, hull=hull)
def print_results(self):
"""
Print various statistics on NLP resolution.
"""
dcm_last = self.p_last + self.pd_last / self.omega
dcm_error = norm(dcm_last - self.dcm_target)
print "\n"
print "%14s: " % "Desired dur.", "%.3f s" % self.desired_duration
print "%14s: " % "Duration", "%.3f s" % self.duration
print "%14s: " % "DCM error", "%.3f cm" % (100 * dcm_error)
print "%14s: " % "Comp. time", "%.1f ms" % (1000 *
self.nlp.solve_time)
print "%14s: " % "Iter. count", self.nlp.iter_count
print "%14s: " % "Status", self.nlp.return_status
print "\n"
def force_rays(self):
"""
Rays (V-rep) of the force friction cone in world frame.
"""
if self.is_sliding:
mu = self.kinetic_friction / sqrt(2) # inner approximation
nv = norm(self.v)
vx, vy, _ = self.v
return dot(self.R, [-mu * vx / nv, -mu * vy / nv, +1])
else: # fixed contact mode
mu = self.static_friction / sqrt(2) # inner approximation
f1 = dot(self.R, [+mu, +mu, +1])
f2 = dot(self.R, [+mu, -mu, +1])
f3 = dot(self.R, [-mu, +mu, +1])
f4 = dot(self.R, [-mu, -mu, +1])
return [f1, f2, f3, f4]
def expm(A, q=7):
"""Compute the matrix exponential using Pade approximation.
Parameters
----------
A : array, shape(M,M)
Matrix to be exponentiated
q : integer
Order of the Pade approximation
Returns
-------
expA : array, shape(M,M)
Matrix exponential of A
"""
A = asarray(A)
# Scale A so that norm is < 1/2
nA = norm(A,Inf)
if nA==0:
return identity(len(A), A.dtype.char)
from numpy import log2
val = log2(nA)
e = int(floor(val))
j = max(0,e+1)
A = A / 2.0**j
# Pade Approximation for exp(A)
X = A
c = 1.0/2
N = eye(*A.shape) + c*A
D = eye(*A.shape) - c*A
for k in range(2,q+1):
c = c * (q-k+1) / (k*(2*q-k+1))
X = dot(A,X)
cX = c*X
N = N + cX
if not k % 2:
D = D + cX;
else:
D = D - cX;
F = solve(D,N)
for k in range(1,j+1):
F = dot(F,F)
return F
def expm(A, q=7):
"""Compute the matrix exponential using Pade approximation.
Parameters
----------
A : array, shape(M,M)
Matrix to be exponentiated
q : integer
Order of the Pade approximation
Returns
-------
expA : array, shape(M,M)
Matrix exponential of A
"""
A = asarray(A)
# Scale A so that norm is < 1/2
nA = norm(A, Inf)
if nA == 0:
return identity(len(A), A.dtype.char)
from numpy import log2
val = log2(nA)
e = int(floor(val))
j = max(0, e + 1)
A = A / 2.0**j
# Pade Approximation for exp(A)
X = A
c = 1.0 / 2
N = eye(*A.shape) + c * A
D = eye(*A.shape) - c * A
for k in range(2, q + 1):
c = c * (q - k + 1) / (k * (2 * q - k + 1))
X = dot(A, X)
cX = c * X
N = N + cX
if not k % 2:
D = D + cX
else:
D = D - cX
F = solve(D, N)
for k in range(1, j + 1):
F = dot(F, F)
return F
def logm(A, disp=True):
"""Compute matrix logarithm.
The matrix logarithm is the inverse of expm: expm(logm(A)) == A
Parameters
----------
A : array, shape(M,M)
Matrix whose logarithm to evaluate
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
logA : array, shape(M,M)
Matrix logarithm of A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
"""
# Compute using general funm but then use better error estimator and
# make one step in improving estimate using a rotation matrix.
A = mat(asarray(A))
F, errest = funm(A,log,disp=0)
errtol = 1000*eps
# Only iterate if estimate of error is too large.
if errest >= errtol:
# Use better approximation of error
errest = norm(expm(F)-A,1) / norm(A,1)
if not isfinite(errest) or errest >= errtol:
N,N = A.shape
X,Y = ogrid[1:N+1,1:N+1]
R = mat(orth(eye(N,dtype='d')+X+Y))
F, dontcare = funm(R*A*R.H,log,disp=0)
F = R.H*F*R
if (norm(imag(F),1)<=1000*errtol*norm(F,1)):
F = mat(real(F))
E = mat(expm(F))
temp = mat(solve(E.T,(E-A).T))
F = F - temp.T
errest = norm(expm(F)-A,1) / norm(A,1)
if disp:
if not isfinite(errest) or errest >= errtol:
print "Result may be inaccurate, approximate err =", errest
return F
else:
return F, errest
def logm(A, disp=True):
"""Compute matrix logarithm.
The matrix logarithm is the inverse of expm: expm(logm(A)) == A
Parameters
----------
A : array, shape(M,M)
Matrix whose logarithm to evaluate
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
logA : array, shape(M,M)
Matrix logarithm of A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
"""
# Compute using general funm but then use better error estimator and
# make one step in improving estimate using a rotation matrix.
A = mat(asarray(A))
F, errest = funm(A,log,disp=0)
errtol = 1000*eps
# Only iterate if estimate of error is too large.
if errest >= errtol:
# Use better approximation of error
errest = norm(expm(F)-A,1) / norm(A,1)
if not isfinite(errest) or errest >= errtol:
N,N = A.shape
X,Y = ogrid[1:N+1,1:N+1]
R = mat(orth(eye(N,dtype='d')+X+Y))
F, dontcare = funm(R*A*R.H,log,disp=0)
F = R.H*F*R
if (norm(imag(F),1)<=1000*errtol*norm(F,1)):
F = mat(real(F))
E = mat(expm(F))
temp = mat(solve(E.T,(E-A).T))
F = F - temp.T
errest = norm(expm(F)-A,1) / norm(A,1)
if disp:
if not isfinite(errest) or errest >= errtol:
print "Result may be inaccurate, approximate err =", errest
return F
else:
return F, errest
def force_face(self):
"""
Face (H-rep) of the force friction cone in world frame.
"""
if self.is_sliding:
mu = self.kinetic_friction / sqrt(2) # inner approximation
nv = norm(self.vel)
vx, vy, _ = self.vel
local_cone = array([
[-1, 0, -mu * vx / nv],
[+1, 0, +mu * vx / nv],
[0, -1, -mu * vy / nv],
[0, +1, -mu * vy / nv]])
else: # fixed contact mode
mu = self.static_friction / sqrt(2) # inner approximation
local_cone = array([
[-1, 0, -mu],
[+1, 0, -mu],
[0, -1, -mu],
[0, +1, -mu]])
return dot(local_cone, self.R.T)
def step_ik(self, dt, method='safe', verbose=False):
"""
Apply velocities computed by inverse kinematics.
Parameters
----------
dt : scalar
Time step in [s].
method : string, optional
Choice between 'fast' and 'safe' (default).
"""
qd = self.ik.compute_velocity(dt, method)
if verbose:
print "\n TASK COST",
print "\n------------------------------"
for task in self.ik.tasks.itervalues():
J = task.jacobian()
r = task.residual(dt)
print "%20s %.2e" % (task.name, norm(dot(J, qd) - r))
print ""
self.set_dof_values(self.q + qd * dt, clamp=True)
self.set_dof_velocities(qd)
def compute_cone_face_matrix(S):
"""
Compute the face matrix of a polyhedral convex cone from its span matrix.
Parameters
----------
S : array, shape=(n, m)
Span matrix defining the cone as :math:`x = S \\lambda` with
:math:`\\lambda \\geq 0`.
Returns
-------
F : array, shape=(k, n)
Face matrix defining the cone equivalently by :math:`F x \\leq 0`.
"""
V = vstack([
hstack([zeros((S.shape[1], 1)), S.T]),
hstack([1, zeros(S.shape[0])])
])
# V-representation: first column is 0 for rays
mat = cdd.Matrix(V, number_type='float')
mat.rep_type = cdd.RepType.GENERATOR
P = cdd.Polyhedron(mat)
ineq = P.get_inequalities()
H = array(ineq)
if H.shape == (0, ): # H == []
return H
A = []
for i in xrange(H.shape[0]):
# H matrix is [b, -A] for A * x <= b
if norm(H[i, 1:]) < 1e-10:
continue
elif abs(H[i, 0]) > 1e-10: # b should be zero for a cone
raise Exception("Polyhedron is not a cone")
elif i not in ineq.lin_set:
A.append(-H[i, 1:])
return array(A)
def __init__(self,
com,
left_foot=None,
right_foot=None,
left_hand=None,
right_hand=None,
label=None,
duration=None):
contacts = filter(None, [left_foot, right_foot, left_hand, right_hand])
super(Stance, self).__init__(contacts)
if not issubclass(type(com), Point):
com = Point(com, visible=False)
self.com = com
self.duration = duration
self.label = label
self.left_foot = left_foot
self.left_hand = left_hand
self.right_foot = right_foot
self.right_hand = right_hand
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
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
def signm(a, disp=True):
"""Matrix sign function.
Extension of the scalar sign(x) to matrices.
Parameters
----------
A : array, shape(M,M)
Matrix at which to evaluate the sign function
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
sgnA : array, shape(M,M)
Value of the sign function at A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
Examples
--------
>>> from scipy.linalg import signm, eigvals
>>> a = [[1,2,3], [1,2,1], [1,1,1]]
>>> eigvals(a)
array([ 4.12488542+0.j, -0.76155718+0.j, 0.63667176+0.j])
>>> eigvals(signm(a))
array([-1.+0.j, 1.+0.j, 1.+0.j])
"""
def rounded_sign(x):
rx = real(x)
if rx.dtype.char=='f':
c = 1e3*feps*amax(x)
else:
c = 1e3*eps*amax(x)
return sign( (absolute(rx) > c) * rx )
result,errest = funm(a, rounded_sign, disp=0)
errtol = {0:1e3*feps, 1:1e3*eps}[_array_precision[result.dtype.char]]
if errest < errtol:
return result
# Handle signm of defective matrices:
# See "E.D.Denman and J.Leyva-Ramos, Appl.Math.Comp.,
# 8:237-250,1981" for how to improve the following (currently a
# rather naive) iteration process:
a = asarray(a)
#a = result # sometimes iteration converges faster but where??
# Shifting to avoid zero eigenvalues. How to ensure that shifting does
# not change the spectrum too much?
vals = svd(a,compute_uv=0)
max_sv = np.amax(vals)
#min_nonzero_sv = vals[(vals>max_sv*errtol).tolist().count(1)-1]
#c = 0.5/min_nonzero_sv
c = 0.5/max_sv
S0 = a + c*np.identity(a.shape[0])
prev_errest = errest
for i in range(100):
iS0 = inv(S0)
S0 = 0.5*(S0 + iS0)
Pp=0.5*(dot(S0,S0)+S0)
errest = norm(dot(Pp,Pp)-Pp,1)
if errest < errtol or prev_errest==errest:
break
prev_errest = errest
if disp:
if not isfinite(errest) or errest >= errtol:
print "Result may be inaccurate, approximate err =", errest
return S0
else:
return S0, errest
def rsf2csf(T, Z, check_finite=True):
"""Convert real Schur form to complex Schur form.
Convert a quasi-diagonal real-valued Schur form to the upper triangular
complex-valued Schur form.
Parameters
----------
T : array, shape (M, M)
Real Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix
check_finite : boolean, optional
Whether to check the input matrixes contain only finite numbers.
Disabling may give a performance gain, but may result to problems
(crashes, non-termination) if the inputs do contain infinities or NaNs.
Returns
-------
T : array, shape (M, M)
Complex Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix corresponding to the complex form
See also
--------
schur : Schur decompose a matrix
"""
if check_finite:
Z, T = map(asarray_chkfinite, (Z, T))
else:
Z,T = map(asarray, (Z,T))
if len(Z.shape) != 2 or Z.shape[0] != Z.shape[1]:
raise ValueError("matrix must be square.")
if len(T.shape) != 2 or T.shape[0] != T.shape[1]:
raise ValueError("matrix must be square.")
if T.shape[0] != Z.shape[0]:
raise ValueError("matrices must be same dimension.")
N = T.shape[0]
arr = numpy.array
t = _commonType(Z, T, arr([3.0],'F'))
Z, T = _castCopy(t, Z, T)
conj = numpy.conj
dot = numpy.dot
r_ = numpy.r_
transp = numpy.transpose
for m in range(N-1, 0, -1):
if abs(T[m,m-1]) > eps*(abs(T[m-1,m-1]) + abs(T[m,m])):
k = slice(m-1, m+1)
mu = eigvals(T[k,k]) - T[m,m]
r = misc.norm([mu[0], T[m,m-1]])
c = mu[0] / r
s = T[m,m-1] / r
G = r_[arr([[conj(c), s]], dtype=t), arr([[-s, c]], dtype=t)]
Gc = conj(transp(G))
j = slice(m-1, N)
T[k,j] = dot(G, T[k,j])
i = slice(0, m+1)
T[i,k] = dot(T[i,k], Gc)
i = slice(0, N)
Z[i,k] = dot(Z[i,k], Gc)
T[m,m-1] = 0.0;
return T, Z
def funm(A, func, disp=True):
"""Evaluate a matrix function specified by a callable.
Returns the value of matrix-valued function f at A. The function f
is an extension of the scalar-valued function func to matrices.
Parameters
----------
A : array, shape(M,M)
Matrix at which to evaluate the function
func : callable
Callable object that evaluates a scalar function f.
Must be vectorized (eg. using vectorize).
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
fA : array, shape(M,M)
Value of the matrix function specified by func evaluated at A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
"""
# Perform Shur decomposition (lapack ?gees)
A = asarray(A)
if len(A.shape)!=2:
raise ValueError("Non-matrix input to matrix function.")
if A.dtype.char in ['F', 'D', 'G']:
cmplx_type = 1
else:
cmplx_type = 0
T, Z = schur(A)
T, Z = rsf2csf(T,Z)
n,n = T.shape
F = diag(func(diag(T))) # apply function to diagonal elements
F = F.astype(T.dtype.char) # e.g. when F is real but T is complex
minden = abs(T[0,0])
# implement Algorithm 11.1.1 from Golub and Van Loan
# "matrix Computations."
for p in range(1,n):
for i in range(1,n-p+1):
j = i + p
s = T[i-1,j-1] * (F[j-1,j-1] - F[i-1,i-1])
ksl = slice(i,j-1)
val = dot(T[i-1,ksl],F[ksl,j-1]) - dot(F[i-1,ksl],T[ksl,j-1])
s = s + val
den = T[j-1,j-1] - T[i-1,i-1]
if den != 0.0:
s = s / den
F[i-1,j-1] = s
minden = min(minden,abs(den))
F = dot(dot(Z, F),transpose(conjugate(Z)))
if not cmplx_type:
F = toreal(F)
tol = {0:feps, 1:eps}[_array_precision[F.dtype.char]]
if minden == 0.0:
minden = tol
err = min(1, max(tol,(tol/minden)*norm(triu(T,1),1)))
if product(ravel(logical_not(isfinite(F))),axis=0):
err = Inf
if disp:
if err > 1000*tol:
print "Result may be inaccurate, approximate err =", err
return F
else:
return F, err
# estimating transition matrix
DIM = 5
trans0 = np.abs(np.random.randn(DIM, DIM))
trans0 /= trans0.sum(axis=1).reshape(-1, 1)
print(trans0, '\n')
pi0 = np.abs(np.random.randn(DIM, 1))
pi0 /= pi0.sum()
trans, errors = proj_grad(trans0, pi0)
plt.figure()
plt.plot(np.log(errors))
plt.show()
print('trans', trans)
print(trans.T @ pi0 - pi0, trans.sum(axis=1), norm(trans0 - trans), '\n')
# comparing repartition functions
N_THROWS = 10_000
xxx_0, yyy_0 = get_cdf(np.random.randn(N_THROWS))
xxx_1, yyy_1 = get_cdf(np.random.randn(N_THROWS))
plt.figure()
plt.plot(xxx_0, yyy_0)
plt.show()
print('k-dist', dist_cdfs(xxx_0, yyy_0, xxx_1, yyy_1))
def signm(a, disp=True):
"""Matrix sign function.
Extension of the scalar sign(x) to matrices.
Parameters
----------
A : array, shape(M,M)
Matrix at which to evaluate the sign function
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
sgnA : array, shape(M,M)
Value of the sign function at A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
Examples
--------
>>> from scipy.linalg import signm, eigvals
>>> a = [[1,2,3], [1,2,1], [1,1,1]]
>>> eigvals(a)
array([ 4.12488542+0.j, -0.76155718+0.j, 0.63667176+0.j])
>>> eigvals(signm(a))
array([-1.+0.j, 1.+0.j, 1.+0.j])
"""
def rounded_sign(x):
rx = real(x)
if rx.dtype.char=='f':
c = 1e3*feps*amax(x)
else:
c = 1e3*eps*amax(x)
return sign( (absolute(rx) > c) * rx )
result,errest = funm(a, rounded_sign, disp=0)
errtol = {0:1e3*feps, 1:1e3*eps}[_array_precision[result.dtype.char]]
if errest < errtol:
return result
# Handle signm of defective matrices:
# See "E.D.Denman and J.Leyva-Ramos, Appl.Math.Comp.,
# 8:237-250,1981" for how to improve the following (currently a
# rather naive) iteration process:
a = asarray(a)
#a = result # sometimes iteration converges faster but where??
# Shifting to avoid zero eigenvalues. How to ensure that shifting does
# not change the spectrum too much?
vals = svd(a,compute_uv=0)
max_sv = np.amax(vals)
#min_nonzero_sv = vals[(vals>max_sv*errtol).tolist().count(1)-1]
#c = 0.5/min_nonzero_sv
c = 0.5/max_sv
S0 = a + c*np.identity(a.shape[0])
prev_errest = errest
for i in range(100):
iS0 = inv(S0)
S0 = 0.5*(S0 + iS0)
Pp=0.5*(dot(S0,S0)+S0)
errest = norm(dot(Pp,Pp)-Pp,1)
if errest < errtol or prev_errest==errest:
break
prev_errest = errest
if disp:
if not isfinite(errest) or errest >= errtol:
print "Result may be inaccurate, approximate err =", errest
return S0
else:
return S0, errest
def rsf2csf(T, Z, check_finite=True):
"""Convert real Schur form to complex Schur form.
Convert a quasi-diagonal real-valued Schur form to the upper triangular
complex-valued Schur form.
Parameters
----------
T : array, shape (M, M)
Real Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix
check_finite : boolean, optional
Whether to check the input matrixes contain only finite numbers.
Disabling may give a performance gain, but may result to problems
(crashes, non-termination) if the inputs do contain infinities or NaNs.
Returns
-------
T : array, shape (M, M)
Complex Schur form of the original matrix
Z : array, shape (M, M)
Schur transformation matrix corresponding to the complex form
See also
--------
schur : Schur decompose a matrix
"""
if check_finite:
Z, T = map(asarray_chkfinite, (Z, T))
else:
Z, T = map(asarray, (Z, T))
if len(Z.shape) != 2 or Z.shape[0] != Z.shape[1]:
raise ValueError("matrix must be square.")
if len(T.shape) != 2 or T.shape[0] != T.shape[1]:
raise ValueError("matrix must be square.")
if T.shape[0] != Z.shape[0]:
raise ValueError("matrices must be same dimension.")
N = T.shape[0]
arr = numpy.array
t = _commonType(Z, T, arr([3.0], 'F'))
Z, T = _castCopy(t, Z, T)
conj = numpy.conj
dot = numpy.dot
r_ = numpy.r_
transp = numpy.transpose
for m in range(N - 1, 0, -1):
if abs(T[m, m - 1]) > eps * (abs(T[m - 1, m - 1]) + abs(T[m, m])):
k = slice(m - 1, m + 1)
mu = eigvals(T[k, k]) - T[m, m]
r = misc.norm([mu[0], T[m, m - 1]])
c = mu[0] / r
s = T[m, m - 1] / r
G = r_[arr([[conj(c), s]], dtype=t), arr([[-s, c]], dtype=t)]
Gc = conj(transp(G))
j = slice(m - 1, N)
T[k, j] = dot(G, T[k, j])
i = slice(0, m + 1)
T[i, k] = dot(T[i, k], Gc)
i = slice(0, N)
Z[i, k] = dot(Z[i, k], Gc)
T[m, m - 1] = 0.0
return T, Z
def funm(A, func, disp=True):
"""Evaluate a matrix function specified by a callable.
Returns the value of matrix-valued function f at A. The function f
is an extension of the scalar-valued function func to matrices.
Parameters
----------
A : array, shape(M,M)
Matrix at which to evaluate the function
func : callable
Callable object that evaluates a scalar function f.
Must be vectorized (eg. using vectorize).
disp : boolean
Print warning if error in the result is estimated large
instead of returning estimated error. (Default: True)
Returns
-------
fA : array, shape(M,M)
Value of the matrix function specified by func evaluated at A
(if disp == False)
errest : float
1-norm of the estimated error, ||err||_1 / ||A||_1
"""
# Perform Shur decomposition (lapack ?gees)
A = asarray(A)
if len(A.shape)!=2:
raise ValueError("Non-matrix input to matrix function.")
if A.dtype.char in ['F', 'D', 'G']:
cmplx_type = 1
else:
cmplx_type = 0
T, Z = schur(A)
T, Z = rsf2csf(T,Z)
n,n = T.shape
F = diag(func(diag(T))) # apply function to diagonal elements
F = F.astype(T.dtype.char) # e.g. when F is real but T is complex
minden = abs(T[0,0])
# implement Algorithm 11.1.1 from Golub and Van Loan
# "matrix Computations."
for p in range(1,n):
for i in range(1,n-p+1):
j = i + p
s = T[i-1,j-1] * (F[j-1,j-1] - F[i-1,i-1])
ksl = slice(i,j-1)
val = dot(T[i-1,ksl],F[ksl,j-1]) - dot(F[i-1,ksl],T[ksl,j-1])
s = s + val
den = T[j-1,j-1] - T[i-1,i-1]
if den != 0.0:
s = s / den
F[i-1,j-1] = s
minden = min(minden,abs(den))
F = dot(dot(Z, F),transpose(conjugate(Z)))
if not cmplx_type:
F = toreal(F)
tol = {0:feps, 1:eps}[_array_precision[F.dtype.char]]
if minden == 0.0:
minden = tol
err = min(1, max(tol,(tol/minden)*norm(triu(T,1),1)))
if product(ravel(logical_not(isfinite(F))),axis=0):
err = Inf
if disp:
if err > 1000*tol:
print "Result may be inaccurate, approximate err =", err
return F
else:
return F, err
def on_tick(self, sim):
super(ZMPSupportAreaDrawer, self).on_tick(sim)
if norm(self.stance.com.p - self.last_com) > 1e-10:
self.update_contact_poses()
self.update_polygon()
self.last_com = self.stance.com.p
misc.mkdir(folder_main)
misc.mkdir('{}/py'.format(folder_main))
misc.mkdir('{}/logs'.format(folder_main))
# load real data and convert to odl
file_data = '{}/data_{}.npy'.format(folder_data, data_suffix)
(data, background, factors, image, image_mr, image_ct) = np.load(file_data)
Y = mMR.operator_mmr().range
data = Y.element(data)
background = Y.element(background)
factors = Y.element(factors)
# define operator
K = mMR.operator_mmr(factors=factors)
X = K.domain
norm_K = misc.norm(K, '{}/norm_1subset.npy'.format(folder_norms))
KL = misc.kullback_leibler(Y, data, background)
for alpha in alphas:
print('<<< <<< alpha = {}'.format(alpha))
folder_param = '{}/alpha{:.2g}'.format(folder_main, alpha)
misc.mkdir(folder_param)
misc.mkdir('{}/pics'.format(folder_param))
folder_today = '{}/nepochs{}'.format(folder_param, nepoch)
misc.mkdir(folder_today)
misc.mkdir('{}/npy'.format(folder_today))
misc.mkdir('{}/pics'.format(folder_today))
misc.mkdir('{}/figs'.format(folder_today))
def _compute_dotproduct_4d(data, spacing, c):
#compute dot product
dot_deep = (
data[:, :, :-1, :, 1] * data[:, :, 1:, :, 1] +
data[:, :, :-1, :, 2] * data[:, :, 1:, :, 2] +
data[:, :, :-1, :, 3] * data[:, :, 1:, :, 3]).ravel() / spacing[2]
dot_right = (
data[:, :-1, :, :, 1] * data[:, 1:, :, :, 1] +
data[:, :-1, :, :, 2] * data[:, 1:, :, :, 2] +
data[:, :-1, :, :, 3] * data[:, 1:, :, :, 3]).ravel() / spacing[1]
dot_down = (
data[:-1, :, :, :, 1] * data[1:, :, :, :, 1] +
data[:-1, :, :, :, 2] * data[1:, :, :, :, 2] +
data[:-1, :, :, :, 3] * data[1:, :, :, :, 3]).ravel() / spacing[0]
dot_time = (
data[:, :, :, :-1, 1] * data[:, :, :, 1:, 1] +
data[:, :, :, :-1, 2] * data[1, :, :, 1:, 2] +
data[:, :, :, :-1, 3] * data[:, :, :, 1:, 3]).ravel() / spacing[3]
#calculate the magnitude
dot_deep_magn = ((misc.norm(data[:, :, :-1, :, 1], data[:, :, :-1, :, 2],
data[:, :, :-1, :, 3])).ravel() *
(misc.norm(data[:, :, 1:, :, 1], data[:, :, 1:, :, 2],
data[:, :, 1:, :, 3])).ravel())
dot_right_magn = ((misc.norm(data[:, :-1, :, :, 1], data[:, :-1, :, :, 2],
data[:, :-1, :, :, 3])).ravel() *
(misc.norm(data[:, 1:, :, :, 1], data[:, 1:, :, :, 2],
data[:, 1:, :, :, 3])).ravel())
dot_down_magn = ((misc.norm(data[:-1, :, :, :, 1], data[:-1, :, :, :, 2],
data[:-1, :, :, :, 3])).ravel() *
(misc.norm(data[1:, :, :, :, 1], data[1:, :, :, :, 2],
data[1:, :, :, :, 3])).ravel())
dot_time_magn = ((misc.norm(data[:, :, :, :-1, 1], data[:, :, :, :-1, 2],
data[:, :, :, :-1, 3])).ravel() *
(misc.norm(data[:, :, :, 1:, 1], data[:, :, :, 1:, 2],
data[:, :, :, 1:, 3])).ravel())
# add residual to avoid dividing by zero
eps = 1e-6
dot_deep_magn += eps
dot_right_magn += eps
dot_down_magn += eps
dot_time_magn += eps
# normalize the dot products
dot_deep_norm = dot_deep / dot_deep_magn
dot_right_norm = dot_right / dot_right_magn
dot_down_norm = dot_down / dot_down_magn
dot_time_norm = dot_time / dot_time_magn
dot_deep_norm = -(dot_deep_norm - 1)
dot_right_norm = -(dot_right_norm - 1)
dot_down_norm = -(dot_down_norm - 1)
dot_time_norm = -(dot_time_norm - 1)
dot_deep_norm *= c
dot_right_norm *= c
dot_down_norm *= c
dot_time_norm *= c
return np.r_[dot_deep_norm, dot_right_norm, dot_down_norm, dot_time_norm]
cb(x)
if alg.startswith('SPDHG') or alg.startswith('PDHG'):
g = odl.solvers.functional.IndicatorBox(X, lower=X.zero())
if alg.startswith('MLEM'):
misc.MLEM(x, KL.data, KL.background, K, niter, callback=cb)
elif alg.startswith('OSEM'):
misc.OSEM(x, KLs.data, KLs.background, Ks, niter, callback=cb)
elif alg.startswith('COSEM'):
misc.COSEM(x, KLs.data, KLs.background, Ks, niter, callback=cb)
elif alg.startswith('PDHG1'):
norm_K = misc.norm(K,
'{}/norm_1subset.npy'.format(folder_norms))
sigma = rho / norm_K
tau = rho / norm_K
f = KL
A = K
pdhg(x, f, g, A, tau, sigma, niter, callback=cb)
elif alg.startswith('SPDHG1'):
norm_K = misc.norms(
Ks, '{}/norm_{}subsets.npy'.format(folder_norms,
nsub[alg]))
sigma = [rho / nk for nk in norm_K]
tau = rho / (len(Ks) * max(norm_K))
f = KLs
A = Ks
def on_tick(self, sim):
for (k, c) in self.contact_dict.iteritems():
if norm(c.pose - self.contact_poses[k]) > 1e-10:
self.update_contact_poses()
self.update_polygon()
break
def compute_stability_criteria(self):
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
def on_tick(self, sim):
super(ZMPSupportAreaDrawer, self).on_tick(sim)
if norm(self.stance.com.p - self.last_com) > 1e-10:
self.update_contact_poses()
self.update_polygon()
self.last_com = self.stance.com.p
