def projectedNVertices(self,p):
# dimension of the projected polytope
A = -1 * self.A_ineq
b = -1 * self.b_ineq
C = self.A_eq
d = self.b_eq
ineq = (A, b) # A * x <= b
eq = (C, d) # C * x == d
n = len(A[0]) # dimension of the original polytope
# dimension of the projected polytope
# Projection is proj(x) = [x_0 x_1]
E = np.zeros((p, n))
E[0, 0] = 1.
E[1, 1] = 1.
f = np.zeros(p)
proj = (E, f) # proj(x) = E * x + f
#
vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl')
verts= np.asmatrix(vertices)
hull = ConvexHull(vertices, qhull_options="QJ")
faces = hull.simplices
print ("verts:")
print (verts)
print ("faces:")
print (faces)
ax = self.ax
ax.dist=10
ax.azim=30
ax.elev=10
ax.set_xlim([0,1])
ax.set_ylim([0,1])
ax.set_zlim([0,1])
for s in faces:
sq = [
[verts[s[0], 0], verts[s[0], 1]],
[verts[s[1], 0], verts[s[1], 1]]
]
plt.plot([sq])
#f = a3.art3d.Poly3DCollection([sq])#, alpha=0.1, linewidths=1)
#f.set_color(colors.rgb2hex(sp.rand(3)))
#f.set_edgecolor('k')
#f.set_facecolor('Red')
#ax.add_collection3d(f)
#ax.set_axis_off()
plt.show()
return verts
def plotConvexHull(self, colour=(sp.rand(),sp.rand(),sp.rand(), 0.1)):
A = -1 * self.A_ineq
b = -1 * self.b_ineq
C = self.A_eq
d = self.b_eq
ineq = (A, b) # A * x <= b
eq = (C, d) # C * x == d
n = len(A[0]) # dimension of the original polytope
p = 3 # dimension of the projected polytope
# Projection is proj(x) = [x_0 x_1]
E = np.zeros((p, n))
E[0, 0] = 1.
E[1, 1] = 1.
E[2, 2] = 1.
f = np.zeros(p)
proj = (E, f) # proj(x) = E * x + f
#
vertices = pypoman.project_polytope(proj, ineq, eq, method='cdd')
verts= np.around(np.array(vertices),4)
#verts = np.around(self.vertices(), 4) #np.matrix([[ 0.4, 0.0, 0.2, 0.4], [1.0, 0.0, 0.0, 0.0] , [ 0.25, 0.5, 0.0, 0.25], [ 0.222, 0.444, 0.111, 0.222],[ 0.0, 0.0, 0.0, 1.0]])
hull = ConvexHull(verts, qhull_options="QJ")
faces = hull.simplices
ax = self.ax
ax.dist=10
ax.azim=30
ax.elev=10
ax.set_xlim([0,1])
ax.set_ylim([0,1])
ax.set_zlim([0,1])
for s in faces:
sq = [
[verts[s[0], 0], verts[s[0], 1], verts[s[0], 2]],
[verts[s[1], 0], verts[s[1], 1], verts[s[1], 2]],
[verts[s[2], 0], verts[s[2], 1], verts[s[2], 2]]
]
f = a3.art3d.Poly3DCollection([sq])#, alpha=0.1, linewidths=1)
f.set_color(colors.rgb2hex(sp.rand(3)))
f.set_edgecolor('k')
f.set_facecolor(colour)
ax.add_collection3d(f)
#ax.set_axis_off()
plt.show()
def iterative_projection_bretl(self,
iterative_projection_params,
saturate_normal_force=False):
start_t_IP = time.time()
# print stanceLegs, contacts, normals, comWF, ng, mu, saturate_normal_force
proj, self.eq, self.ineq, actuation_polygons, isIKoutOfWorkSpace = self.setup_iterative_projection(
iterative_projection_params, saturate_normal_force)
if isIKoutOfWorkSpace:
return False, False, False
else:
vertices_WF = pypoman.project_polytope(proj,
self.ineq,
self.eq,
method='bretl',
max_iter=500,
init_angle=0.0)
if vertices_WF is False:
print 'Project polytope function is False'
return False, False, False
else:
compressed_vertices = np.compress([True, True],
vertices_WF,
axis=1)
try:
hull = ConvexHull(compressed_vertices)
except Exception as err:
print("QHull type error: " + str(err))
print("matrix to compute qhull:", compressed_vertices)
return False, False, False
# print 'hull ', hull.vertices
compressed_hull = compressed_vertices[hull.vertices]
compressed_hull = self.geom.clockwise_sort(compressed_hull)
compressed_hull = compressed_hull
# print compressed_hull
#vertices_WF = vertices_BF + np.transpose(comWF[0:2])
computation_time = (time.time() - start_t_IP)
#print("Iterative Projection (Bretl): --- %s seconds ---" % computation_time)
# print np.size(actuation_polygons,0), np.size(actuation_polygons,1), actuation_polygons
if np.size(actuation_polygons, 0) is 4:
if np.size(actuation_polygons, 1) is 3:
# print actuation_polygons
p = self.reorganizeActuationPolytopes(actuation_polygons[1])
return compressed_hull, actuation_polygons, computation_time
def preAB(self, A, B, Fu, bu):
n = A.shape[1]
d = B.shape[1]
# Original polytope:
F1 = np.hstack((np.dot(self.F, A), np.dot(self.F, B)))
b1 = self.b
F2 = np.hstack((np.zeros((Fu.shape[0], n)), Fu))
b2 = bu
ineq = (np.vstack((F1, F2)), np.hstack(
(b1, b2))) # A * x + Bu <= b, F_u u <= bu
# Projection is proj(x) = [x_0 x_1]
E = np.zeros((n, n + d))
E[0:n, 0:n] = np.eye(n)
f = np.zeros(n)
proj = (E, f) # proj(x) = E * x + f
vertices = pypoman.project_polytope(proj,
ineq) #, eq=None, method='bretl')
F, b = pypoman.duality.compute_polytope_halfspaces(vertices)
return F, b
def projected3DVertices(self):
A = -1 * self.A_ineq
b = self.b_ineq
C = self.A_eq
d = self.b_eq
ineq = (A, b) # A * x <= b
eq = (C, d) # C * x == d
n = len(A[0]) # dimension of the original polytope
p = 3 # dimension of the projected polytope
# Projection is proj(x) = [x_0 x_1]
E = np.zeros((p, n))
E[0, 0] = 1.
E[1, 1] = 1.
E[2, 2] = 1.
f = np.zeros(p)
proj = (E, f) # proj(x) = E * x + f
#
vertices = pypoman.project_polytope(proj, ineq, eq, method='cdd')
verts= np.around(np.array(vertices),4)
return verts
C4 = array([[-0.75, -0.75, -mu * 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., -0.75, -0.75, -mu * 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., -0.75, -0.75, -mu * 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., -0.75, -0.75, -mu * 1.]])
# concatenate all the inequality constraints above
C = vstack([C1, C2, C3, C4])
print(C)
# vector of known coefficients
b = zeros((m_ineq, 1)).reshape(m_ineq)
print(b)
ineq = (C, b) # C * x <= b
# Solve the projection problem
vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl')
pylab.ion()
pylab.figure()
print(vertices)
pypoman.plot_polygon(vertices)
# Project Tau_0 into CoM coordinates as in Eq. 53
# p_g = (n/mg) x tau_0 + z_g*n
n = array([0., 0., 1. / (mass * g)])
points = []
for j in range(0, len(vertices)):
vx = vertices[j][0]
vy = vertices[j][1]
tau = array([vx, vy, 0.])
p = np.cross(n, tau)
points.append([p[0], p[1]])
#print points
def compute_local_actuation_dependent_polygon(robot, contacts, method="bretl"):
"""
Compute constraint matrices of the problem:
A * w_all <= b
C * w_all == d
and output variables are given by:
[x_com y_com] = E * w_all + f
where w_all is the stacked vector of external contact wrenches.
Parameters
----------
robot : pymanoid.Robot
Robot model.
contacts : pymanoid.ContactSet
Contacts with the environment.
method : string, optional
Choice between 'bretl' and 'cdd'.
Returns
-------
vertices : list of arrays
2D vertices of the static-equilibrium polygon.
"""
g = robot.compute_static_gravity_torques()
J_contact = robot.compute_contact_jacobian(contacts)
# Friction limits:
#
# A_fric * w_all <= b_fric
#
A_fric = block_diag(*[c.wrench_inequalities for c in contacts.contacts])
b_fric = zeros((A_fric.shape[0], ))
# Torque limits:
#
# |tau| <= robot.tau_max
#
# where tau == g - J_c^T w_all in static equilibrium.
#
A_act = vstack([-J_contact.T[:-6], +J_contact.T[:-6]])
b_act = hstack([(robot.tau_max - g)[:-6], (robot.tau_max + g)[:-6]])
# Net wrench constraint:
#
# w_0.force = -mass * gravity
# w_0.moment.z = 0
#
# where w_0 = G_0 * w_all is the contact wrench at the origin of the
# inertial frame, with G_0 the corresponding grasp matrix.
#
G_0 = contacts.compute_grasp_matrix([0., 0., 0.])
C = G_0[(0, 1, 2, 5), :]
d = array([0., 0., robot.mass * gravity_const, 0.])
# Output matrix: CoM horizontal coordinates
E = 1. / (robot.mass * 9.81) * vstack([-G_0[4, :], +G_0[3, :]])
f = zeros(2)
A = vstack([A_fric, A_act])
b = hstack([b_fric, b_act])
return project_polytope(ineq=(A, b),
eq=(C, d),
proj=(E, f),
method=method,
max_iter=100)