def main(margin):
mu = 0.5
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
contacts[2].mu = 0.5
polyhedron = stab.StabilityPolygon(200, dimension=3, radius=1.5, robust_sphere=False)
polyhedron.contacts = contacts
polygon = stab.StabilityPolygon(200, dimension=2, radius=1.5)
polygon.contacts = contacts
shape = [
np.array([[-1., 0, 0]]).T,
np.array([[1., 0, 0]]).T,
np.array([[0, 1., 0]]).T,
np.array([[0, -1., 0]]).T,
np.array([[0, 0., 1]]).T,
np.array([[0, 0., -1]]).T
]
polytope = [margin*s for s in shape]
polyhedron.gravity_envelope = polytope
polyhedron.compute(stab.Mode.iteration, epsilon=2e-3, maxIter=10, solver='qhull',
record_anim=False, plot_init=False,
plot_step=False, plot_final=True)
def create_polyhedron(self, contacts):
self.handle = None
self.nr_iter = 0
try:
self.polyhedron = stabilipy.StabilityPolygon(
robot.mass, dimension=3, radius=1.5)
stabilipy_contacts = []
for contact in contacts:
hmatrix = matrixFromPose(contact.pose)
X, Y = contact.shape
displacements = [array([[X, Y, 0]]).T,
array([[-X, Y, 0]]).T,
array([[-X, -Y, 0]]).T,
array([[X, -Y, 0]]).T]
for disp in displacements:
stabilipy_contacts.append(
stabilipy.Contact(
contact.friction,
hmatrix[:3, 3:] + hmatrix[:3, :3].dot(disp),
hmatrix[:3, 2:3]))
self.polyhedron.contacts = stabilipy_contacts
self.polyhedron.select_solver(self.method)
self.polyhedron.make_problem()
self.polyhedron.init_algo()
self.polyhedron.build_polys()
vertices = self.polyhedron.polyhedron()
self.handle = draw_polytope(
[(x[0], x[1], x[2]) for x in vertices])
except Exception as e:
print("SupportPolyhedronDrawer: {}".format(e))
def main():
mu = 0.5
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
contacts[2].mu = 0.5
polyhedron = stab.StabilityPolygon(200, dimension=2, radius=1.5)
polyhedron.contacts = contacts
polyhedron.select_solver('plain')
polyhedron.make_problem()
polyhedron.init_algo()
polyhedron.build_polys()
points = np.random.random((100, 2)) - 0.5
truths = np.array([
polyhedron.sample(point, plot_final=False, plot_step=False)
for point in points
])
polyhedron.plot()
polyhedron.ax.plot(*list(zip(*points[truths, :])),
linestyle="none",
marker="*",
markerfacecolor="green")
polyhedron.ax.plot(*list(zip(*points[~truths, :])),
linestyle="none",
marker="x",
markerfacecolor="red")
polyhedron.show()
def main():
#normals = map(stab.normalize,
# [np.array([[-0.7, 0, 1]]).T,
# np.array([[0.1, 0.5, 1]]).T,
# np.array([[0.5, -0.5, 1]]).T])
#pos = [np.array([[0, 0, 0]]).T,
# np.array([[0, 1, 0]]).T,
# np.array([[1, 0, 0.2]]).T]
#normals = map(stab.normalize,
# [np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T])
#pos = [np.array([[0.1, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.055, -0.105]]).T,
# np.array([[0.1, 0.055, -0.105]]).T]
#normals = [np.array([[0, 0, 1]]).T]*3
normals = list(
map(stab.normalize, [
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.0, 0.0, 1]]).T
]))
pos = [
np.array([[0.1, -0.2, 0]]).T,
np.array([[-0.1, -0.2, 0]]).T,
np.array([[0.2, 0, 1]]).T
]
bar = sum(pos) / float(len(pos))
pos = [p - bar for p in pos]
mu = 0.5
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60, dimension=2)
poly.contacts = contacts
poly.compute(stab.Mode.iteration,
epsilon=2e-3,
maxIter=10,
solver='plain',
record_anim=False,
plot_step=True,
plot_final=False)
def main():
#normals = map(stab.normalize,
# [np.array([[-0.7, 0, 1]]).T,
# np.array([[0.1, 0.5, 1]]).T,
# np.array([[0.5, -0.5, 1]]).T])
#pos = [np.array([[0, 0, 0]]).T,
# np.array([[0, 1, 0]]).T,
# np.array([[1, 0, 0.2]]).T]
#normals = map(stab.normalize,
# [np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T])
#pos = [np.array([[0.1, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.055, -0.105]]).T,
# np.array([[0.1, 0.055, -0.105]]).T]
#normals = [np.array([[0, 0, 1]]).T]*3
normals = list(
map(stab.normalize, [
np.array([[-1, 0.0, 0]]).T,
np.array([[1, 0.0, 0]]).T,
np.array([[0.0, 1., 0]]).T
]))
pos = [
np.array([[1., 0., 0.]]).T,
np.array([[-1., 0.0, 0.]]).T,
np.array([[0., -1., 0.]]).T
]
bar = sum(pos) / float(len(pos))
pos = [p - bar for p in pos]
mu = 0.7
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60)
poly.contacts = contacts
poly.compute(3e-2, True, False, False, True)
def main():
global pos, normals
mu = 0.7
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly_3d = stab.StabilityPolygon(100, dimension=3)
poly_3d.contacts = contacts
poly_2d = stab.StabilityPolygon(100, dimension=2)
poly_2d.contacts = contacts
polys = [poly_2d, poly_3d]
point = np.array([[0.36510907, 0.31419711, 0.73607441]]).T
for poly in polys:
poly.addTorqueConstraint(contacts[-4:-2],
point,
10*np.ones((3, 1)))
gravities = np.linspace(0, 2, 10)
shape = [
np.array([[-1., 0, 0]]).T,
np.array([[1., 0, 0]]).T,
np.array([[0, 1., 0]]).T,
np.array([[0, -1., 0]]).T
]
for gravity in gravities:
for poly in polys:
poly.gravity_envelope = [gravity*s for s in shape]
poly.compute(stab.Mode.best, maxIter=50, epsilon=1e-2, solver='cdd',
plot_init=False, plot_final=False, plot_step=False,
plot_direction=False, plot_error=False)
filename = os.path.join('different_gravities', 'g_{}_{}d'.format(gravity, poly.dimension))
poly.save_polyhedron(filename)
poly.plot()
plt.show()
return poly
def main():
global pos, normals
#bar = sum(pos)/float(len(pos))
#pos = [p-bar for p in pos]
mu = 0.7
frame_dir = 'interp_vs_cstr'
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(100, dimension=2, radius=100., force_lim=1.)
#poly.contacts = contacts
#poly.reset_fig()
#poly.plot_contacts()
#poly.show()
#poly.make_problem()
#poly.check_sizes()
poly.contacts = contacts[0:4]
poly.contacts += [stab.Contact(mu, p, n) for p, n in zip(*lfc())]
poly.compute(stab.Mode.best,
maxIter=50,
epsilon=1e-2,
solver='plain',
plot_init=False,
plot_final=False,
plot_step=False,
plot_direction=False,
plot_error=False)
p1 = poly.polygon()
poly.reset()
poly.contacts = contacts
poly.contacts += [stab.Contact(mu, p, n) for p, n in zip(*lfc())]
for k in range(4, 8):
poly.addForceConstraint(contacts[k:k + 1], 1.0)
point = np.array([[0.36510907, 0.31419711, 0.73607441]]).T
#poly.addTorqueConstraint(contacts[-4:-2],
# point,
# 10*np.ones((3, 1)))
#gravities = np.linspace(0, 2, 10)
#shape = [
# np.array([[-1., 0, 0]]).T,
# np.array([[1., 0, 0]]).T,
# np.array([[0, 1., 0]]).T,
# np.array([[0, -1., 0]]).T
# ]
#for gravity in gravities:
# poly.gravity_envelope = [gravity*s for s in shape]
poly.compute(stab.Mode.best,
maxIter=50,
epsilon=1e-2,
solver='plain',
plot_init=False,
plot_final=False,
plot_step=False,
plot_direction=False,
plot_error=False)
p0 = poly.polygon()
interp = shapes.PolygonInterpolator(p0, p1)
for i in range(3):
x, y = list(
zip(*[[c.r[0], c.r[1]] for c in poly.contacts[4 * i:4 * (i + 1)]]))
print(x)
x, y = list(x), list(y)
x.append(x[0])
y.append(y[0])
plt.plot(x, y)
with open(os.path.join(frame_dir, 'contact_{}'.format(i)), 'w') as f:
for xi, yi in zip(x, y):
f.write('{} {}{}'.format(xi[0], yi[0], os.linesep))
plt.plot(*p1.exterior.coords.xy)
plt.plot(*p0.exterior.coords.xy)
plt.show()
#poly.addTorqueConstraint(contacts[-2:],
# point,
# 10*np.ones((3, 1)))
#poly.reset_fig()
#poly.plot_contacts()
A = np.array(p0.exterior.coords)
np.savetxt(os.path.join(frame_dir, 'p0'), A)
A = np.array(p1.exterior.coords)
np.savetxt(os.path.join(frame_dir, 'p1'), A)
f = open('top_lel.txt', 'w')
nr_tests = 10
polygons = []
for i in range(nr_tests + 1):
poly.clearConstraints()
cur_percent = (nr_tests - i) / nr_tests
for k in range(4, 8):
poly.addForceConstraint(contacts[k:k + 1], cur_percent)
poly.compute(stab.Mode.best,
maxIter=50,
epsilon=1e-2,
solver='plain',
plot_init=False,
plot_final=False,
plot_step=False,
plot_direction=False,
plot_error=False)
cur_p = poly.polygon()
fname = 'cstr_{}'.format(nr_tests - i)
A = np.array(cur_p.exterior.coords)
np.savetxt(os.path.join(frame_dir, fname), A)
cur_area = cur_p.area
max_j = 1.
min_j = 0.
j = 0.5
pi = interp.fast_interpolate(j)
interp_area = pi.area
nr_iter = 100
while abs(cur_area - interp_area) > 1e-12 and nr_iter < 1000:
if cur_area > interp_area:
max_j = j
j = (min_j + j) / 2
else:
min_j = j
j = (max_j + j) / 2
pi = interp.fast_interpolate(j)
interp_area = pi.area
#print cur_area - interp_area, max_j, j, min_j
nr_iter += 1
f.write("{} {} {} {}\n".format(cur_percent, j, cur_area, interp_area))
fname = 'interp_{}'.format(nr_tests - i)
A = np.array(pi.exterior.coords)
np.savetxt(os.path.join(frame_dir, fname), A)
polygons.append(A)
#To watch the log file grow
f.flush()
print(cur_area - interp_area)
#poly.plot()
#poly.ax.plot(*pi.exterior.coords.xy)
#poly.show()
#plt.plot(*pi.exterior.coords.xy)
#plt.show()
f.close()
poly.reset_fig()
poly.plot_contacts()
for mat in polygons:
x, y = list(zip(*mat))
poly.ax.plot(x, y)
poly.show()
##A = np.vstack([p.T for p in poly.points])
#print zip(*A)
#qhull = ConvexHull(A)
#coords, tri = [c for c in qhull.points.T], qhull.simplices
#dirs = np.vstack(poly.directions[:-1])
#offsets = np.vstack(poly.offsets[:-1])
#mat = np.hstack([offsets, -dirs])
#out = cdd.Matrix(mat, number_type='fraction')
#print np.array(out)
#out.rep_type = cdd.RepType.INEQUALITY
#polyhedron = cdd.Polyhedron(out)
#gen = np.array(polyhedron.get_generators())
#f = np.vectorize(lambda x: Fraction(str(x)).limit_denominator(1000))
#ppl_poly = PPL_Poly(hrep=f(mat))
#p3 = ppl_poly.vrep()[:, 1:]
#qhull = ConvexHull(p3)
#coords3, tri3 = [c for c in qhull.points.T], qhull.simplices
#print np.array(out).shape
#print np.array(poly.outer).shape
##print np.array(out) - np.vstack([np.array(poly.outer), np.zeros((1, 4))])
#p2 = gen[:, 1:]
#qhull = ConvexHull(p2)
#coords2, tri2 = [c for c in qhull.points.T], qhull.simplices
#plt.close("all")
#fig = plt.figure()
#ax = fig.add_subplot('111', aspect='equal', projection='3d')
#x, y, z = zip(*A)
#ax.scatter(x, y, z)
#x, y, z = zip(*p3)
#ax.scatter(x, y, z, color='b', marker='^', s=120.)
#ax.plot_trisurf(*coords, triangles=tri, color='r', alpha=0.5)
#ax.plot_trisurf(*coords2, triangles=tri2, color='b', alpha=0.1)
#ax.plot_trisurf(*coords3, triangles=tri3, color='g', alpha=0.1)
#plt.show()
return poly
def main():
global pos, normals
#bar = sum(pos)/float(len(pos))
#pos = [p-bar for p in pos]
mu = 0.7
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
#right_foot_contacts = contacts[0:4]
gripper_contacts = contacts[4:]
assert(len(gripper_contacts) == 4)
left_foot_contacts = [stab.Contact(mu, p, n) for p, n in zip(*lfcontact.contacts())]
poly = stab.StabilityPolygon(100, dimension=2)
poly.contacts = left_foot_contacts + gripper_contacts
#poly.reset_fig()
#poly.plot_contacts()
#poly.show()
#poly.make_problem()
#poly.check_sizes()
#poly.contacts = contacts[0:4]
#poly.compute(stab.Mode.best, maxIter=50, epsilon=1e-2, solver='cdd',
# plot_init=False, plot_final=False, plot_step=False,
# plot_direction=False, plot_error=False)
#p1 = poly.polygon()
#poly.contacts = contacts
point = np.array([[0.36510907, 0.31419711, 0.73607441]]).T
poly.addTorqueConstraint(poly.contacts[4:-2],
point,
np.ones((3, 1)),
np.zeros((3, 1)))
poly.compute(stab.Mode.best, maxIter=50, epsilon=1e-2, solver='plain',
plot_init=False, plot_final=True, plot_step=False,
plot_direction=False, plot_error=False)
print("Print area : ", poly.volume_convex(poly.inner))
poly.contacts = contacts + left_foot_contacts
poly.torque_constraints = []
poly.addTorqueConstraint(contacts[4:8],
point,
3*np.ones((3, 1)),
2*np.ones((3, 1)))
poly.compute(stab.Mode.best, maxIter=50, epsilon=1e-2, solver='cdd',
plot_init=False, plot_final=True, plot_step=False,
plot_direction=False, plot_error=False)
print("Print area : ", poly.volume_convex(poly.inner))
#p0 = poly.polygon()
#interp = shapes.PolygonInterpolator(p0, p1)
#poly.addTorqueConstraint(contacts[-2:],
# point,
# 10*np.ones((3, 1)))
#poly.reset_fig()
#poly.plot_contacts()
#f = open('top_lel.txt', 'w')
#for i in range(101):
# poly.forces_constraints = []
# for k in range(4, 8):
# poly.addForceConstraint(contacts[k:k+1], (100-i)*0.01)
# poly.compute(stab.Mode.best, maxIter=50, epsilon=1e-2, solver='cdd',
# plot_init=False, plot_final=False, plot_step=False,
# plot_direction=False, plot_error=False)
# cur_p = poly.polygon()
# cur_area = cur_p.area
# max_j = 1.
# min_j = 0.
# j = 0.5
# pi = interp.fast_interpolate(j)
# interp_area = pi.area
# nr_iter = 0
# while abs(cur_area - interp_area) > 1e-3 and nr_iter < 200:
# if cur_area > interp_area:
# max_j = j
# j = (min_j+j)/2
# else:
# min_j = j
# j = (max_j+j)/2
# pi = interp.fast_interpolate(j)
# interp_area = pi.area
# #print cur_area - interp_area, max_j, j, min_j
# nr_iter += 1
# f.write("{},{}\n".format((100-i)*0.01, j))
# #poly.plot()
# #poly.ax.plot(*pi.exterior.coords.xy)
# #poly.show()
#f.close()
##A = np.vstack([p.T for p in poly.points])
#print zip(*A)
#qhull = ConvexHull(A)
#coords, tri = [c for c in qhull.points.T], qhull.simplices
#dirs = np.vstack(poly.directions[:-1])
#offsets = np.vstack(poly.offsets[:-1])
#mat = np.hstack([offsets, -dirs])
#out = cdd.Matrix(mat, number_type='fraction')
#print np.array(out)
#out.rep_type = cdd.RepType.INEQUALITY
#polyhedron = cdd.Polyhedron(out)
#gen = np.array(polyhedron.get_generators())
#f = np.vectorize(lambda x: Fraction(str(x)).limit_denominator(1000))
#ppl_poly = PPL_Poly(hrep=f(mat))
#p3 = ppl_poly.vrep()[:, 1:]
#qhull = ConvexHull(p3)
#coords3, tri3 = [c for c in qhull.points.T], qhull.simplices
#print np.array(out).shape
#print np.array(poly.outer).shape
##print np.array(out) - np.vstack([np.array(poly.outer), np.zeros((1, 4))])
#p2 = gen[:, 1:]
#qhull = ConvexHull(p2)
#coords2, tri2 = [c for c in qhull.points.T], qhull.simplices
#plt.close("all")
#fig = plt.figure()
#ax = fig.add_subplot('111', aspect='equal', projection='3d')
#x, y, z = zip(*A)
#ax.scatter(x, y, z)
#x, y, z = zip(*p3)
#ax.scatter(x, y, z, color='b', marker='^', s=120.)
#ax.plot_trisurf(*coords, triangles=tri, color='r', alpha=0.5)
#ax.plot_trisurf(*coords2, triangles=tri2, color='b', alpha=0.1)
#ax.plot_trisurf(*coords3, triangles=tri3, color='g', alpha=0.1)
#plt.show()
return poly
def main():
global pos, normals
#bar = sum(pos)/float(len(pos))
#pos = [p-bar for p in pos]
offset = np.array([[0.0], [0.], [0.]])
mu = 0.5
contacts = [stab.Contact(mu, offset+p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60, dimension=2, radius=100.)
poly.printer.verbosity = stab.Verbosity.none
poly.contacts = contacts
point = offset+np.array([[0.36510907, 0.31419711, 0.73607441]]).T
poly.make_problem()
poly.reset_fig()
poly.plot_contacts()
poly.ax.plot(point[0], point[1], point[2], 'o', markersize=10)
poly.show()
sol = 'cdd'
poly.compute(stab.Mode.iteration, maxIter=50, epsilon=2e-3,
solver=sol, plot_error=False, plot_step=False,
plot_init=False, plot_final=False)
unconstrained = poly.polyhedron()
bar1 = sum([c.r for c in poly.contacts[0:4]])/4
bar2 = sum([c.r for c in poly.contacts[4:8]])/4
#Foot
poly.addDistConstraint(bar1, 1.5)
print(bar1)
#Hand
poly.addDistConstraint(bar2, 1.5)
poly.compute(stab.Mode.iteration, maxIter=50, epsilon=2e-3,
solver=sol, plot_error=False, plot_step=False,
plot_init=False, plot_final=False)
dist_constrained = poly.polyhedron()
poly.addTorqueConstraint(contacts[-4:-2],
point,
1*np.ones((3, 1)))
poly.addTorqueConstraint(contacts[-2:],
point,
1*np.ones((3, 1)))
poly.compute(stab.Mode.iteration, maxIter=50, epsilon=2e-3,
solver=sol, plot_error=False, plot_step=False,
plot_init=False, plot_final=False)
all_constrained = poly.polyhedron()
def envelope(points):
hull = ConvexHull(points)
exterior = hull.points[hull.vertices, :]
exterior = np.vstack([exterior, hull.points[hull.vertices[0], :]])
return exterior
poly.reset_fig()
poly.plot_contacts()
unconstrained = envelope(unconstrained)
poly.ax.plot(unconstrained[:, 0], unconstrained[:, 1], label="Unconstrained")
dist_constrained = envelope(dist_constrained)
poly.ax.plot(dist_constrained[:, 0], dist_constrained[:, 1], label="Dist constraint")
all_constrained = envelope(all_constrained)
poly.ax.plot(all_constrained[:, 0], all_constrained[:, 1], label="Dist + force constrained")
plt.legend()
plt.show()
if sol == 'plain':
ineq = [l/abs(l[0]) for l in poly.inner.inequalities]
print(np.vstack(ineq))
elif sol == 'cdd':
poly = cdd.Polyhedron(poly.inner)
print(poly.get_inequalities())
return poly
def main():
global pos, normals
#bar = sum(pos)/float(len(pos))
#pos = [p-bar for p in pos]
mu = 0.2
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60, dimension=2, force_lim=1000.)
poly.contacts = contacts[0:8]
for c in poly.contacts[0:4]:
c.r[2] = 0.
c.n = np.array([[0.4], [0.4], [np.sqrt(1 - 2 * (0.4**2))]])
#c.n = np.array([[0., 1., 0.]]).T
for c in poly.contacts[4:]:
c.r[2] = 0.
#c.n = np.array([[0., 0., 1.]]).T
c.n = np.array([[-0.4], [-0.4], [np.sqrt(1 - 2 * (0.4**2))]])
poly.reset_fig()
poly.plot_contacts()
poly.show()
sol = 'plain'
#Compute the unconstrained and save ineqs
poly.compute(stab.Mode.iteration,
maxIter=20,
epsilon=2e-3,
solver=sol,
plot_error=False,
plot_step=False,
plot_init=False,
plot_final=False)
poly_ineq = poly.backend.doublepoly.inner.inequalities
radius = 0.11
fc = 4
nc = 8
for c in range(fc, nc):
poly.addForceConstraint([poly.contacts[c]], radius)
poly.compute(stab.Mode.iteration,
maxIter=20,
epsilon=2e-3,
solver=sol,
plot_error=False,
plot_step=False,
plot_init=False,
plot_final=False)
poly.plot()
poly.show()
assert (poly.dimension == 2)
assert (len(poly.gravity_envelope) == 1)
A1, A2, t = poly.A1, poly.A2, poly.t
sphere_ineq = np.array([[1., -1., 1.], [1., -1., -1.], [1., 1., -1.],
[1., 1., 1.], [-1., 1., -1.], [-1., 1., 1.],
[-1., -1., -1.], [-1., -1., 1.]])
sphere = np.zeros((8 * (nc - fc), 1 + poly.nrVars()))
for contact_id in range(fc, nc):
line = 8 * (contact_id - fc)
col = 1 + 3 * contact_id
sphere[line:line + 8, col:col + 3] = sphere_ineq
sphere[line:line + 8, 0] = radius * poly.mass * 9.81
nr_lines = poly_ineq.shape[0]
exp_poly_ineq = np.hstack([
poly_ineq[:, 0:1],
np.zeros((nr_lines, poly.nrVars() - 2)), poly_ineq[:, 1:]
])
eq = np.hstack((t, -A1, -A2))
mat = cdd.Matrix(sphere, number_type='fraction')
mat.rep_type = cdd.RepType.INEQUALITY
mat.extend(exp_poly_ineq)
mat.extend(eq, linear=True)
print("Let's goooooo")
cdd_poly = cdd.Polyhedron(mat)
vertices = np.array(cdd_poly.get_generators())
print(vertices.shape)
if len(vertices.shape) > 1:
point_mask = vertices[:, 0] == 1
points = vertices[point_mask, -2:]
rays = vertices[~point_mask, -2:]
hull = ConvexHull(points)
poly.reset_fig()
poly.plot_contacts()
poly.plot_polyhedron(poly.inner, 'blue', 0.5)
poly.ax.plot(hull.points[hull.vertices, 0],
hull.points[hull.vertices, 1],
'red',
label='cdd',
marker='^',
markersize=10)
for ray in rays:
if np.linalg.norm(ray) > 1e-10:
print(ray)
pp = np.vstack([i * ray for i in np.linspace(0.01, 1)])
poly.ax.plot(pp[:, 0], pp[:, 1], 'red')
else:
print("This is a zero ray")
poly.show()
else:
print("No vertices")
def main():
#normals = map(stab.normalize,
# [np.array([[-0.7, 0, 1]]).T,
# np.array([[0.1, 0.5, 1]]).T,
# np.array([[0.5, -0.5, 1]]).T])
#pos = [np.array([[0, 0, 0]]).T,
# np.array([[0, 1, 0]]).T,
# np.array([[1, 0, 0.2]]).T]
#normals = map(stab.normalize,
# [np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T,
# np.array([[0., 0., 1.]]).T])
#pos = [np.array([[0.1, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.155, -0.105]]).T,
# np.array([[-0.07, 0.055, -0.105]]).T,
# np.array([[0.1, 0.055, -0.105]]).T]
#normals = [np.array([[0, 0, 1]]).T]*3
normals = list(
map(stab.normalize, [
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.0, 0.0, 1]]).T
]))
pos = [
np.array([[0.1, -0.2, 0]]).T,
np.array([[-0.1, -0.2, 0]]).T,
np.array([[0.2, 0, 1]]).T
]
bar = sum(pos) / float(len(pos))
pos = [p - bar for p in pos]
mu = 0.5
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60)
poly.contacts = contacts
poly.compute(stab.Mode.precision,
epsilon=2e-2,
record_anim=False,
plot_final=False)
polyhedron = poly.polyhedron()
m = np.min(polyhedron, axis=0)
v = np.max(polyhedron, axis=0) - np.min(polyhedron, axis=0)
points = np.random.rand(10000, 3)
points = points * v + m
stability = np.array([poly.sample(p.T[:, np.newaxis]) for p in points])
poly.reset_fig()
poly.plot()
#fig = plt.figure()
#ax = fig.add_subplot('121', aspect='equal', projection='3d')
stable = points[stability]
unstable = points[~stability]
#x, y, z = zip(*stable)
#poly.ax.scatter(x, y, z, c='g', marker='o')
#ax = fig.add_subplot('122', aspect='equal', projection='3d')
if unstable.size > 0:
pass
#x, y, z = zip(*unstable)
#poly.ax.scatter(x, y, z, c='r', marker='^')
vol_in = stab.volume_convex(poly.inner)
vol_out = stab.volume_convex(poly.outer)
vol_sampling = v[0] * v[1] * v[2]
ineq_in = np.array(cdd.Polyhedron(poly.inner).get_inequalities())
ineq_out = np.array(cdd.Polyhedron(poly.outer).get_inequalities())
#H format : b - A x > 0 stored as b | -A
test_in_points = ineq_in[:, [0]] + ineq_in[:, 1:].dot(points.T)
in_points = np.all(test_in_points > 0, axis=0)
test_out_points = ineq_out[:, [0]] + ineq_out[:, 1:].dot(points.T)
out_points = np.all(test_out_points > 0, axis=0)
und_points = np.logical_and(out_points, np.logical_not(in_points))
criterion = np.logical_or(und_points, np.equal(in_points, stability))
c = np.all(criterion)
print("Criterion : {}".format(c))
if not c:
print("Criterion is false...")
print("Number of failures : {}".format(criterion.size -
np.count_nonzero(criterion)))
print("Failure points : {}".format(points.T[~criterion]))
else:
print("Hurray all decidable points are confirmed!")
print(
"Volume inner : {} | Volume outer : {} | Volume sampling : {}".format(
vol_in, vol_out, vol_sampling))
print("Ratio vol. in./sampling : {} | Ratio points in/total : {}".format(
vol_in / vol_sampling, stable.shape[0] / points.shape[0]))
print(
"Ratio vol. undecidable/sampling : {} | Ratio undec. points/total: {}".
format((vol_out - vol_in) / vol_sampling,
np.count_nonzero(und_points) / points.shape[0]))
plt.show()
def main(n, alphas):
if len(alphas) != n:
raise RuntimeError("Need same number of constraints as contacts")
if sum(alphas) < 1:
raise RuntimeError("Bound too low")
mu = 0.5
pos, normals = contacts(n)
cont = [stab.Contact(mu, pi, ni) for pi, ni in zip(pos, normals)]
polygon = stab.StabilityPolygon(200, dimension=2, radius=1.5)
polygon.contacts = cont
for i in range(n):
polygon.addForceConstraint([polygon.contacts[i]], alphas[i])
polygon.compute(stab.Mode.best,
epsilon=2e-3,
maxIter=100,
solver='plain',
record_anim=False,
plot_init=False,
plot_step=False,
plot_final=False)
polygon.reset_fig()
polygon.plot_contacts()
points = polygon.inner.vertices
hull = ConvexHull(points)
vertices = np.hstack((hull.vertices, hull.vertices[0]))
polygon.ax.plot(points[vertices, 0], points[vertices, 1], 'r--', lw=2)
shrink_points = []
seq = CircularBuffer(point_seq(n))
alphas = CircularBuffer(alphas)
points = CircularBuffer([c.r[0:2, :] for c in polygon.contacts])
for i in range(n):
a0 = a1 = alphas[i]
r0 = r1 = alphas[i] * points[i]
k0 = k1 = i
ex0 = "{}*p{}".format(alphas[i], i)
ex1 = "{}*p{}".format(alphas[i], i)
done0 = done1 = False
niter = 1
while not (done0 and done1):
k0 = i + seq[niter]
k1 = i - seq[niter]
if not done0:
if a0 + alphas[k0] >= 1:
r0 = r0 + (1 - a0) * points[k0]
ex0 += "+ {}*p{}".format(1 - a0, k0)
done0 = True
else:
r0 = r0 + alphas[k0] * points[k0]
ex0 += "+ {}*p{}".format(alphas[k0], k0)
a0 += alphas[k0]
if not done1:
if a1 + alphas[k1] >= 1:
r1 = r1 + (1 - a1) * points[k1]
ex1 += " {}*p{}".format(1 - a1, k1)
done1 = True
else:
r1 = r1 + alphas[k1] * points[k1]
ex1 += " {}*p{}".format(alphas[k1], k1)
a1 += alphas[k1]
niter += 1
print(ex0)
print(ex1)
shrink_points.append(r0)
shrink_points.append(r1)
polygon.ax.plot(r0[0], r0[1], marker='^', markersize=20)
polygon.ax.plot(r1[0], r1[1], marker='^', markersize=20)
polygon.show()
def main():
normals = map(stab.normalize, [
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.5, 0.0, 1]]).T,
np.array([[0.0, 0.0, 1]]).T
])
pos = [
np.array([[0.1, -0.2, 0]]).T,
np.array([[-0.1, -0.2, 0]]).T,
np.array([[0.2, 0, 1]]).T
]
mu = 0.5
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60, dimension=2)
poly.contacts = contacts
poly.select_solver('cdd')
poly.make_problem()
poly.init_algo()
poly.build_polys()
dbpoly = DoublePolygon(poly.inner, poly.outer)
print dbpoly.outer.edges
print "Corresp:"
for i, j in enumerate(dbpoly.correspondences):
print "{} --> ({}, {})".format(i, *dbpoly.outer.edges[j][1:])
for e in dbpoly.inner.edges:
print "({}, {})".format(*e[1:])
print "Vertex"
print "-dbpoly:"
print dbpoly.inner.vertices
print "-dineq"
dineq = [l / abs(l[0]) for l in dbpoly.inner.inequalities]
print np.vstack(dineq)
ineq = cdd.Polyhedron(poly.inner).get_inequalities()
ineq.canonicalize()
print "-ineq"
print ineq
poly.compute(stab.Mode.iteration, maxIter=2, plot_final=False)
print "Adding..."
print poly.points[-2]
#dbpoly.addPointToInner(poly.points[-2], 3)
print "Vertices"
print "-dbpoly"
print dbpoly.inner.vertices
print "-inner"
print poly.inner
print "Ineq"
dineq = [l / abs(l[0]) for l in dbpoly.inner.inequalities]
print "-dineq"
print np.vstack(dineq)
vertices = cdd.Matrix(
np.hstack([
np.ones((dbpoly.inner.vertices.shape[0], 1)), dbpoly.inner.vertices
]))
vertices.rep_type = cdd.RepType.GENERATOR
ineq = cdd.Polyhedron(vertices).get_inequalities()
ineq.canonicalize()
print "-ineq from vertex"
print ineq
spoly = geom.Polygon([(l[1], l[2]) for l in vertices])
print spoly.exterior.is_ccw
print spoly.convex_hull.exterior.coords.xy
spoly.exterior.coords = list(spoly.exterior.coords)[::-1]
print spoly.exterior.is_ccw
i1 = dbpoly.inner.edges[3][1]
i2 = dbpoly.inner.edges[3][2]
print i1, i2
e1 = dbpoly.outer.edges[dbpoly.correspondences[i1]]
e2 = dbpoly.outer.edges[dbpoly.correspondences[i2]]
print e1, e2
old_edges = copy.deepcopy(dbpoly.outer.edges)
print dbpoly.outer.vertices
dbpoly.addInequalityToOuter(poly.offsets[-2], poly.directions[-2], i1, i2)
print poly.outer
print dbpoly.outer.inequalities
print cdd.Polyhedron(poly.outer).get_generators()
print dbpoly.outer.vertices
print old_edges
print dbpoly.outer.edges
poly.plot()
x, y = zip(*dbpoly.inner.vertices)
poly.ax.plot(x, y)
poly.show()
def main():
global pos, normals
#bar = sum(pos)/float(len(pos))
#pos = [p-bar for p in pos]
mu = 0.1
contacts = [stab.Contact(mu, p, n) for p, n in zip(pos, normals)]
poly = stab.StabilityPolygon(60, dimension=2,
force_lim=1000.)
poly.contacts = contacts[0:8]
for c in poly.contacts[0:4]:
c.r[2] = 0.
c.n = np.array([[0.4], [0.4], [np.sqrt(1-2*(0.4**2))]])
#c.n = np.array([[0., 1., 0.]]).T
for c in poly.contacts[4:]:
c.r[2] = 0.
#c.n = np.array([[0., 0., 1.]]).T
c.n = np.array([[-0.4], [-0.4], [np.sqrt(1-2*(0.4**2))]])
#poly.reset_fig()
#poly.plot_contacts()
#poly.show()
#poly.make_problem()
#poly.check_sizes()
#point = np.array([[0.36510907, 0.31419711, 0.73607441]]).T
#poly.addTorqueConstraint(contacts[-4:-2],
# point,
# 10*np.ones((3, 1)))
#poly.addTorqueConstraint(contacts[-2:],
# point,
# 10*np.ones((3, 1)))
#bar1 = sum([c.r for c in poly.contacts[0:4]])/4
#bar2 = sum([c.r for c in poly.contacts[4:8]])/4
#poly.addDistConstraint(bar1, 1.5)
#poly.addDistConstraint(bar2, 1.5)
#poly.make_problem()
#poly.reset_fig()
#poly.plot_contacts()
#poly.ax.plot(point[0], point[1], point[2], 'o', markersize=10)
#poly.show()
sol = 'plain'
poly.compute(stab.Mode.iteration, maxIter=50, epsilon=2e-3,
solver=sol, plot_error=False, plot_step=False,
plot_init=False, plot_final=False)
assert(poly.dimension == 2)
assert(len(poly.gravity_envelope) == 1)
A1, A2, t = poly.A1, poly.A2, poly.t
nr_generators = 4
friction_cstr = []
for i, contact in enumerate(poly.contacts):
cone = build_cone(nr_generators, mu, contact.n, offset_angle=np.pi/4)
nr_ineq = cone.shape[0]
cstr = np.zeros((nr_ineq+1, 1+poly.nrVars()))
cstr[:-1, 0] = cone[:, 0]
cstr[:-1, 3*i+1:3*i+4] = cone[:, 1:]
cstr[-1, 0] = 0.
cstr[-1, 3*i+1:3*i+4] = contact.n.T
friction_cstr.append(copy.deepcopy(cstr))
com_max = 2.
limit_cstr = np.zeros((4, 1+poly.nrVars()))
limit_cstr[:, 0:1] = com_max*np.vstack((np.ones((2, 1)), np.ones((2, 1))))
limit_cstr[:, -2:] = np.vstack((-np.identity(2), np.identity(2)))
nr_forces = 3*len(poly.contacts)
f_max = 5.*poly.mass*9.81
limit_force = np.zeros((2*nr_forces, 1+poly.nrVars()))
limit_force[:, 0:1] = f_max*np.vstack((np.ones((nr_forces, 1)), np.ones((nr_forces, 1))))
limit_force[:, 1:-2] = np.vstack((-np.identity(nr_forces), np.identity(nr_forces)))
def limit_fz(percent):
nr_fz = len(poly.contacts)
f_max = percent*poly.mass*9.81
limit_force = np.zeros((2*nr_fz, 1+poly.nrVars()))
for i, c in enumerate(poly.contacts):
limit_force[i, 1+3*i:4+3*i] = c.n.T
limit_force[nr_fz+i, 1+3*i:4+3*i] = -c.n.T
limit_force[:, 0] = f_max
return limit_force
friction = np.vstack(friction_cstr)
eq = np.hstack((t, -A1, -A2))
f_lim = limit_fz(0.2)
print(friction.shape)
print(f_lim.shape)
#mat = cdd.Matrix(friction, number_type='fraction')
##mat = cdd.Matrix(friction)
#mat.rep_type = cdd.RepType.INEQUALITY
##mat.extend(limit_force)
##mat.extend(limit_cstr)
#mat.extend(f_lim)
#print np.vstack(friction)
#mat.extend(eq, linear=True)
#cdd_poly = cdd.Polyhedron(mat)
#print "Let's go boiz"
#vertices = np.array(cdd_poly.get_generators())
parmapoly = pyparma.Polyhedron(hrep=fractionize(friction))
print(eq.shape)
for equality in eq:
ex = ex_from_line(fractionize(equality.squeeze()))
parmapoly.poly.add_constraint(ex == 0)
vertices = parmapoly.vrep()
print("Got full vrep")
for ineq in f_lim:
print("Insert")
parmapoly.add_ineq(fractionize(ineq))
print("Inserted shit")
vertex_lim = parmapoly.vrep()
print("Got small vrep")
#print np.array(mat).shape
print(vertices.shape)
if len(vertices.shape) > 1:
points = vertices[:, -2:]
hull = ConvexHull(points)
points_lim = vertex_lim[:, -2:]
hull_lim = ConvexHull(points_lim)
poly.reset_fig()
poly.plot_contacts()
poly.plot_polyhedron(poly.inner, 'blue', 0.5)
poly.ax.plot(points[hull.vertices, 0], points[hull.vertices, 1], label='linear')
poly.ax.plot(points_lim[hull_lim.vertices, 0], points_lim[hull_lim.vertices, 1], label='linear lim')
poly.show()
else:
print("No vertices")