def example(): pathpoints = 30 ref_path = {} ref_path['x'] = 5*np.sin(np.linspace(0,2*np.pi, pathpoints+1)) ref_path['y'] = np.linspace(1,2, pathpoints+1)**2*10 wp = ca.horzcat(ref_path['x'], ref_path['y']).T N = 5 N1 = N-1 wpts = np.array(wp[:, 0:N]) ocp = Ocp(N) x = ocp.state('x') x_dot = ocp.control('x_dot') dt = ocp.get_time() ocp.set_der(x, x_dot) ocp.set_objective(x, wpts[0, :]) ocp.set_objective(dt, ca.GenMX_zeros(N1)) ocp.set_constraints(x, wpts[0, 0]) x00 = wpts[0, :] T = 5 dt00 = [T/N1] * N1 xd00 = (x00[1:] - x00[:-1]) / dt00 ocp.set_inital(x, x00) ocp.set_inital(x_dot, xd00) ocp.set_inital(dt, dt00) states, controls, times = ocp.solve() x = states x_dots = controls total_time = np.sum(times) print(f"Times: {times}") print(f"Total Time: {total_time}") print(f"xs: {x.T}") print(f"X dots: {x_dots.T}") plt.figure(1) plt.plot(wpts[0, :], np.ones_like(wpts[0, :]), 'o', markersize=12) plt.plot(x, np.ones_like(x), '+', markersize=20) plt.show()
def example2D(): pathpoints = 30 ref_path = {} ref_path['x'] = 5*np.sin(np.linspace(0,2*np.pi, pathpoints+1)) ref_path['y'] = np.linspace(1,2, pathpoints+1)**2*10 wp = ca.horzcat(ref_path['x'], ref_path['y']).T N = 30 N1 = N-1 wpts = np.array(wp[:, 0:N]) ocp = Ocp(N) x = ocp.state('x') y = ocp.state('y') x_dot = ocp.control('x_dot') y_dot = ocp.control('y_dot') dt = ocp.get_time() ocp.set_der(x, x_dot) ocp.set_der(y, y_dot) ocp.set_objective(x, wpts[0, :]) ocp.set_objective(y, wpts[1, :]) ocp.set_objective(dt, ca.GenMX_zeros(N1), 0.01) ocp.set_constraints(x, wpts[0, 0]) ocp.set_constraints(y, wpts[1, 0]) max_speed = 1 ocp.set_lims(x, 0, ca.inf) ocp.set_lims(y, 0, ca.inf) ocp.set_lims(x_dot, -max_speed, max_speed) ocp.set_lims(y_dot, -max_speed, max_speed) T = 5 dt00 = [T/N1] * N1 ocp.set_inital(dt, dt00) x00 = wpts[0, :] y00 = wpts[1, :] xd00 = (x00[1:] - x00[:-1]) / dt00 ocp.set_inital(x, x00) ocp.set_inital(x_dot, xd00) yd00 = (y00[1:] - y00[:-1]) / dt00 ocp.set_inital(y, y00) ocp.set_inital(y_dot, yd00) states, controls, times = ocp.solve() x = states[:N] y = states[N:] x_dots = controls[:N1] y_dots = controls[N1:] total_time = np.sum(times) print(f"Times: {times}") print(f"Total Time: {total_time}") print(f"xs: {x.T}") print(f"ys: {y.T}") print(f"X dots: {x_dots.T}") print(f"Y dots: {y_dots.T}") plt.figure(1) plt.plot(wpts[0, :], wpts[1, :], 'o', markersize=12) plt.plot(x, y, '+', markersize=20) plt.show()
def example_loop(): pathpoints = 30 ref_path = {} ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1)) ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10 wp = ca.horzcat(ref_path['x'], ref_path['y']).T N = 10 N1 = N - 1 ocp = MPC(N) x = ocp.state('x') y = ocp.state('y') x_dot = ocp.control('x_dot') y_dot = ocp.control('y_dot') dt = ocp.get_time() ocp.set_der(x, x_dot) ocp.set_der(y, y_dot) ocp.set_objective(dt, ca.GenMX_zeros(N1), 0.01) max_speed = 1 ocp.set_lims(x, -ca.inf, ca.inf) ocp.set_lims(y, -ca.inf, ca.inf) ocp.set_lims(x_dot, -max_speed, max_speed) ocp.set_lims(y_dot, -max_speed, max_speed) wpts = np.array(wp[:, 0:N]) T = 5 dt00 = [T / N1] * N1 ocp.set_inital(dt, dt00) x00 = wpts[0, :] y00 = wpts[1, :] xd00 = (x00[1:] - x00[:-1]) / dt00 ocp.set_inital(x, x00) ocp.set_inital(x_dot, xd00) yd00 = (y00[1:] - y00[:-1]) / dt00 ocp.set_inital(y, y00) ocp.set_inital(y_dot, yd00) ocp.set_up_solve() for i in range(20): wpts = np.array(wp[:, i:i + N]) x0 = [wpts[0, 0], wpts[1, 0]] ocp.set_objective(x, wpts[0, :]) ocp.set_objective(y, wpts[1, :]) states, controls, times = ocp.solve(x0) xs = states[:N] ys = states[N:] x_dots = controls[:N1] y_dots = controls[N1:] total_time = np.sum(times) print(f"Times: {times}") print(f"Total Time: {total_time}") print(f"xs: {xs.T}") print(f"ys: {ys.T}") print(f"X dots: {x_dots.T}") print(f"Y dots: {y_dots.T}") plt.figure(1) plt.clf() plt.plot(wpts[0, :], wpts[1, :], 'o', markersize=12) plt.plot(xs, ys, '+', markersize=20) plt.pause(0.5)
def example_loop_v_th(): pathpoints = 30 ref_path = {} ref_path['x'] = 5 * np.sin(np.linspace(0, 2 * np.pi, pathpoints + 1)) ref_path['y'] = np.linspace(1, 2, pathpoints + 1)**2 * 10 wp = ca.horzcat(ref_path['x'], ref_path['y']).T N = 10 N1 = N - 1 ocp = MPC(N) x = ocp.state('x') y = ocp.state('y') th = ocp.control('th') v = ocp.control('v') dt = ocp.get_time() ocp.set_der(x, v * ca.cos(th)) ocp.set_der(y, v * ca.sin(th)) ocp.set_objective(dt, ca.GenMX_zeros(N1), 0.01) # set limits max_speed = 10 ocp.set_lims(x, -ca.inf, ca.inf) ocp.set_lims(y, -ca.inf, ca.inf) ocp.set_lims(v, 0, max_speed) ocp.set_lims(th, -ca.pi, ca.pi) wpts = np.array(wp[:, 0:N]) # find starting vals T = 5 dt00 = np.array([T / N1] * N1) ocp.set_inital(dt, dt00) x00 = wpts[0, :] ocp.set_inital(x, x00) y00 = wpts[1, :] ocp.set_inital(y, y00) th00 = [lib.get_bearing(wpts[:, i], wpts[:, i + 1]) for i in range(N1)] ocp.set_inital(th, th00) v00 = np.array( [lib.get_distance(wpts[:, i], wpts[:, i + 1]) for i in range(N1)]) / dt00 ocp.set_inital(v, v00) ocp.set_up_solve() for i in range(20): wpts = np.array(wp[:, i:i + N]) x0 = [wpts[0, 0], wpts[1, 0]] ocp.set_objective(x, wpts[0, :]) ocp.set_objective(y, wpts[1, :]) states, controls, times = ocp.solve(x0) xs = states[:N] ys = states[N:] ths = controls[:N1] vs = controls[N1:] total_time = np.sum(times) print(f"Times: {times}") print(f"Total Time: {total_time}") print(f"xs: {xs.T}") print(f"ys: {ys.T}") print(f"Thetas: {ths.T}") print(f"Velocities: {vs.T}") plt.figure(1) plt.clf() plt.plot(wpts[0, :], wpts[1, :], 'o', markersize=12) plt.plot(xs, ys, '+', markersize=20) plt.pause(0.5)
def setup(self, bending_BC_type="cantilevered" ): """ Sets up the problem. Run this last. :return: None (in-place) """ ### Discretize and assign loads # Discretize point_load_locations = [load["location"] for load in self.point_loads] point_load_locations.insert(0, 0) point_load_locations.append(self.length) self.x = cas.vertcat(*[ cas.linspace( point_load_locations[i], point_load_locations[i + 1], self.points_per_point_load) for i in range(len(point_load_locations) - 1) ]) # Post-process the discretization self.n = self.x.shape[0] dx = cas.diff(self.x) # Add point forces self.point_forces = cas.GenMX_zeros(self.n - 1) for i in range(len(self.point_loads)): load = self.point_loads[i] self.point_forces[self.points_per_point_load * (i + 1) - 1] = load["force"] # Add distributed loads self.force_per_unit_length = cas.GenMX_zeros(self.n) self.moment_per_unit_length = cas.GenMX_zeros(self.n) for load in self.distributed_loads: if load["type"] == "uniform": self.force_per_unit_length += load["force"] / self.length elif load["type"] == "elliptical": load_to_add = load["force"] / self.length * ( 4 / cas.pi * cas.sqrt(1 - (self.x / self.length) ** 2) ) self.force_per_unit_length += load_to_add else: raise ValueError("Bad value of \"type\" for a load within beam.distributed_loads!") # Initialize optimization variables log_nominal_diameter = self.opti.variable(self.n) self.opti.set_initial(log_nominal_diameter, cas.log(self.diameter_guess)) self.nominal_diameter = cas.exp(log_nominal_diameter) self.opti.subject_to([ log_nominal_diameter > cas.log(self.thickness) ]) def trapz(x): out = (x[:-1] + x[1:]) / 2 # out[0] += x[0] / 2 # out[-1] += x[-1] / 2 return out # Mass self.volume = cas.sum1( cas.pi / 4 * trapz( (self.nominal_diameter + self.thickness) ** 2 - (self.nominal_diameter - self.thickness) ** 2 ) * dx ) self.mass = self.volume * self.density # Mass proxy self.volume_proxy = cas.sum1( cas.pi * trapz( self.nominal_diameter ) * dx * self.thickness ) self.mass_proxy = self.volume_proxy * self.density # Find moments of inertia self.I = cas.pi / 64 * ( # bending (self.nominal_diameter + self.thickness) ** 4 - (self.nominal_diameter - self.thickness) ** 4 ) self.J = cas.pi / 32 * ( # torsion (self.nominal_diameter + self.thickness) ** 4 - (self.nominal_diameter - self.thickness) ** 4 ) if self.bending: # Set up derivatives self.u = 1 * self.opti.variable(self.n) self.du = 0.1 * self.opti.variable(self.n) self.ddu = 0.01 * self.opti.variable(self.n) self.dEIddu = 1 * self.opti.variable(self.n) self.opti.set_initial(self.u, 0) self.opti.set_initial(self.du, 0) self.opti.set_initial(self.ddu, 0) self.opti.set_initial(self.dEIddu, 0) # Define derivatives self.opti.subject_to([ cas.diff(self.u) == trapz(self.du) * dx, cas.diff(self.du) == trapz(self.ddu) * dx, cas.diff(self.E * self.I * self.ddu) == trapz(self.dEIddu) * dx, cas.diff(self.dEIddu) == trapz(self.force_per_unit_length) * dx + self.point_forces, ]) # Add BCs if bending_BC_type == "cantilevered": self.opti.subject_to([ self.u[0] == 0, self.du[0] == 0, self.ddu[-1] == 0, # No tip moment self.dEIddu[-1] == 0, # No tip higher order stuff ]) else: raise ValueError("Bad value of bending_BC_type!") # Stress self.stress_axial = (self.nominal_diameter + self.thickness) / 2 * self.E * self.ddu if self.torsion: # Set up derivatives phi = 0.1 * self.opti.variable(self.n) dphi = 0.01 * self.opti.variable(self.n) # Add forcing term ddphi = -self.moment_per_unit_length / (self.G * self.J) self.stress = self.stress_axial self.opti.subject_to([ self.stress / self.max_allowable_stress < 1, self.stress / self.max_allowable_stress > -1, ])