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,
])
