diff --git a/Shapes/ShapesFunctions.py b/Shapes/ShapesFunctions.py index aae2dfd9..f27ca18c 100644 --- a/Shapes/ShapesFunctions.py +++ b/Shapes/ShapesFunctions.py @@ -71,3 +71,15 @@ def Rectangle6D(grid): data = np.maximum(data, grid.vs[5] - 0.6) data = np.maximum(data, -grid.vs[5] - 0.6) return data + +def ShapeRectangle(grid, target_min, target_max): + data = np.maximum(grid.vs[0] - target_max[0], -grid.vs[0] + target_min[0]) + + for i in range(1, grid.dims): + data = np.maximum(data, grid.vs[i] - target_max[i]) + data = np.maximum(data, -grid.vs[1] + target_min[i]) + + return data + +def Rect_around_point(grid, target_point): + return ShapeRectangle(grid, target_point - 1.5*grid.dx, target_point + 1.5*grid.dx) \ No newline at end of file diff --git a/dynamics/Humannoid6D_sys1.py b/dynamics/Humannoid6D_sys1.py index 9be51929..bdc64b44 100644 --- a/dynamics/Humannoid6D_sys1.py +++ b/dynamics/Humannoid6D_sys1.py @@ -64,7 +64,7 @@ def dynamics(self, t,state, uOpt, dOpt): x6_dot[0] = state[2] * uOpt[2]/ self.J return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0], x5_dot[0], x6_dot[0]) - def opt_ctrl(self, t, state ,spat_deriv): + def opt_ctrl(self, t, state, spat_deriv): """ :param t: time t :param state: tuple of coordinates in 6 dimensions @@ -84,16 +84,22 @@ def opt_ctrl(self, t, state ,spat_deriv): parSum= hcl.scalar(0, "parSum") with hcl.if_(self.uMode == "min"): - parSum[0] = spat_deriv[1] + spat_deriv[5]*state[2]/self.J - SumUU[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMax[0])/state[2] + spat_deriv[3] * self.uMax[1] - SumUL[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMin[0])/state[2] + spat_deriv[3] * self.uMax[1] - SumLU[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMax[0])/state[2] + spat_deriv[3] * self.uMin[1] - SumLL[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMin[0])/state[2] + spat_deriv[3] * self.uMin[1] - + # everything containing (u1, u2) in Hamiltonian, for all combinations of u1 and u2 + SumUU[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMax[0])/state[2] + \ + spat_deriv[3] * self.uMax[1] + SumUL[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMin[0])/state[2] + \ + spat_deriv[3] * self.uMax[1] + SumLU[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMax[0])/state[2] + \ + spat_deriv[3] * self.uMin[1] + SumLL[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMin[0])/state[2] + \ + spat_deriv[3] * self.uMin[1] + + # try every combination of u1 and u2 and take minimum with hcl.if_(SumUU[0] > SumUL[0]): uOpt1[0] = self.uMin[0] uOpt2[0] = self.uMax[1] SumUU[0] = SumUL[0] + with hcl.elif_(SumUU[0] < SumUL[0]): uOpt1[0] = self.uMax[0] uOpt2[0] = self.uMax[1] @@ -107,9 +113,13 @@ def opt_ctrl(self, t, state ,spat_deriv): uOpt1[0] = self.uMin[0] uOpt2[0] = self.uMin[1] - # Find third controls + # Find u3 + # everything multiplied by u3 in Hamiltonian + parSum[0] = spat_deriv[1] + spat_deriv[5]*state[2]/self.J + with hcl.if_(parSum[0] > 0): uOpt3[0] = self.uMin[2] + with hcl.elif_(parSum[0] < 0): uOpt3[0] = self.uMax[2] diff --git a/dynamics/Humanoid12D_sys1.py b/dynamics/Humanoid12D_sys1.py new file mode 100644 index 00000000..56ed3f0c --- /dev/null +++ b/dynamics/Humanoid12D_sys1.py @@ -0,0 +1,124 @@ +import heterocl as hcl +import numpy as np +import time +import math + +class Humanoid12D_sys1: + def __init__(self, x=[0,0,0,0,0,0], \ + uMin=np.array([-50, -50, -50]), \ + uMax=np.array([ 50, 50, 50]), \ + dMin=np.array([0.0, 0.0, 0.0, 0.0]), \ + dMax=np.array([0.0, 0.0, 0.0, 0.0]), \ + dims=6, \ + uMode="min", \ + dMode="max" \ + ): + self.x = x + self.uMode = uMode + self.dMode = dMode + + # Object properties + self.x = x + + # Control bounds + self.uMin = uMin + self.uMax = uMax + + # Disturbance bounds + self.dMin = dMin + self.dMax = dMax + + self.dims = dims + + # Some constants + self.Jxx = 5 + self.Jyy = 5 + self.Jzz = 5 + self.g = 9.81 + + def dynamics(self, t, state, ctrl, dstb): + """ + + :param t: time + :param state: tuple of grid coordinates in 6 dimensions + :param uOpt: tuple of optimal control + :param dOpt: tuple of optimal disturbances + :return: tuple of time derivates in all dimensions + """ + + # System dynamics + # x1_dot = cos(x3) / cos(x2) * x4 + sin(x3) / cos(x2) * x5 + # x2_dot = -sin(x3) * x4 + cos(x3) * x5 + # x3_dot = cos(x3) * tan(x2) * x4 + sin(x3) * tan(x2) * x5 + x6 + # x4_dot = (Jyy - Jzz) * x5 * x6 + u4 / Jxx + # x5_dot = (Jzz - Jxx) * x4 * x6 + u5 / Jyy + # x6_dot = (Jxx - Jyy) * x4 * x5 + u6 / Jzz + + x1_dot = hcl.scalar(0, "x1_dot") + x2_dot = hcl.scalar(0, "x2_dot") + x3_dot = hcl.scalar(0, "x3_dot") + x4_dot = hcl.scalar(0, "x4_dot") + x5_dot = hcl.scalar(0, "x5_dot") + x6_dot = hcl.scalar(0, "x6_dot") + + x1_dot[0] = hcl.cos(state[2]) / hcl.cos(state[1]) * state[3] + \ + hcl.sin(state[2]) / hcl.cos(state[1]) * state[4] + x2_dot[0] = -hcl.sin(state[2]) * state[3] + hcl.cos(state[2]) * state[4] + x3_dot[0] = hcl.cos(state[2]) * hcl.sin(state[1]) / hcl.cos(state[1]) * state[3] + \ + hcl.sin(state[2]) * hcl.sin(state[1]) / hcl.cos(state[1]) * state[4] + state[5] + x4_dot[0] = (self.Jyy - self.Jzz) * state[4] * state[5] + ctrl[0] / self.Jxx + x5_dot[0] = (self.Jzz - self.Jxx) * state[3] * state[5] + ctrl[1] / self.Jyy + x6_dot[0] = (self.Jxx - self.Jyy) * state[3] * state[4] + ctrl[2] / self.Jzz + + return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0], x5_dot[0], x6_dot[0]) + + def opt_ctrl(self, t, state, spat_deriv): + """ + :param t: time t + :param state: tuple of coordinates + :param spat_deriv: tuple of spatial derivative in all dimensions + :return: + """ + # System dynamics + # x1_dot = cos(x3) / cos(x2) * x4 + sin(x3) / cos(x2) * x5 + # x2_dot = -sin(x3) * x4 + cos(x3) * x5 + # x3_dot = cos(x3) * tan(x2) * x4 + sin(x3) * tan(x2) * x5 + x6 + # x4_dot = (Jyy - Jzz) * x5 * x6 + u4 / Jxx + # x5_dot = (Jzz - Jxx) * x4 * x6 + u5 / Jyy + # x6_dot = (Jxx - Jyy) * x4 * x5 + u6 / Jzz + + # By default, pick maximum controls + opt_u4 = hcl.scalar(self.uMax[0], "opt_u4") + opt_u5 = hcl.scalar(self.uMax[1], "opt_u5") + opt_u6 = hcl.scalar(self.uMax[2], "opt_u6") + + if self.uMode == "min": + with hcl.if_(spat_deriv[3] > 0): + opt_u4[0] = self.uMin[0] + with hcl.if_(spat_deriv[4] > 0): + opt_u5[0] = self.uMin[1] + with hcl.if_(spat_deriv[5] > 0): + opt_u6[0] = self.uMin[2] + else: # self.uMode == "max" + with hcl.if_(spat_deriv[3] < 0): + opt_u4[0] = self.uMin[0] + with hcl.if_(spat_deriv[4] < 0): + opt_u5[0] = self.uMin[1] + with hcl.if_(spat_deriv[5] < 0): + opt_u6[0] = self.uMin[2] + + return (opt_u4[0], opt_u5[0], opt_u6[0]) + + def optDstb(self, spat_deriv): + """ + :param spat_deriv: spatial derivative in all dimensions + :return: tuple of optimal disturbance + """ + dOpt1 = hcl.scalar(0, "dOpt1") + dOpt2 = hcl.scalar(0, "dOpt2") + dOpt3 = hcl.scalar(0, "dOpt3") + dOpt4 = hcl.scalar(0, "dOpt4") + dOpt5 = hcl.scalar(0, "dOpt5") + dOpt6 = hcl.scalar(0, "dOpt6") + return (dOpt1[0], dOpt2[0], dOpt3[0], dOpt4[0], dOpt5[0], dOpt6[0]) + diff --git a/dynamics/Humanoid12D_sys2.py b/dynamics/Humanoid12D_sys2.py new file mode 100644 index 00000000..075aa545 --- /dev/null +++ b/dynamics/Humanoid12D_sys2.py @@ -0,0 +1,169 @@ +import heterocl as hcl +import numpy as np +import time +import math + +class Humanoid12D_sys2: + def __init__(self, x=[0,0,0,0,0,0], \ + uMin=np.array([-0.05, -0.05, 5]), \ + uMax=np.array([0.05, 0.05, 15]), \ + dMin=np.array([0.0, 0.0, 0.0, 0.0]), \ + dMax=np.array([0.0, 0.0, 0.0, 0.0]), \ + dims=6, \ + uMode="min", \ + dMode="max" \ + ): + self.x = x + self.uMode = uMode + self.dMode = dMode + + # Object properties + self.x = x + + # Control bounds + self.uMin = uMin + self.uMax = uMax + + # Disturbance bounds + self.dMin = dMin + self.dMax = dMax + + self.dims = dims + + # Constants + self.g = 9.81 + + def dynamics(self, t,state, ctrl, dstb): + """ + + :param t: time + :param state: tuple of grid coordinates in 6 dimensions + :param uOpt: tuple of optimal control + :param dOpt: tuple of optimal disturbances + :return: tuple of time derivates in all dimensions + """ + + # Dynamics + # x7_dot = x10 + # x8_dot = x11 + # x9_dot = x12 + # x10_dot = u3 * (x7 - u1) + # x11_dot = u3 * (x8 - u2) + # x12_dot = u3 * x9 - g + + x7_dot = hcl.scalar(0, "x7_dot") + x8_dot = hcl.scalar(0, "x8_dot") + x9_dot = hcl.scalar(0, "x9_dot") + x10_dot = hcl.scalar(0, "x10_dot") + x11_dot = hcl.scalar(0, "x11_dot") + x12_dot = hcl.scalar(0, "x12_dot") + + x7_dot[0] = state[3] + x8_dot[0] = state[4] + x9_dot[0] = state[5] + x10_dot[0] = ctrl[2] * (state[0] - ctrl[0]) + x11_dot[0] = ctrl[2] * (state[1] - ctrl[1]) + x12_dot[0] = ctrl[2] * state[2] - self.g + + return (x7_dot[0], x8_dot[0], x9_dot[0], x10_dot[0], x11_dot[0], x12_dot[0]) + + def opt_ctrl(self, t, state, spat_deriv): + """ + :param t: time t + :param state: tuple of coordinates in 6 dimensions + :param spat_deriv: spatial derivative in all dimensions + :return: tuple of optimal control + """ + + # Optimal control 1, 2, 3 + uOpt1 = hcl.scalar(0, "uOpt1") + uOpt2 = hcl.scalar(0, "uOpt2") + uOpt3 = hcl.scalar(0, "uOpt3") + + uOpt3UU = hcl.scalar(0, "uOpt3UU") + uOpt3UL = hcl.scalar(0, "uOpt3UL") + uOpt3LU = hcl.scalar(0, "uOpt3LU") + uOpt3LL = hcl.scalar(0, "uOpt3LL") + + SumUU = hcl.scalar(0, "SumUU") + SumUL = hcl.scalar(0, "SumUL") + SumLU = hcl.scalar(0, "SumLU") + SumLL = hcl.scalar(0, "SumLL") + + with hcl.if_(self.uMode == "min"): + # Ham value over all combinations of (u1, u2), ignoring u3 + # In each case u3 is determined from (u1, u2) + SumUU[0] = spat_deriv[3] * (state[0] - self.uMax[0]) + \ + spat_deriv[4] * (state[1] - self.uMax[1]) + \ + spat_deriv[5] * state[2] + with hcl.if_(SumUU[0] > 0): # Pick u3 + uOpt3UU[0] = self.uMin[2] + with hcl.else_(): + uOpt3UU[0] = self.uMax[2] + SumUU[0] = SumUU[0] * uOpt3UU[0] + + SumUL[0] = spat_deriv[3] * (state[0] - self.uMax[0]) + \ + spat_deriv[4] * (state[1] - self.uMin[1]) + \ + spat_deriv[5] * state[2] + with hcl.if_(SumUL[0] > 0): # Pick u3 + uOpt3UL[0] = self.uMin[2] + with hcl.else_(): + uOpt3UL[0] = self.uMax[2] + SumUL[0] = SumUL[0] * uOpt3UL[0] + + SumLU[0] = spat_deriv[3] * (state[0] - self.uMin[0]) + \ + spat_deriv[4] * (state[1] - self.uMax[1]) + \ + spat_deriv[5] * state[2] + with hcl.if_(SumLU[0] > 0): # Pick u3 + uOpt3LU[0] = self.uMin[2] + with hcl.else_(): + uOpt3LU[0] = self.uMax[2] + SumLU[0] = SumLU[0] * uOpt3LU[0] + + SumLL[0] = spat_deriv[3] * (state[0] - self.uMin[0]) + \ + spat_deriv[4] * (state[1] - self.uMin[1]) + \ + spat_deriv[5] * state[2] + with hcl.if_(SumLL[0] > 0): # Pick u3 + uOpt3LL[0] = self.uMin[2] + with hcl.else_(): + uOpt3LL[0] = self.uMax[2] + + SumLL[0] = SumLL[0] * uOpt3LL[0] + + # Go through Hamiltonian value for each case to pick the best (u1, u2, u3) + uOpt1[0] = self.uMax[0] + uOpt2[0] = self.uMax[1] + uOpt3[0] = uOpt3UU[0] + + with hcl.if_(SumUL[0] < SumUU[0]): + uOpt1[0] = self.uMax[0] + uOpt2[0] = self.uMin[1] + uOpt3[0] = uOpt3UL + SumUU[0] = SumUL[0] + + with hcl.if_(SumLU[0] < SumUU[0]): + uOpt1[0] = self.uMin[0] + uOpt2[0] = self.uMax[1] + uOpt3[0] = uOpt3LU + SumUU[0] = SumLU[0] + + with hcl.if_(SumLL[0] < SumUU[0]): + uOpt1[0] = self.uMin[0] + uOpt2[0] = self.uMin[1] + uOpt3[0] = uOpt3LL + + return (uOpt1[0], uOpt2[0], uOpt3[0]) + + def optDstb(self, spat_deriv): + """ + :param spat_deriv: spatial derivative in all dimensions + :return: tuple of optimal disturbance + """ + dOpt1 = hcl.scalar(0, "dOpt1") + dOpt2 = hcl.scalar(0, "dOpt2") + dOpt3 = hcl.scalar(0, "dOpt3") + dOpt4 = hcl.scalar(0, "dOpt4") + dOpt5 = hcl.scalar(0, "dOpt5") + dOpt6 = hcl.scalar(0, "dOpt6") + return (dOpt1[0], dOpt2[0], dOpt3[0], dOpt4[0], dOpt5[0], dOpt6[0]) + diff --git a/solver.py b/solver.py index de327181..3aa19c75 100644 --- a/solver.py +++ b/solver.py @@ -12,10 +12,6 @@ from computeGraphs.graph_6D import * import scipy.io as sio - - -import scipy.io as sio - import math diff --git a/user_definer.py b/user_definer.py index 631d0bec..f5ec0786 100644 --- a/user_definer.py +++ b/user_definer.py @@ -5,6 +5,8 @@ # Specify the file that includes dynamic systems from dynamics.Humannoid6D_sys1 import * from dynamics.DubinsCar4D import * +from dynamics.Humanoid12D_sys1 import * +from dynamics.Humanoid12D_sys2 import * import scipy.io as sio import math @@ -39,7 +41,7 @@ compMethod = "minVWithVInit" my_object = my_car my_shape = Initial_value_f """ - +""" g = grid(np.array([-5.0, -5.0, -1.0, -math.pi]), np.array([5.0, 5.0, 1.0, math.pi]), 4, np.array([40, 40, 50, 50]), [3]) # Define my object @@ -57,8 +59,62 @@ print("Welcome to optimized_dp \n") # Use the following variable to specify the characteristics of computation -compMethod = "none" +compMethod = "minVWithVInit" my_object = my_car my_shape = Initial_value_f +""" +""" +g = grid(np.array([-1, -1, -1, -5, -5, -5]), \ + np.array([ 1, 1, 1, 5, 5, 5]), \ + 6, \ + np.array([21, 21, 21, 21, 21, 21]), \ + ) +# Define my object +dyn_sys = Humanoid12D_sys1() + +# Use the grid to initialize initial value function +target_point = np.array([0, 0, 0, 0, 0, 0]) +Initial_value_f = Rect_around_point(g, target_point) +# Look-back lenght and time step +lookback_length = 2.0 +t_step = 0.05 + +small_number = 1e-5 +tau = np.arange(start = 0, stop = lookback_length + small_number, step = t_step) +print("Welcome to optimized_dp \n") + +# Use the following variable to specify the characteristics of computation +compMethod = "minVWithVInit" +my_object = dyn_sys +my_shape = Initial_value_f +""" +# Grid field in this order: min_range, max_range, number of dims, grid dimensions, list of periodic dim: starting at 0 + +g = grid(np.array([-0.5, -1, -0.5, -1, -0.5, -5]), \ + np.array([ 0.5, 1, 0.5, 1, 0.5, 5]), \ + 6, \ + np.array([21, 21, 21, 21, 21, 21]), \ + ) + +# Define my object +dyn_sys = Humanoid12D_sys2() + +# Use the grid to initialize initial value function +target_min = np.array([-0.05, -1.5*g.dx[1], -0.05, -1.5*g.dx[3], 1-1.5*g.dx[4], -1.5*g.dx[5]]) +target_max = np.array([ 0.05, 1.5*g.dx[1], 0.05, 1.5*g.dx[3], 1+1.5*g.dx[4], 1.5*g.dx[5]]) +Initial_value_f = ShapeRectangle(g, target_min, target_max) + +# Look-back lenght and time step +lookback_length = 2.0 +t_step = 0.05 + +small_number = 1e-5 +tau = np.arange(start = 0, stop = lookback_length + small_number, step = t_step) +print("Welcome to optimized_dp \n") + +# Use the following variable to specify the characteristics of computation +compMethod = "minVWithVInit" +my_object = dyn_sys +my_shape = Initial_value_f \ No newline at end of file diff --git a/user_definer_h12d1.py b/user_definer_h12d1.py new file mode 100644 index 00000000..c7cd5dcd --- /dev/null +++ b/user_definer_h12d1.py @@ -0,0 +1,49 @@ +import numpy as np +from Grid.GridProcessing import grid +from Shapes.ShapesFunctions import * + +# Specify the file that includes dynamic systems +from dynamics.Humanoid12D_sys1 import * +import scipy.io as sio + +import math + +""" USER INTERFACES +- Define grid + +- Generate initial values for grid using shape functions + +- Time length for computations + +- Run +""" + +# Grid field in this order: min_range, max_range, number of dims, grid dimensions, list of periodic dim: starting at 0 + +g = grid(np.array([-1, -1, -1, -5, -5, -5]), \ + np.array([ 1, 1, 1, 5, 5, 5]), \ + 6, \ + np.array([21, 21, 21, 21, 21, 21]), \ + ) + +# Define my object +dyn_sys = Humanoid12D_sys1() + +# Use the grid to initialize initial value function +target_point = np.array([0, 0, 0, 0, 0, 0]) +Initial_value_f = Rect_around_point(g, target_point) + +# Look-back lenght and time step +lookback_length = 2.0 +t_step = 0.05 + +small_number = 1e-5 +tau = np.arange(start = 0, stop = lookback_length + small_number, step = t_step) +print("Welcome to optimized_dp \n") + +# Use the following variable to specify the characteristics of computation +compMethod = "minVWithVInit" +my_object = dyn_sys +my_shape = Initial_value_f + + diff --git a/user_definer_h12d2.py b/user_definer_h12d2.py new file mode 100644 index 00000000..8e0ba4a2 --- /dev/null +++ b/user_definer_h12d2.py @@ -0,0 +1,48 @@ +import numpy as np +from Grid.GridProcessing import grid +from Shapes.ShapesFunctions import * + +# Specify the file that includes dynamic systems +from dynamics.Humanoid12D_sys1 import * +import scipy.io as sio + +import math + +""" USER INTERFACES +- Define grid + +- Generate initial values for grid using shape functions + +- Time length for computations + +- Run +""" + +# Grid field in this order: min_range, max_range, number of dims, grid dimensions, list of periodic dim: starting at 0 + +g = grid(np.array([-0.5, -1, -0.5, -1, -0.5, -5]), \ + np.array([ 0.5, 1, 0.5, 1, 0.5, 5]), \ + 6, \ + np.array([21, 21, 21, 21, 21, 21]), \ + ) + +# Define my object +dyn_sys = Humanoid12D_sys2() + +# Use the grid to initialize initial value function +target_min = np.array([-0.05, -1.5*g.dx[1], -0.05, -1.5*g.dx[3], 1-1.5*g.dx[4], -1.5*g.dx[5]]) +target_max = np.array([ 0.05, 1.5*g.dx[1], 0.05, 1.5*g.dx[3], 1+1.5*g.dx[4], 1.5*g.dx[5]]) +Initial_value_f = ShapeRectangle(g, target_min, target_max) + +# Look-back lenght and time step +lookback_length = 2.0 +t_step = 0.05 + +small_number = 1e-5 +tau = np.arange(start = 0, stop = lookback_length + small_number, step = t_step) +print("Welcome to optimized_dp \n") + +# Use the following variable to specify the characteristics of computation +compMethod = "minVWithVInit" +my_object = dyn_sys +my_shape = Initial_value_f \ No newline at end of file