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")