async def _emparse(self, ctx, embed: Embed, fields: tuple) -> tuple:
i = 0
content = None
while i < len(fields):
if fields[i] == 'content':
content = fields[i]
i += 2
continue
# get a function callback using a string that corresponds to its name
embuilder = utils.EmbedBuilder(ctx)
method = getattr(embuilder, fields[i], None)
if method == None:
raise commands.BadArgument
# extract the number of parameters
params = len(sig(method).parameters)
# create the embed, catching any embed limit violations
try:
await method(embed, *fields[i + 1:i + params])
if len(embed) > 6000: raise utils.EmbedException('embed', 6000)
except utils.EmbedException as err:
embed.set_author(name=err.what(),
icon_url='attachment://unknown.png')
embed.color = utils.Color.red
await ctx.reply(embed=embed,
file=File('assets/reddot.png', 'unknown.png'))
i += params
return content
def get_instance(cls, type_name, params=None, **kwargs):
"""Get instance."""
_params = deepcopy(params)
if not _params:
return
t_cls_name = _params.pop('type')
if kwargs:
_params.update(kwargs)
t_cls = cls.get_cls(type_name, t_cls_name)
if type_name != ClassType.NETWORK:
return t_cls(**_params) if _params else t_cls()
# remove extra params
params_sig = sig(t_cls.__init__).parameters
for k, v in params_sig.items():
try:
if '*' in str(v) and '**' not in str(v):
return t_cls(*list(_params.values())) if list(_params.values()) else t_cls()
if '**' in str(v):
return t_cls(**_params) if _params else t_cls()
except Exception as ex:
logging.error("Failed to create instance:{}".format(t_cls))
raise ex
extra_param = {k: v for k, v in _params.items() if k not in params_sig}
_params = {k: v for k, v in _params.items() if k not in extra_param}
try:
instance = t_cls(**_params) if _params else t_cls()
except Exception as ex:
logging.error("Failed to create instance:{}".format(t_cls))
raise ex
for k, v in extra_param.items():
setattr(instance, k, v)
return instance
def getDefaultArgs(func):
signature = sig(func)
return {
k: v.default
for k, v in signature.parameters.items()
if v.default is not Parameter.empty
}
def get_viz_value_str(self):
res_str = str(self.prop_value)
if hasattr(self.prop_value, "__impl_origin__"):
res_str = "Impl. from " + self.prop_value.__impl_origin__ + ": " + \
self.prop_value.__name__ + str(sig(self.prop_value))
return res_str
def wrapper(*args, **kwargs):
"""Make function as a wrapper."""
params_sig = sig(func).parameters
params = {
param: value
for param, value in kwargs.items() if param in params_sig
}
return func(*args, **params)
def func_key(func, fargs=None, fkwargs=None):
"""
Returns the key used in :attr:`Energy.values` for `func`.
Parameters
----------
func : :class:`function`
The function whose results are to be stored in
:attr:`Energy.values`.
fargs : :class:`tuple`, optional
The arguments passed to `func`.
fkwargs : :class:`dict`, optional
The keyword arguments passed to `func`.
Returns
-------
:class:`.FunctionData`
The :class:`.FunctionData` object representing the key when
`func` is called with the arguments `fargs` and keyword
arguments `fkwargs`.
"""
if fargs is None:
fargs = []
if fkwargs is None:
fkwargs = []
# Check if the function has a `key` attribute. If it does use this
# to get its key rather than the general purpose code written here.
if hasattr(getattr(Energy, func.__name__, False), 'key'):
return getattr(Energy, func.__name__).key(fargs, fkwargs)
fsig = sig(func)
# Get a dictionary of all the supplied parameters.
bound = dict(fsig.bind_partial(*fargs, **fkwargs).arguments)
# Get a dictionary of all the default initialized parameters.
default = {
key: value.default
for key, value in dict(fsig.parameters).items()
if key not in bound.keys()
}
# Combine the two sets of parameters and get rid of the `self`
# parameter, if present.
bound.update(default)
if 'self' in bound.keys():
bound.pop('self')
# Remove any parameters that should not form key, listed in the
# `exclude` attribute.
if hasattr(func, 'exclude'):
for key in func.exclude:
bound.pop(key)
# Return an FunctionData object representing the function and
# chosen parameters.
return FunctionData(func.__name__, **bound)
def curried(*args):
if len(args) >= len(sig(func).parameters):
return func(*args)
else:
def partial(*args2):
return curried(*(args + args2))
return partial
def tune(default=None, tuning_range=(), args=None, name=None, tuner=None):
"""
Return TuneInt/Enum/Float instantiated obj
Property @val will return actual value
----------
Parameters:
default : value adopted in deafault mode
tuning_range : dimensional search space
args : list of args for function
name : identifier for var look-up
----------
Return:
value : string float or integer
"""
if default == None: # start tuning
assert tuner, "not specify tuner"
start()
return
if name: # checke name validity
import uptune as ut
assert name not in ut.mapping.keys(), \
"invalid name for registeration"
if isinstance(tuning_range, list):
assert len(tuning_range) > 0, \
"must specify the tuning range as list"
assert default in tuning_range, \
"default should be in list"
return types.TuneEnum(default, tuning_range, name=name).val
# custom func defining enum inter-deps
elif callable(tuning_range):
assert args, "args for function not specified"
assert len(args) == len(sig(tuning_range).parameters), \
"parameter number not match"
return types.TuneEnum(default, tuning_range, args=args, name=name).val
# create nodes for numerical ops
assert isinstance(tuning_range, tuple), \
"tuple range for boolean and numerical values"
if len(tuning_range) == 2:
lower, upper = tuning_range
if isinstance(lower, float) or isinstance(upper, float):
return types.TuneFloat(default, tuning_range, name=name).val
return types.TuneInt(default, tuning_range, name=name).val
# create permutation or boolean param
assert len(tuning_range) == 0 and \
isinstance(default, (bool, list)), \
"for boolean or permutation range should be ()"
if isinstance(default, bool):
return types.TuneBool(default, name=name).val
else: # create permutation param
return types.TunePermutation(default, name=name).val
def pseudoformation_key(fargs, fkwargs):
"""
Generates key of the :meth:`Energy.pseudoformation`.
Parameters
----------
fargs : :class:`tuple`
The arguments with which :meth:`Energy.pseudoformation` was
called.
fkwargs : :class:`dict`
The keyword arguments with which :meth:`Energy.pseudoformation`
was called.
Returns
-------
:class:`.FunctionData`
The :class:`.FunctionData` object representing the key when
:meth:`Energy.pseudoformation` is called with the arguments
`fargs` and keyword arguments `fkwargs`.
"""
fsig = sig(Energy.pseudoformation)
# Get a dictionary of all the supplied parameters.
bound = dict(fsig.bind_partial(*fargs, **fkwargs).arguments)
# Get a dictionary of all the default initialized parameters.
default = {
key: value.default
for key, value in dict(fsig.parameters).items()
if key not in bound.keys()
}
# Combine the two sets of parameters and get rid of the `self`
# parameter, if present.
bound.update(default)
if 'self' in bound.keys():
bound.pop('self')
# Replace the energy function to be used with the key of the
# energy function to be used.
efuncdata = bound['func']
efunc = getattr(Energy, efuncdata.name)
bound['func'] = func_key(efunc, None, efuncdata.params)
# Don't want this paramter in the key as it doenst affect the
# result.
bound.pop('force_e_calc')
# Return an FunctionData object representing the function and
# chosen parameters.
return FunctionData('pseudoformation', **bound)
def __new__(cls, *args, **kwargs):
"""Record params."""
desc = {}
params_sig = sig(cls.__init__).parameters
param_names = list(params_sig.keys())
if len(param_names) > len(args):
# not dynamic parameter for connections
for idx, arg in enumerate(args):
arg_name = param_names[idx + 1]
desc[arg_name] = arg
if kwargs:
desc.update(kwargs)
instance = super(Serializable, cls).__new__(cls)
instance.desc = Config(desc) if desc else {}
return instance
def decorate(func):
if not __debug__:
return func
fun_sig = sig(func)
# 将签名与参数字典绑定
bound_types = fun_sig.bind_partial(*ty_args, **ty_kwargs).arguments
@wraps(func)
def wrapper(*args, **kwargs):
# 参数顺序字典
bound_values = fun_sig.bind(*args, **kwargs)
for name, value in bound_values.arguments.items():
if name in bound_types:
if not isinstance(value, bound_types[name]):
raise TypeError('Argument {} must be {}'.format(
name, bound_types[name]))
return func(*args, **kwargs)
return wrapper
def accumulate(operator, iterable, default=None):
"""repeatedly call function with args last_result, item. first call args is item[0], item[1]"""
from inspect import signature as sig
i = iter(iterable)
try:
r = next(i)
except StopIteration:
# raise ArithmeticError("! attempted arithmetic on 0 items")
return default
while True:
try:
item = next(i)
except StopIteration:
break
try:
r = operator(r, item)
except TypeError as e:
if len(sig(operator).parameters) != 2:
raise TypeError("! operator function should take two arguments")
raise e
return r
def get_instance(cls, type_name, params=None, **kwargs):
"""Get instance."""
_params = deepcopy(params)
if not _params:
return
t_cls_name = _params.pop('type')
if kwargs:
_params.update(kwargs)
t_cls = cls.get_cls(type_name, t_cls_name)
if type_name != ClassType.NETWORK:
return t_cls(**_params) if _params else t_cls()
# remove extra params
params_sig = sig(t_cls.__init__).parameters
for k, v in params_sig.items():
if '**' in str(v) or '*' in str(v):
return t_cls(**_params) if _params else t_cls()
extra_param = {k: v for k, v in _params.items() if k not in params_sig}
_params = {k: v for k, v in _params.items() if k not in extra_param}
instance = t_cls(**_params) if _params else t_cls()
for k, v in extra_param.items():
setattr(instance, k, v)
return instance
def _generate_disp_info(
func
) -> Tuple[Dict[str, _Param], Dict[str, _AnnotationPair],
List[_AnnotationPair]]:
defaults: Dict[str, _Param]
keywords: Dict[str, _AnnotationPair]
positionals: List[_AnnotationPair]
defaults, keywords, positionals = {}, {}, []
for n, p in sig(func).parameters.items():
ann = p.annotation
if ann is Parameter.empty:
raise ValueError(f"parameter '{n}' is not annotated.")
if not isinstance(ann, Annotation):
raise TypeError(
f"unexpected annotation type: {type(ann).__name__}")
defaults[n] = p.default if p.default is not Parameter.empty else None
if ann.positional:
positionals.append((n, ann))
continue
keywords[_to_param_name(n)] = (n, ann)
if ann.short_name:
keywords[f"-{ann.short_name}"] = (n, ann)
positionals.sort(key=lambda x: not x[1].required)
return defaults, keywords, positionals
def shargs(func):
from inspect import signature as sig
from sys import argv
if '.py' in argv[0]:
del argv[0]
else:
del argv[0:1]
if argv[0] in ('-h', '--help'):
help(func)
exit()
sig = sig(func)
params = sig.parameters.values()
args = []
kwargs = {}
for par in params:
key = par.name
mod = par.kind
typ = 0
if len(dir(par.default)) != 26:
typ = type(par.default)
elif len(dir(par.annotation)) != 26:
typ = par.annotation
if typ not in [str, int, float, bool, list, tuple]:
del typ
_arg = par.POSITIONAL_OR_KEYWORD
_args = par.VAR_POSITIONAL
_kwargs = par.VAR_KEYWORD
_pos = par.POSITIONAL_ONLY
_key = par.KEYWORD_ONLY
if len(argv) > 0:
if mod in (_arg, _pos):
if f'--{key}' in argv:
i = argv.index(f'--{key}')
if 'typ' in locals():
val = typ(argv[i + 1])
else:
val = argv[i + 1]
args.append(val)
del argv[i]
del argv[i]
else:
if 'typ' in locals():
val = typ(argv[0])
else:
val = argv[0]
args.append(val)
del argv[0]
elif mod == _args:
s = e = 0
for a in argv:
if 'typ' in locals():
val = typ(a)
else:
val = a
if f'--' in a:
break
else:
args.append(val)
e += 1
del argv[s:e]
else:
if f'--{key}' in argv:
i = argv.index(f'--{key}')
if 'typ' in locals():
val = typ(argv[i + 1])
else:
val = argv[i + 1]
kwargs[key] = val
return func(*args, **kwargs)
def arityof(method):
return len([
param for param in sig(method).parameters.values()
if param.kind not in (Parameter.VAR_POSITIONAL, Parameter.VAR_KEYWORD)
]) if callable(method) else -1
def p_arityof(method):
return len([
param for param in sig(method).parameters.values()
if param.kind in (Parameter.POSITIONAL_ONLY,
Parameter.POSITIONAL_OR_KEYWORD)
]) if callable(method) else -1
def params(f) -> int:
return len(sig(f).parameters)
def simulation_OFC(self,ncmE,ncmC,f,g,Cfun,h,dt,tf,x0,z0,Qe,Qc,Re,Rc,\
dscale=10,xnames="num",Ncol=1,FigSize=(20,10),FontSize=20,phis=None):
"""
Perform NCM-based output feedback control of given nolinear dynamical
systems and return simulation results
Parameters
----------
dt : float
simulation time step
tf : float
terminal time
x0 : ndarray - (n, )
initial state
z0 : ndarray - (n, ), to be used for iEC = "est"
estimated initial state
dscale : float, optional, default is 10
scale of external disturbance
xnames : str, optional, default is "num"
list containing names of each state, when xnames = "num", they are
denoted as xnames = ["state 1","state 2",...]
Ncol : int, optional, default is 1
# columns of state figures to be generated
FigSize : tuple, optional, default is (20,10)
size of state figures to be generated
FontSize : float, optional, default is 20
font size of figures to be generated
phis : system parameter history, optional, default is None
history of system parameters
Returns
-------
this : ndarray - (int(tf/dt)+1, )
time histry
xhis : ndarray - (int(tf/dt)+1,n)
state history
zhis : ndarray - (int(tf/dt)+1,n), to be used for estimation tasks
estimated state history
"""
"""
1) SIMULATION
"""
if len(sig(f).parameters) == 1:
fun1 = f
f = lambda x, p: fun1(x)
if len(sig(g).parameters) == 1:
fun2 = g
g = lambda x, p: fun2(x)
if len(sig(Cfun).parameters) == 1:
fun3 = Cfun
Cfun = lambda x, p: fun3(x)
if len(sig(h).parameters) == 1:
fun4 = h
h = lambda x, p: fun4(x)
print("========================================================")
print("====================== SIMULATIOM ======================")
print("========================================================")
if dt <= self.dt_rk:
self.dt_rk = dt
self.Nrk = int(dt / self.dt_rk)
Nsim = int(tf / dt)
np.set_printoptions(precision=1)
print("time step =", dt)
print("terminal time =", tf)
print("initial state =", x0)
print("estimated initial state =", z0)
funx = lambda x, p, dEf: f(x, p) + dEf(x, p)
z = z0
zhis = np.zeros((Nsim + 1, self.n))
zhis[0, :] = z
x = x0
xhis = np.zeros((Nsim + 1, self.n))
xhis[0, :] = x
z2 = z0
z2his = np.zeros((Nsim + 1, self.n))
z2his[0, :] = z2
x2 = x0
x2his = np.zeros((Nsim + 1, self.n))
x2his[0, :] = x2
uehis = np.zeros(Nsim + 1)
ue = 0
ue2his = np.zeros(Nsim + 1)
ue2 = 0
tit1 = "Performance of NCM-based Output Feedback (1)"
tit2 = "Performance of NCM-based Output Feedback (2)"
tit3 = "Performance of NCM-based Output Feedback (3)"
tit4 = "Performance of NCM-based Output Feedback (4)"
tit5 = "Performance of NCM-based Output Feedback (5)"
ly = r"estimation error: $\|x-\hat{x}\|_2$"
lyb = r"tracking error: $\|x-x_d\|_2$"
lyc = r"control effort $\int_0^t\|u(\tau)\|_2d\tau$"
l1 = r"estimation error (NCM)"
l2 = r"estimation error (SDRE)"
l1b = r"tracking error (NCM)"
l2b = r"tracking error (SDRE)"
bNam1 = "=================== ESTIMATION ERROR ==================="
bNam2 = "============ ESTIMATION ERROR OF EACH STATE ============"
bNam3 = "==================== TRACKING ERROR ===================="
bNam4 = "============= TRACKING ERROR OF EACH STATE ============="
bNam5 = "==================== CONTROL EFFORT ===================="
l3 = r"optimal steady-state upper bound"
if phis == None:
phis = np.linspace(self.plims[0, :], self.plims[1, :], Nsim)
for k in range(Nsim):
p = phis[k, :]
Ax2 = self.Afun(z2, p)
Mc = ncmC.ncm(z, p)
Kc, _, _ = control.lqr(Ax2, g(z2, p), Qc, Rc)
u = -g(z, p).T @ Mc @ z
u2 = -Kc @ z2
dEfC = lambda x, p: g(x, p) @ u
dEfC2 = lambda x, p: g(x, p) @ u2
d1 = self.unifrand2(ncmC.d1_over, np.size(ncmC.Bw(x, p),
1)) * dscale
x = self.rk4(x, p, dEfC, funx) + ncmC.Bw(x, p) @ d1 * dt
x2 = self.rk4(x2, p, dEfC2, funx) + ncmC.Bw(x2, p) @ d1 * dt
xhis[k + 1, :] = x
x2his[k + 1, :] = x2
Me = ncmE.ncm(z, p)
Cx = Cfun(z, p)
Lx = Me @ Cx.T
Cx2 = Cfun(z2, p)
Ke, _, _ = control.lqr(Ax2.T, Cx2.T, Qe, Re)
Lx2 = Ke.T
#Lx = K.T
d2 = self.unifrand2(ncmE.d2_over, np.size(ncmE.Gw(x, p),
1)) * dscale
y = h(x, u, p) + ncmE.Gw(x, p) @ d2
y2 = h(x2, u2, p) + ncmE.Gw(x2, p) @ d2
funz = lambda z, p, dEf: f(z, p) + g(z, p) @ u + dEf(z, p)
funz2 = lambda z, p, dEf: f(z, p) + g(z, p) @ u2 + dEf(z, p)
dEfE = lambda z, p: Lx @ (y - h(z, u, p))
dEfE2 = lambda z, p: Lx2 @ (y2 - h(z, u2, p))
z = self.rk4(z, p, dEfE, funz)
z2 = self.rk4(z2, p, dEfE2, funz2)
zhis[k + 1, :] = z
z2his[k + 1, :] = z2
ue += np.linalg.norm(u) * dt
ue2 += np.linalg.norm(u2) * dt
uehis[k + 1] = ue
ue2his[k + 1] = ue2
this = np.linspace(0, tf, Nsim + 1)
"""
2) FIGURE GENERATION
"""
print("========================================================")
print(bNam1)
print("========================================================")
matplotlib.rcParams.update({"font.size": 15})
matplotlib.rc("text", usetex=True)
plt.figure()
plt.plot(this, np.sqrt(np.sum((xhis - zhis)**2, 1)))
plt.plot(this, np.sqrt(np.sum((x2his - z2his)**2, 1)))
plt.plot(this, np.ones(np.size(this)) * ncmE.Jcv_opt)
plt.xlabel(r"time", fontsize=FontSize)
plt.ylabel(ly, fontsize=FontSize)
plt.legend([l1, l2, l3], loc="best")
plt.title(tit1, fontsize=FontSize)
plt.show()
print("========================================================")
print(bNam2)
print("========================================================")
Nrow = int(self.n / Ncol) + np.remainder(self.n, Ncol)
fig, ax = plt.subplots(Nrow, Ncol, figsize=FigSize)
plt.subplots_adjust(wspace=0.25, hspace=0.25)
if Ncol == 1:
ax = np.reshape(ax, (self.n, 1))
elif Nrow == 1:
ax = np.reshape(ax, (1, self.n))
if xnames == "num":
xnames = []
for i in range(self.n):
xnames += [r"state " + str(i + 1)]
for row in range(Nrow):
for col in range(Ncol):
i = Ncol * row + col
if i + 1 <= self.n:
ax[row, col].plot(this, xhis[:, i] - zhis[:, i])
ax[row, col].plot(this, x2his[:, i] - z2his[:, i])
ax[row, col].set_xlabel(r"time", fontsize=FontSize)
LabelName = r"estimation error: " + xnames[i]
ax[row, col].set_ylabel(LabelName, fontsize=FontSize)
ax[row, col].legend([r"NCM", r"SDRE"], loc="best")
fig.suptitle(tit2, fontsize=FontSize)
plt.show()
print("========================================================")
print(bNam3)
print("========================================================")
matplotlib.rcParams.update({"font.size": 15})
matplotlib.rc("text", usetex=True)
plt.figure()
plt.plot(this, np.sqrt(np.sum((xhis)**2, 1)))
plt.plot(this, np.sqrt(np.sum((x2his)**2, 1)))
plt.plot(this, np.ones(np.size(this)) * ncmC.Jcv_opt)
plt.xlabel(r"time", fontsize=FontSize)
plt.ylabel(lyb, fontsize=FontSize)
plt.legend([l1b, l2b, l3], loc="best")
plt.title(tit3, fontsize=FontSize)
plt.show()
print("========================================================")
print(bNam4)
print("========================================================")
Nrow = int(self.n / Ncol) + np.remainder(self.n, Ncol)
fig, ax = plt.subplots(Nrow, Ncol, figsize=FigSize)
plt.subplots_adjust(wspace=0.25, hspace=0.25)
if Ncol == 1:
ax = np.reshape(ax, (self.n, 1))
elif Nrow == 1:
ax = np.reshape(ax, (1, self.n))
if xnames == "num":
xnames = []
for i in range(self.n):
xnames += [r"state " + str(i + 1)]
for row in range(Nrow):
for col in range(Ncol):
i = Ncol * row + col
if i + 1 <= self.n:
ax[row, col].plot(this, xhis[:, i])
ax[row, col].plot(this, x2his[:, i])
ax[row, col].set_xlabel(r"time", fontsize=FontSize)
LabelName = r"tracking error: " + xnames[i]
ax[row, col].set_ylabel(LabelName, fontsize=FontSize)
ax[row, col].legend([r"NCM", r"SDRE"], loc="best")
fig.suptitle(tit4, fontsize=FontSize)
plt.show()
print("========================================================")
print(bNam5)
print("========================================================")
matplotlib.rcParams.update({"font.size": 15})
matplotlib.rc("text", usetex=True)
plt.figure()
plt.plot(this, uehis)
plt.plot(this, ue2his)
plt.xlabel(r"time", fontsize=FontSize)
plt.ylabel(lyc, fontsize=FontSize)
plt.legend([r"NCM", r"SDRE"], loc="best")
plt.title(tit5, fontsize=FontSize)
plt.show()
print("========================================================")
print("==================== SIMULATIOM END ====================")
print("========================================================")
return this, xhis, zhis
def simulation(self,dt,tf,x0,z0=None,dscale=10.0,xnames="num",Ncol=1,\
FigSize=(20,10),FontSize=20,phis=None):
"""
Perform NCM-based estimation or control of given nolinear dynamical
systems and return simulation results
Parameters
----------
dt : float
simulation time step
tf : float
terminal time
x0 : ndarray - (n, )
initial state
z0 : ndarray - (n, ), to be used for iEC = "est"
estimated initial state
dscale : float, optional, default is 10
scale of external disturbance
xnames : str, optional, default is "num"
list containing names of each state, when xnames = "num", they are
denoted as xnames = ["state 1","state 2",...]
Ncol : int, optional, default is 1
# columns of state figures to be generated
FigSize : tuple, optional, default is (20,10)
size of state figures to be generated
FontSize : float, optional, default is 20
font size of figures to be generated
phis : system parameter history, optional, default is None
history of system parameters
Returns
-------
this : ndarray - (int(tf/dt)+1, )
time histry
xhis : ndarray - (int(tf/dt)+1,n)
state history
zhis : ndarray - (int(tf/dt)+1,n), to be used for estimation tasks
estimated state history
"""
"""
1) SIMULATION
"""
print("========================================================")
print("====================== SIMULATIOM ======================")
print("========================================================")
if dt <= self.dt_rk:
self.dt_rk = dt
self.Nrk = int(dt / self.dt_rk)
Nsim = int(tf / dt)
np.set_printoptions(precision=1)
print("time step =", dt)
print("terminal time =", tf)
print("initial state =", x0)
if self.iEC == "est":
print("estimated initial state =", z0)
funx = lambda x, p, u: self.dynamicsf(x, p)
funz = self.dynamics
z = z0
zhis = np.zeros((Nsim + 1, self.n))
zhis[0, :] = z
tit1 = "Performance of NCM-based Estimation (1)"
tit2 = "Performance of NCM-based Estimation (2)"
ly = r"estimation error: $\|x-\hat{x}\|_2$"
l1 = r"estimation error"
bNam1 = "=================== ESTIMATION ERROR ==================="
bNam2 = "============ ESTIMATION ERROR OF EACH STATE ============"
elif self.iEC == "con":
funx = self.dynamics
zhis = np.zeros((Nsim + 1, self.n))
tit1 = "Performance of NCM-based Control (1)"
tit2 = "Performance of NCM-based Control (2)"
ly = r"tracking error: $\|x-x_d\|_2$"
l1 = r"tracking error"
bNam1 = "==================== TRACKING ERROR ===================="
bNam2 = "============= TRACKING ERROR OF EACH STATE ============="
else:
raise ValueError('Invalid iEC: iEC = "est" or "con"')
l2 = r"optimal steady-state upper bound"
x = x0
xhis = np.zeros((Nsim + 1, self.n))
xhis[0, :] = x
this = np.linspace(0, tf, Nsim + 1)
if phis == None:
phis = np.linspace(self.plims[0, :], self.plims[1, :], Nsim)
if (self.iEC == "est") and (len(sig(self.Cfun).parameters) == 1):
fun1 = self.Cfun
self.Cfun = lambda x, p: fun1(x)
if (len(sig(self.Bw).parameters) == 1):
fun2 = self.Bw
self.Bw = lambda x, p: fun2(x)
if (self.iEC == "est") and (len(sig(self.Gw).parameters) == 1):
fun3 = self.Gw
self.Gw = lambda x, p: fun3(x)
for k in range(Nsim):
p = phis[k, :]
if self.iEC == "est":
Mx = self.ncm(z, p)
Cx = self.Cfun(z, p)
Lx = Mx @ Cx.T
d2 = self.unifrand2(self.d2_over, np.size(self.Gw(x, p),
1)) * dscale
y = self.h_or_g(x, p) + self.Gw(x, p) @ d2
dEf = lambda z, p: Lx @ (y - self.h_or_g(z, p))
z = self.rk4(z, p, dEf, funz)
zhis[k + 1, :] = z
elif self.iEC == "con":
Mx = self.ncm(x, p)
Bx = self.h_or_g(x, p)
Kx = Bx.T @ Mx
u = -Kx @ x
dEf = lambda x, p: self.h_or_g(x, p) @ u
else:
raise ValueError('Invalid iEC: iEC = "est" or "con"')
d1 = self.unifrand2(self.d1_over, np.size(self.Bw(x, p),
1)) * dscale
x = self.rk4(x, p, dEf, funx) + self.Bw(x, p) @ d1 * dt
xhis[k + 1, :] = x
"""
2) FIGURE GENERATION
"""
print("========================================================")
print(bNam1)
print("========================================================")
matplotlib.rcParams.update({"font.size": 15})
matplotlib.rc("text", usetex=True)
plt.figure()
plt.plot(this, np.sqrt(np.sum((xhis - zhis)**2, 1)))
plt.plot(this, np.ones(np.size(this)) * self.Jcv_opt)
plt.xlabel(r"time", fontsize=FontSize)
plt.ylabel(ly, fontsize=FontSize)
plt.legend([l1, l2], loc="best")
plt.title(tit1, fontsize=FontSize)
plt.show()
print("========================================================")
print(bNam2)
print("========================================================")
Nrow = int(self.n / Ncol) + np.remainder(self.n, Ncol)
fig, ax = plt.subplots(Nrow, Ncol, figsize=FigSize)
plt.subplots_adjust(wspace=0.25, hspace=0.25)
if Ncol == 1:
ax = np.reshape(ax, (self.n, 1))
elif Nrow == 1:
ax = np.reshape(ax, (1, self.n))
if xnames == "num":
xnames = []
for i in range(self.n):
xnames += [r"state " + str(i + 1)]
for row in range(Nrow):
for col in range(Ncol):
i = Ncol * row + col
if i + 1 <= self.n:
ax[row, col].plot(this, xhis[:, i] - zhis[:, i])
ax[row, col].set_xlabel(r"time", fontsize=FontSize)
if self.iEC == "est":
LabelName = r"estimation error: " + xnames[i]
elif self.iEC == "con":
LabelName = r"tracking error: " + xnames[i]
else:
txterr = 'Invalid iEC: iEC = "est" or "con"'
raise ValueError(txterr)
ax[row, col].set_ylabel(LabelName, fontsize=FontSize)
fig.suptitle(tit2, fontsize=FontSize)
plt.show()
print("========================================================")
print("==================== SIMULATIOM END ====================")
print("========================================================")
return this, xhis, zhis
def cvstem0(self, xs, ps, alp):
"""
Run one single instance of CV-STEM algorithm for given states xs and
contraction rate alpha
Parameters
----------
xs : ndarray - (Ncv,n), where Ncv is # state samples
state samples for solving CV-STEM
ps : ndarray - (Ncv,n_p), where Ncv is # state samples
system parameter samples for solving CV-STEM
alp : float
contraction rate of interest
Objects to be updated
-------
Ws : list of length Ncv
list containing inverse of n by n optimal contraction metrics
in current instance of CV-STEM
chi : numpy.float64
optimal upper bound of condition number of contraction metrics
in current instance of CV-STEM
nu : numpy.float64
optimal upper bound of induced 2-norm of contraction metrics
in current instance of CV-STEM
Jcv : numpy.float64
optimal steady-state upper bound of estimation or tracking error
in current instance of CV-STEM
cvx_status : str
problem status of CV-STEM, "optimal", "infeasible", "unbounded",
"infeasible_inaccurate", or "unbounded_inaccurate"
"""
epsilon = self.epsilon
Ncv = np.size(xs, 0)
n = self.n
I = np.identity(n)
Ws = []
for k in range(Ncv):
Ws.append(cp.Variable((n, n), PSD=True))
nu = cp.Variable(nonneg=True)
chi = cp.Variable(nonneg=True)
errtxt = "https://github.com/AstroHiro/ncm#troubleshooting"
if len(sig(self.Afun).parameters) == 1:
fun1 = self.Afun
self.Afun = lambda x, p: fun1(x)
if (self.iEC == "est") and (len(sig(self.Cfun).parameters) == 1):
fun2 = self.Cfun
self.Cfun = lambda x, p: fun2(x)
if self.iEC == "est":
Af = self.Afun
Cf = self.Cfun
J = (self.d1_over*self.b_over*chi\
+self.d2_over*self.c_over*self.g_over*nu)/alp
print(self.d1_over * self.b_over / alp)
print(self.d2_over * self.c_over * self.g_over / alp)
elif self.iEC == "con":
Af = lambda x, p: self.Afun(x, p).T
Cf = lambda x, p: self.h_or_g(x, p).T
J = self.d1_over * self.b_over * chi / alp + self.d2_over * nu
else:
raise ValueError('Invalid iEC: iEC = "est" or "con"')
constraints = []
for k in range(Ncv):
x = xs[k, :]
p = ps[k, :]
Ax = Af(x, p)
Cx = Cf(x, p)
W = Ws[k]
constraints += [chi * I - W >> 0, W - I >> 0]
constraints += [-2*alp*W-((W-I)/self.dt+W@Ax+Ax.T@W-2*nu*Cx.T@Cx)\
>> epsilon*I]
prob = cp.Problem(cp.Minimize(J), constraints)
prob.solve(solver=cp.MOSEK)
cvx_status = prob.status
if cvx_status in ["infeasible", "infeasible_inaccurate"]:
raise ValueError("Problem infeasible: see " + errtxt)
elif cvx_status in ["unbounded", "unbounded_inaccurate"]:
raise ValueError("Problem unbounded: " + errtxt)
Wsout = []
for k in range(Ncv):
Wk = Ws[k].value / nu.value
Wsout.append(Wk)
self.Ws = Wsout
self.nu = nu.value
self.chi = chi.value
self.Jcv = prob.value
self.cvx_status = cvx_status
pass
def __init__(self,dt,dynamicsf,h_or_g,xlims,alims,iEC,fname,d1_over=0.1,\
d2_over=0.1,da=0.1,Nx=1000,Nls=100,plims=np.empty((2,0))):
"""
This class provides several objects and methods for designing a Neural
Contraction Metric (NCM) of a given nonliner dynamical system both for
state estimation and feedback control.
See the NCM paper https://arxiv.org/abs/2006.04361 and
the CV-STEM paper https://arxiv.org/abs/2006.04359 for more details.
See https://github.com/AstroHiro/ncm/wiki/Documentation for the
documentation of this class file.
Parameters
(let n: state dimension and m: measurement or control input dimension)
----------
dt : float
discrete sampling period of CV-STEM
dynamicsf : function - ndarray (n,n_p) -> (n, )
vector field of given dynamical system
(i.e. f of dx/dt = f(x) or dx/dt = f(x)+g(x)u)
h_or_g : function - ndarray (n,n_p) -> (m, ) for h, -> (n,m) for g
measurement equation h or actuation matrix g
(i.e. h of y = h(x,p) or g of dx/dt = f(x,p)+g(x,p)u)
xlims : ndarray - (2,n)
lower and upper buonds of eash state
(i.e. xlims[0,:]: lower bounds, xlims[1,:]: upper bounds)
alims : ndarray - (2, )
lower and upper bound of contraction rate alpha
(i.e. alims[0]: lower bound, alims[0]: upper bound)
iEC : str
iEC = "est" for estimation and = "con" for control
fname : str
file name of your choice for storing NCM models and parameters
d1_over : float, optional, default is 0.1
upper bound of process noise
(i.e. d1_over or d_over in the NCM paper)
d2_over : float, optional, default is 0.1
upper bound of measurement noise or penalty on feedback gains
(i.e. d2_over or lambda in in the NCM paper)
da : float, optional, default is 0.1
step size of contraction rate alpha for line search in CV-STEM
Nx : int, optional, default is 1000
# samples of CV-STEM to be used for NCM training
Nls : int, optional, default is 100
# samples to be used for line search in CV-STEM
plims : ndarray - (2,n_p), default is np.empty((2,0))
lower and upper bound of system parameters
(i.e. plims[0,:]: lower bounds, plims[0,:]: upper bounds)
Any other objects to be updated
-------
n : int
state dimension
m : int
measurement or control input dimension
n_p : int
system parameter dimension
Afun : function - ndarray (n,n_p) -> (n,n)
Jacobian of dynamicsf (can be set to state-dependent coefficient
matrix A s.t. f(x) = A(x)x, see the CV-STEM paper for details)
Cfun : function - ndarray (n,n_p) -> (n,m), to be used for iEC = "est"
Jacobian of measurement equation h (can be set to C s.t.
h(x) = C(x)x, see the CV-STEM paper for details)
Bw : function - ndarray (n,n_p) -> (n,k1)
B(x) given in equation (9) or B_2(x) in equation (17) of the NCM
paper (B(x) = I and B_2(x) = g(x) are used by default, where g(x)
is actuation matrix)
Gw : function - ndarray (n,n_p) -> (m,k2), to be used for iEC = "est"
G(x) given in equation (9) of the NCM paper (G(x) = I is used by
default)
c_over : numpy.float64, to be used for iEC = "est"
approximate upper bound of Cfun(x) in given state space
b_over : numpy.float64
approximate upper bound of Bw(x) in given state space
g_over : numpy.float64, to be used for iEC = "est"
approximate upper bound of Gw(x) in given state space
model : keras neural net model - ndarray (k,n) -> (k,int(n*(n+1)/2))
function that returns cholesky-decomposed approximate optimal
contraction metrics (i.e. NCMs) for given k states
alp_opt : float
optimal contraction rate
chi_opt : numpy.float64
optimal upper bound of condition number of contraction metrics
nu_opt : numpy.float64
optimal upper bound of induced 2-norm of contraction metrics
Jcv_opt : numpy.float64
optimal steady-state upper bound of estimation or tracking error
xs_opt : ndarray - (Nx,n)
randomized state samples
Ws_opt : list of length Nx
list containing inverse of ndarray (n,n) optimal contraction
metrics sampled by CV-STEM
Ms_opt : list of length Nx
list containing ndarray (n,n) optimal contraction metrics sampled
by CV-STEM
cholMs : list of length Nx
list containing ndarray (int(n*(n+1)/2), ) optimal contraction
metrics sampled by CV-STEM
Ws : list of length Ncv
list containing inverse of n by n optimal contraction metrics
in current instance of CV-STEM
chi : numpy.float64
optimal upper bound of condition number of contraction metrics
in current instance of CV-STEM
nu : numpy.float64
optimal upper bound of induced 2-norm of contraction metrics
in current instance of CV-STEM
Jcv : numpy.float64
optimal steady-state upper bound of estimation or tracking error
in current instance of CV-STEM
cvx_status : str
problem status of CV-STEM, "optimal", "infeasible", "unbounded",
"infeasible_inaccurate", or "unbounded_inaccurate"
epsilon : float, default is 0.0
non-negative constant introduced to relax stability condition
dt_rk : float, default is 0.01
time step of numerical integration
"""
self.dt = dt
if (len(sig(dynamicsf).parameters) == 1):
self.dynamicsf = lambda x, p: dynamicsf(x)
else:
self.dynamicsf = dynamicsf
if (len(sig(h_or_g).parameters) == 1):
self.h_or_g = lambda x, p: h_or_g(x)
else:
self.h_or_g = h_or_g
self.xlims = xlims
self.alims = alims
self.iEC = iEC
self.fname = fname
self.d1_over = d1_over
self.d2_over = d2_over
self.da = da
self.Nx = Nx
self.Nls = Nls
self.n = np.size(xlims, 1)
self.m = np.size(self.h_or_g(xlims[0, :], plims[0, :]).T, 0)
self.n_p = np.size(plims, 1)
self.Afun = lambda x, p: self.jacobian(x, p, self.dynamicsf)
if self.iEC == "est":
self.Cfun = lambda x, p: self.jacobian(x, p, self.h_or_g)
self.Bw = lambda x, p: np.identity(self.n)
self.Gw = lambda x, p: np.identity(self.m)
elif self.iEC == "con":
self.Bw = self.h_or_g
else:
raise ValueError('Invalid iEC: iEC = "est" or "con"')
self.epsilon = 0
self.dt_rk = 0.01
self.plims = plims
def cvstem(self):
"""
Sample optimal contraction metrics by CV-STEM for constructing NCM
Objects to be updated
-------
c_over : numpy.float64, to be used for iEC = "est"
Approximate upper bound of Cfun(x) in given state space
b_over : numpy.float64
Approximate upper bound of Bw(x) in given state space
g_over : numpy.float64, to be used for iEC = "est"
Approximate upper bound of Gw(x) in given state space
xs_opt : ndarray - (Nx,n), where Nx is # samples to be used for NCM
randomized state samples
Ws_opt : list of length Nx
list containing inverse of ndarray (n,n) optimal contraction
metrics
chi_opt : numpy.float64
optimal upper bound of condition number of contraction metrics
nu_opt : numpy.float64
optimal upper bound of induced 2-norm of contraction metrics
Jcv_opt : numpy.float64
optimal steady-state upper bound of estimation or tracking error
"""
if (self.iEC == "est") and (len(sig(self.Cfun).parameters) == 1):
fun1 = self.Cfun
self.Cfun = lambda x, p: fun1(x)
if (self.iEC == "est") and (len(sig(self.Gw).parameters) == 1):
fun2 = self.Gw
self.Gw = lambda x, p: fun2(x)
if self.iEC == "est":
self.c_over = self.matrix_2bound(self.Cfun)
self.g_over = self.matrix_2bound(self.Gw)
if (len(sig(self.Bw).parameters) == 1):
fun3 = self.Bw
self.Bw = lambda x, p: fun3(x)
self.b_over = self.matrix_2bound(self.Bw)
self.linesearch()
alp = self.alp_opt
Nx = self.Nx
Nsplit = 1
Np = int(Nx / Nsplit)
Nr = np.remainder(Nx, Nsplit)
xpmin = np.hstack((self.xlims[0, :], self.plims[0, :]))
xpmax = np.hstack((self.xlims[1, :], self.plims[1, :]))
Nxp = self.n + self.n_p
xps = np.random.uniform(xpmin, xpmax, size=(Nx, Nxp))
xs_opt, ps_opt, _ = np.hsplit(xps, np.array([self.n, Nxp]))
Ws_opt = []
chi_opt = 0
nu_opt = 0
print("========================================================")
print("====== SAMPLING OF CONTRACTION METRICS BY CV-STEM ======")
print("========================================================")
for p in range(Np):
if np.remainder(p, int(Np / 10)) == 0:
print("# sampled metrics: ", p * Nsplit, "...")
xs_p = xs_opt[Nsplit * p:Nsplit * (p + 1), :]
ps_p = ps_opt[Nsplit * p:Nsplit * (p + 1), :]
self.cvstem0(xs_p, ps_p, alp)
Ws_opt += self.Ws
if self.nu >= nu_opt:
nu_opt = self.nu
if self.chi >= chi_opt:
chi_opt = self.chi
if Nr != 0:
print("# samples metrics: ", Nx, "...")
xs_p = xs_opt[Nsplit * (p + 1):Nx, :]
ps_p = ps_opt[Nsplit * (p + 1):Nx, :]
self.cvstem0(xs_p, ps_p, alp)
Ws_opt += self.Ws
if self.nu >= nu_opt:
nu_opt = self.nu
if self.chi >= chi_opt:
chi_opt = self.chi
self.xs_opt = xs_opt
self.ps_opt = ps_opt
self.Ws_opt = Ws_opt
self.chi_opt = chi_opt
self.nu_opt = nu_opt
if self.iEC == "est":
self.Jcv_opt = (self.d1_over*self.b_over*np.sqrt(chi_opt)\
+self.d2_over*self.c_over*self.g_over*nu_opt)/alp
print("Optimal steady-state estimation error =",\
"{:.2f}".format(self.Jcv_opt))
elif self.iEC == "con":
self.Jcv_opt = self.d1_over * self.b_over * np.sqrt(chi_opt) / alp
print("Optimal steady-state tracking error =",\
"{:.2f}".format(self.Jcv_opt))
else:
raise ValueError('Invalid iEC: iEC = "est" or "con"')
self.M2cholM()
path = "models/optvals/" + self.fname
if os.path.exists(path) == False:
try:
os.makedirs(path)
except:
raise OSError("Creation of directory %s failed" % path)
else:
print("Successfully created directory %s " % path)
else:
print("Directory %s already exists" % path)
np.save(path + "/alp_opt.npy", alp)
np.save(path + "/chi_opt.npy", self.chi_opt)
np.save(path + "/nu_opt.npy", self.nu_opt)
np.save(path + "/Jcv_opt.npy", self.Jcv_opt)
print("========================================================")
print("==== SAMPLING OF CONTRACTION METRICS BY CV-STEM END ====")
print("========================================================\n\n")
pass
def equals(a, b):
"Compares `a` and `b` for equality; supports sublists, tensors and arrays too"
cmp = (torch.equal if isinstance(a, Tensor) and a.dim() else
np.array_equal if isinstance(a, ndarray) else operator.eq
if isinstance(a, str) else all_equal if is_iter(a) else operator.eq)
return cmp(a, b)
# my own inspection helpers
from inspect import getdoc as doc, getsourcelines as source, getmodule as module, signature as sig, getmembers as member, getmro as clstree
def clstree(a):
return getmro(a)
def dr(a):
return a.__dir__()
def dt(a):
return a.__dict__
doc(DataLoader)
sig(DataLoader)
source(DataLoader)
module(DataLoader)
member(DataLoader)
def wrapped_f(*args, **kwargs):
# Check for if the target kwarg is missing
if self.kw not in kwargs:
# Missing. Must fetch.
# Retrieve and materialize the enumerated arguments list.
# Depends on the parameters being contained in an OrderedDict
# so that signature order is retained.
params = list(sig(f).parameters)
# Initialize as empty the arguments list to pass to the
# callable
fetch_args = []
# Populate fetch_args sequentially in the order specified by
# the particular decorator constructor
for a in self.arglist:
# Simple type checks on the argument specifiers
# should suffice since they were checked at
# construction.
if isinstance(a, str):
# Keyword argument; handle possible absence with get()
fetch_args.append(kwargs.get(a))
else:
# Integer argument for (optional-)positional args.
# Could be present as positional or as keyword,
# or could be absent.
if len(args) > a:
# Sufficient positional arguments; assume
# present and passed as optional-positional
fetch_args.append(args[a])
else:
# The **kwargs is not valid for this, so exclude
pname = params[:-1][a]
if pname in kwargs:
# Present in the passed-in kwargs
fetch_args.append(kwargs[pname])
else:
# Not found; pass the function default
fetch_args.append(
sig(f).parameters[pname].default)
# Populate fetch_kwargs according to what was specified
# at decorator construction
fetch_kwargs = {}
for item in self.kwarglist.items():
# Same as above -- simple type checks should suffice
if isinstance(item[1], str):
# Keyword argument; handle possible absence with get()
fetch_kwargs.update({item[0]: kwargs.get(item[1])})
else:
# Optional-positional
if len(args) > item[1]:
# Sufficient positional args
fetch_kwargs.update({item[0]: args[item[1]]})
else:
# The **kwargs is not valid for this, so exclude
pname = params[:-1][item[1]]
if pname in kwargs:
# Insufficient positional; add from kwargs
# if present
fetch_kwargs.update({item[0]: kwargs[pname]})
else:
# Not found; store the function default
fetch_kwargs.update({
item[0]:
sig(f).parameters[pname].default
})
# Call the callable and store the result into the target
# keyword
c_result = self.c(*fetch_args, **fetch_kwargs)
kwargs.update({self.kw: c_result})
# Whether the target kwarg was present or generated/injected,
# call the wrapped function
return f(*args, **kwargs)
def isnullary(method):
return callable(method) and len(sig(method).parameters) == 0
