Skip to content

humanoid12D #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mochen72 wants to merge 1 commit into
base: master
Choose a base branch
from
Open

humanoid12D #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions Shapes/ShapesFunctions.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
26 changes: 18 additions & 8 deletions dynamics/Humannoid6D_sys1.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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]
Expand All @@ -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]

Expand Down
124 changes: 124 additions & 0 deletions dynamics/Humanoid12D_sys1.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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])

169 changes: 169 additions & 0 deletions dynamics/Humanoid12D_sys2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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])

4 changes: 0 additions & 4 deletions solver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,6 @@
from computeGraphs.graph_6D import *
import scipy.io as sio



import scipy.io as sio

import math


Expand Down
Loading