def build_Constraint():
"""Build a Constraint and delete any references to external
objects so its size can be computed."""
expr = sum(x*c for x,c in zip(build_Constraint.xlist,
build_Constraint.clist))
obj = Constraint(expr=(0, expr, 1))
obj._parent = build_Constraint.dummy_parent
obj.construct()
obj._parent = None
return obj
def _Combine(port, name, index_set):
port_parent = port.parent_block()
var = port.vars[name]
in_vars = []
sources = port.sources(active=True)
if not len(sources):
return in_vars
if len(sources) == 1 and len(sources[0].source.dests(active=True)) == 1:
# This is a 1-to-1 connection, no need for evar, just equality.
arc = sources[0]
Port._add_equality_constraint(arc, name, index_set)
return in_vars
for arc in sources:
eblock = arc.expanded_block
# Make and record new variables for every arc with this member.
evar = Port._create_evar(port.vars[name], name, eblock, index_set)
in_vars.append(evar)
# Create constraint: var == sum of evars
# Same logic as Port._Split
cname = unique_component_name(port_parent, "%s_%s_insum" %
(alphanum_label_from_name(port.local_name), name))
def rule(m, *args):
if len(args):
return sum(evar[args] for evar in in_vars) == var[args]
else:
return sum(evar for evar in in_vars) == var
con = Constraint(index_set, rule=rule)
port_parent.add_component(cname, con)
return in_vars
def build_indexed_Constraint():
"""Build an indexed Constraint with no references to external
objects so its size can be computed."""
model = build_indexed_Constraint.model
model.indexed_Constraint = Constraint(model.ndx,
rule=build_indexed_Constraint.rule)
model.indexed_Constraint._component = None
return model.indexed_Constraint
def build_BlockData_with_objects():
"""Build an empty _BlockData"""
obj = _BlockData(build_BlockData_with_objects.owner)
obj.x = Var()
obj.x._domain = None
obj.c = Constraint()
obj.o = Objective()
obj._component = None
return obj
def build_Block_with_objects():
"""Build an empty Block"""
obj = Block(concrete=True)
obj.construct()
obj.x = Var()
obj.x._domain = None
obj.c = Constraint()
obj.o = Objective()
return obj
def get_hessian_of_constraint(constraint, wrt1=None, wrt2=None, nlp=None):
constraints = [constraint]
if wrt1 is None and wrt2 is None:
variables = list(
identify_variables(constraint.expr, include_fixed=False))
wrt1 = variables
wrt2 = variables
elif wrt1 is not None and wrt2 is not None:
variables = wrt1 + wrt2
elif wrt1 is not None: # but wrt2 is None
wrt2 = wrt1
variables = wrt1
else:
# wrt2 is not None and wrt1 is None
wrt1 = wrt2
variables = wrt1
if nlp is None:
block = create_subsystem_block(constraints, variables=variables)
# Could fix input_vars so I don't evaluate the Hessian with respect
# to variables I don't care about...
# HUGE HACK: Variables not included in a constraint are not written
# to the nl file, so we cannot take the derivative with respect to
# them, even though we know this derivative is zero. To work around,
# we make sure all variables appear on the block in the form of a
# dummy constraint. Then we can take derivatives of any constraint
# with respect to them. Conveniently, the extract_submatrix_
# call deals with extracting the variables and constraint we care
# about, in the proper order.
block._dummy_var = Var()
block._dummy_con = Constraint(expr=sum(variables) == block._dummy_var)
block._obj = Objective(expr=0.0)
nlp = PyomoNLP(block)
saved_duals = nlp.get_duals()
saved_obj_factor = nlp.get_obj_factor()
temp_duals = np.zeros(len(saved_duals))
# NOTE: This makes some assumption about how the Lagrangian is constructed.
# TODO: Define the convention we assume and convert if necessary.
idx = nlp.get_constraint_indices(constraints)[0]
temp_duals[idx] = 1.0
nlp.set_duals(temp_duals)
nlp.set_obj_factor(0.0)
# NOTE: The returned matrix preserves explicit zeros. I.e. it contains
# coordinates for every entry that could possibly be nonzero.
submatrix = nlp.extract_submatrix_hessian_lag(wrt1, wrt2)
nlp.set_obj_factor(saved_obj_factor)
nlp.set_duals(saved_duals)
return submatrix
def _add_equality_constraint(arc, name, index_set):
# This function will add the equality constraint if it doesn't exist.
eblock = arc.expanded_block
cname = name + "_equality"
if eblock.component(cname) is not None:
# already exists, skip
return
port1, port2 = arc.ports
def rule(m, *args):
if len(args):
return port1.vars[name][args] == port2.vars[name][args]
else:
return port1.vars[name] == port2.vars[name]
con = Constraint(index_set, rule=rule)
eblock.add_component(cname, con)
def build_Constraint():
"""Build a Constraint and delete any references to external
objects so its size can be computed."""
expr = sum(x * c
for x, c in zip(build_Constraint.xlist, build_Constraint.clist))
obj = Constraint(expr=(0, expr, 1))
obj._parent = build_Constraint.dummy_parent
obj.construct()
obj._parent = None
return obj
def apply(self, **kwds):
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug("Calling ConnectorExpander")
instance = kwds['instance']
blockList = list(instance.block_data_objects(active=True))
noConnectors = True
for b in blockList:
if b.component_map(Connector):
noConnectors = False
break
if noConnectors:
return
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" Connectors found!")
#
# At this point, there are connectors in the model, so we must
# look for constraints that involve connectors and expand them.
#
#options = kwds['options']
#model = kwds['model']
# In general, blocks should be relatively self-contained, so we
# should build the connectors from the "bottom up":
blockList.reverse()
# Expand each constraint involving a connector
for block in blockList:
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" block: " + block.cname())
CCC = {}
for name, constraint in itertools.chain\
( iteritems(block.component_map(Constraint)),
iteritems(block.component_map(ConstraintList)) ):
cList = []
CCC[name+'.expanded'] = cList
for idx, c in iteritems(constraint._data):
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (looking at constraint %s[%s])", name, idx)
connectors = []
self._gather_connectors(c.body, connectors)
if len(connectors) == 0:
continue
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (found connectors in constraint)")
# Validate that all connectors match
errors, ref, skip = self._validate_connectors(connectors)
if errors:
logger.error(
( "Connector mismatch: errors detected when "
"constructing constraint %s\n " %
(name + (idx and '[%s]' % idx or '')) ) +
'\n '.join(reversed(errors)) )
raise ValueError(
"Connector mismatch in constraint %s" % \
name + (idx and '[%s]' % idx or ''))
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (connectors valid)")
# Fill in any empty connectors
for conn in connectors:
if conn.vars:
continue
for var in ref.vars:
if var in skip:
continue
v = Var()
block.add_component(conn.cname() + '.auto.' + var, v)
conn.vars[var] = v
v.construct()
# OK - expand this constraint
self._expand_constraint(block, name, idx, c, ref, skip, cList)
# Now deactivate the original constraint
c.deactivate()
for name, exprs in iteritems(CCC):
cList = ConstraintList()
block.add_component( name, cList )
cList.construct()
for expr in exprs:
cList.add(expr)
# Now, go back and implement VarList aggregators
for block in blockList:
for conn in itervalues(block.component_map(Connector)):
for var, aggregator in iteritems(conn.aggregators):
c = Constraint(expr=aggregator(block, var))
block.add_component(
conn.cname() + '.' + var.cname() + '.aggregate', c)
c.construct()
def make_separation_problem(model_data, config):
"""
Swap out uncertain param Param objects for Vars
Add uncertainty set constraints and separation objectives
"""
separation_model = model_data.original.clone()
separation_model.del_component("coefficient_matching_constraints")
separation_model.del_component("coefficient_matching_constraints_index")
uncertain_params = separation_model.util.uncertain_params
separation_model.util.uncertain_param_vars = param_vars = Var(
range(len(uncertain_params)))
map_new_constraint_list_to_original_con = ComponentMap()
if config.objective_focus is ObjectiveType.worst_case:
separation_model.util.zeta = Param(initialize=0, mutable=True)
constr = Constraint(expr=separation_model.first_stage_objective +
separation_model.second_stage_objective -
separation_model.util.zeta <= 0)
separation_model.add_component("epigraph_constr", constr)
substitution_map = {}
#Separation problem initialized to nominal uncertain parameter values
for idx, var in enumerate(list(param_vars.values())):
param = uncertain_params[idx]
var.set_value(param.value, skip_validation=True)
substitution_map[id(param)] = var
separation_model.util.new_constraints = constraints = ConstraintList()
uncertain_param_set = ComponentSet(uncertain_params)
for c in separation_model.component_data_objects(Constraint):
if any(v in uncertain_param_set
for v in identify_mutable_parameters(c.expr)):
if c.equality:
constraints.add(
replace_expressions(expr=c.lower,
substitution_map=substitution_map) ==
replace_expressions(expr=c.body,
substitution_map=substitution_map))
elif c.lower is not None:
constraints.add(
replace_expressions(expr=c.lower,
substitution_map=substitution_map) <=
replace_expressions(expr=c.body,
substitution_map=substitution_map))
elif c.upper is not None:
constraints.add(
replace_expressions(expr=c.upper,
substitution_map=substitution_map) >=
replace_expressions(expr=c.body,
substitution_map=substitution_map))
else:
raise ValueError(
"Unable to parse constraint for building the separation problem."
)
c.deactivate()
map_new_constraint_list_to_original_con[constraints[
constraints.index_set().last()]] = c
separation_model.util.map_new_constraint_list_to_original_con = map_new_constraint_list_to_original_con
# === Add objectives first so that the uncertainty set
# Constraints do not get picked up into the set
# of performance constraints which become objectives
make_separation_objective_functions(separation_model, config)
add_uncertainty_set_constraints(separation_model, config)
# === Deactivate h(x,q) == 0 constraints
for c in separation_model.util.h_x_q_constraints:
c.deactivate()
return separation_model
def _Split(port, name, index_set, include_splitfrac=False,
write_var_sum=True):
port_parent = port.parent_block()
var = port.vars[name]
out_vars = []
no_splitfrac = False
dests = port.dests(active=True)
if not len(dests):
return out_vars
if len(dests) == 1:
# No need for splitting on one outlet.
# Make sure they do not try to fix splitfrac not at 1.
splitfracspec = port.get_split_fraction(dests[0])
if splitfracspec is not None:
if splitfracspec[0] != 1 and splitfracspec[1] == True:
raise ValueError(
"Cannot fix splitfrac not at 1 for port '%s' with a "
"single dest '%s'" % (port.name, dests[0].name))
no_splitfrac = True
if len(dests[0].destination.sources(active=True)) == 1:
# This is a 1-to-1 connection, no need for evar, just equality.
arc = dests[0]
Port._add_equality_constraint(arc, name, index_set)
return out_vars
for arc in dests:
eblock = arc.expanded_block
# Make and record new variables for every arc with this member.
evar = Port._create_evar(port.vars[name], name, eblock, index_set)
out_vars.append(evar)
if no_splitfrac:
continue
# Create and potentially initialize split fraction variables.
# This function will be called for every Extensive member of this
# port, but we only need one splitfrac variable per arc, so check
# if it already exists before making a new one. However, we do not
# need a splitfrac if there is only one Extensive data object,
# so first check whether or not we need it.
if eblock.component("splitfrac") is None:
if not include_splitfrac:
num_data_objs = 0
for k, v in iteritems(port.vars):
if port.is_extensive(k):
if v.is_indexed():
num_data_objs += len(v)
else:
num_data_objs += 1
if num_data_objs > 1:
break
if num_data_objs <= 1:
# Do not make splitfrac, do not make split constraints.
# Make sure they didn't specify splitfracs.
# This inner loop will only run once.
for arc in dests:
if port.get_split_fraction(arc) is not None:
raise ValueError(
"Cannot specify splitfracs for port '%s' "
"(found arc '%s') because this port only "
"has one variable. To have control over "
"splitfracs, please pass the "
" include_splitfrac=True argument." %
(port.name, arc.name))
no_splitfrac = True
continue
eblock.splitfrac = Var()
splitfracspec = port.get_split_fraction(arc)
if splitfracspec is not None:
eblock.splitfrac = splitfracspec[0]
if splitfracspec[1]:
eblock.splitfrac.fix()
# Create constraint for this member using splitfrac.
cname = "%s_split" % name
def rule(m, *args):
if len(args):
return evar[args] == eblock.splitfrac * var[args]
else:
return evar == eblock.splitfrac * var
con = Constraint(index_set, rule=rule)
eblock.add_component(cname, con)
if write_var_sum:
# Create var total sum constraint: var == sum of evars
# Need to alphanum port name in case it is indexed.
cname = unique_component_name(port_parent, "%s_%s_outsum" %
(alphanum_label_from_name(port.local_name), name))
def rule(m, *args):
if len(args):
return sum(evar[args] for evar in out_vars) == var[args]
else:
return sum(evar for evar in out_vars) == var
con = Constraint(index_set, rule=rule)
port_parent.add_component(cname, con)
else:
# OR create constraint on splitfrac vars: sum == 1
if no_splitfrac:
raise ValueError(
"Cannot choose to write split fraction sum constraint for "
"ports with a single destination or a single Extensive "
"variable.\nSplit fractions are skipped in this case to "
"simplify the model.\nPlease use write_var_sum=True on "
"this port (the default).")
cname = unique_component_name(port_parent,
"%s_frac_sum" % alphanum_label_from_name(port.local_name))
con = Constraint(expr=
sum(a.expanded_block.splitfrac for a in dests) == 1)
port_parent.add_component(cname, con)
return out_vars
def build_model(skel_dict, project_dir) -> ConcreteModel:
"""
Builds a pyomo model from a given saved skeleton dictionary
"""
links = skel_dict["links"]
positions = skel_dict["positions"]
dofs = skel_dict["dofs"]
markers = skel_dict["markers"]
rot_dict = {}
pose_dict = {}
L = len(positions)
phi = [sp.symbols(f"\\phi_{{{l}}}") for l in range(L)]
theta = [sp.symbols(f"\\theta_{{{l}}}") for l in range(L)]
psi = [sp.symbols(f"\\psi_{{{l}}}") for l in range(L)]
i = 0
for part in dofs:
rot_dict[part] = sp.eye(3)
if dofs[part][1]:
rot_dict[part] = rot_y(theta[i]) @ rot_dict[part]
if dofs[part][0]:
rot_dict[part] = rot_x(phi[i]) @ rot_dict[part]
if dofs[part][2]:
rot_dict[part] = rot_z(psi[i]) @ rot_dict[part]
rot_dict[part + "_i"] = rot_dict[part].T
i += 1
x, y, z = sp.symbols("x y z")
dx, dy, dz = sp.symbols("\\dot{x} \\dot{y} \\dot{z}")
ddx, ddy, ddz = sp.symbols("\\ddot{x} \\ddot{y} \\ddot{z}")
for link in links:
if len(link) == 1:
pose_dict[link[0]] = sp.Matrix([x, y, z])
else:
if link[0] not in pose_dict:
pose_dict[link[0]] = sp.Matrix([x, y, z])
translation_vec = sp.Matrix([
positions[link[1]][0] - positions[link[0]][0],
positions[link[1]][1] - positions[link[0]][1],
positions[link[1]][2] - positions[link[0]][2]
])
rot_dict[link[1]] = rot_dict[link[1]] @ rot_dict[link[0]]
rot_dict[link[1] + "_i"] = rot_dict[link[1] + "_i"].T
pose_dict[link[1]] = pose_dict[
link[0]] + rot_dict[link[0] + "_i"] @ translation_vec
t_poses = []
for pose in pose_dict:
t_poses.append(pose_dict[pose].T)
t_poses_mat = sp.Matrix(t_poses)
func_map = {"sin": sin, "cos": cos, "ImmutableDenseMatrix": np.array}
sym_list = [x, y, z, *phi, *theta, *psi]
pose_to_3d = sp.lambdify(sym_list, t_poses_mat, modules=[func_map])
pos_funcs = []
for i in range(t_poses_mat.shape[0]):
lamb = sp.lambdify(sym_list, t_poses_mat[i, :], modules=[func_map])
pos_funcs.append(lamb)
scene_path = os.path.join(project_dir, "4_cam_scene_static_sba.json")
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1, 4))
markers_dict = dict(enumerate(markers))
print(f"\n\n\nLoading data")
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
print("2d df points:")
print(points_2d_df)
# break here
def get_meas_from_df(n, c, l, d):
n_mask = points_2d_df["frame"] == n - 1
l_mask = points_2d_df["marker"] == markers[l - 1]
c_mask = points_2d_df["camera"] == c - 1
d_idx = {1: "x", 2: "y"}
val = points_2d_df[n_mask & l_mask & c_mask]
return val[d_idx[d]].values[0]
def get_likelihood_from_df(n, c, l):
n_mask = points_2d_df["frame"] == n - 1
if (markers[l - 1] == "neck"):
return 0
else:
l_mask = points_2d_df["marker"] == markers[l - 1]
c_mask = points_2d_df["camera"] == c - 1
val = points_2d_df[n_mask & l_mask & c_mask]
return val["likelihood"].values[0]
h = 1 / 120 #timestep
start_frame = 1600 # 50
N = 50
P = 3 + len(phi) + len(theta) + len(psi)
L = len(pos_funcs)
C = len(K_arr)
D2 = 2
D3 = 3
proj_funcs = [pt3d_to_x2d, pt3d_to_y2d]
R = 5 # measurement standard deviation
Q = np.array([ # model parameters variance
4.0, 7.0, 5.0, 13.0, 32.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 9.0, 18.0, 43.0, 53.0, 90.0, 118.0, 247.0, 186.0, 194.0,
164.0, 295.0, 243.0, 334.0, 149.0, 26.0, 12.0, 0.0, 34.0, 43.0, 51.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
])**2
triangulate_func = calib.triangulate_points_fisheye
points_2d_filtered_df = points_2d_df[points_2d_df['likelihood'] > 0.5]
points_3d_df = calib.get_pairwise_3d_points_from_df(
points_2d_filtered_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
print("3d points")
print(points_3d_df)
# estimate initial points
nose_pts = points_3d_df[points_3d_df["marker"] == "forehead"][[
"x", "y", "z", "frame"
]].values
print(nose_pts[:, 3])
print(nose_pts[:, 0])
xs = stats.linregress(nose_pts[:, 3], nose_pts[:, 0])
ys = stats.linregress(nose_pts[:, 3], nose_pts[:, 1])
zs = stats.linregress(nose_pts[:, 3], nose_pts[:, 2])
frame_est = np.arange(N)
x_est = np.array([
frame_est[i] * (xs.slope) + (xs.intercept)
for i in range(len(frame_est))
])
y_est = np.array([
frame_est[i] * (ys.slope) + (ys.intercept)
for i in range(len(frame_est))
])
z_est = np.array([
frame_est[i] * (zs.slope) + (zs.intercept)
for i in range(len(frame_est))
])
print(x_est)
#print("x est shape:")
#print(x_est.shape)
psi_est = np.arctan2(ys.slope, xs.slope)
print("Started Optimisation")
m = ConcreteModel(name="Skeleton")
# ===== SETS =====
m.N = RangeSet(N) #number of timesteps in trajectory
m.P = RangeSet(
P
) #number of pose parameters (x, y, z, phi_1..n, theta_1..n, psi_1..n)
m.L = RangeSet(L) #number of labels
m.C = RangeSet(C) #number of cameras
m.D2 = RangeSet(D2) #dimensionality of measurements
m.D3 = RangeSet(D3) #dimensionality of measurements
def init_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n + start_frame, c, l)
if likelihood > 0.5:
return 1 / R
else:
return 0
m.meas_err_weight = Param(
m.N, m.C, m.L, initialize=init_meas_weights, mutable=True, within=Any
) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_model_weights(m, p):
#if Q[p-1] != 0.0:
#return 1/Q[p-1]
#else:
return 0.01
m.model_err_weight = Param(m.P, initialize=init_model_weights, within=Any)
m.h = h
def init_measurements_df(m, n, c, l, d2):
if (markers[l - 1] == "neck"):
return Constraint.Skip
else:
return get_meas_from_df(n + start_frame, c, l, d2)
m.meas = Param(m.N,
m.C,
m.L,
m.D2,
initialize=init_measurements_df,
within=Any)
# ===== VARIABLES =====
m.x = Var(m.N, m.P) #position
m.dx = Var(m.N, m.P) #velocity
m.ddx = Var(m.N, m.P) #acceleration
m.poses = Var(m.N, m.L, m.D3)
m.slack_model = Var(m.N, m.P)
m.slack_meas = Var(m.N, m.C, m.L, m.D2, initialize=0.0)
# ===== VARIABLES INITIALIZATION =====
init_x = np.zeros((N, P))
init_x[:, 0] = x_est #x
init_x[:, 1] = y_est #y
init_x[:, 2] = z_est #z
#init_x[:,(3+len(pos_funcs)*2)] = psi_est #yaw - psi
init_dx = np.zeros((N, P))
init_ddx = np.zeros((N, P))
for n in range(1, N + 1):
for p in range(1, P + 1):
if n < len(init_x): #init using known values
m.x[n, p].value = init_x[n - 1, p - 1]
m.dx[n, p].value = init_dx[n - 1, p - 1]
m.ddx[n, p].value = init_ddx[n - 1, p - 1]
else: #init using last known value
m.x[n, p].value = init_x[-1, p - 1]
m.dx[n, p].value = init_dx[-1, p - 1]
m.ddx[n, p].value = init_ddx[-1, p - 1]
#init pose
var_list = [m.x[n, p].value for p in range(1, P + 1)]
for l in range(1, L + 1):
[pos] = pos_funcs[l - 1](*var_list)
for d3 in range(1, D3 + 1):
m.poses[n, l, d3].value = pos[d3 - 1]
# ===== CONSTRAINTS =====
# 3D POSE
def pose_constraint(m, n, l, d3):
#get 3d points
var_list = [m.x[n, p] for p in range(1, P + 1)]
[pos] = pos_funcs[l - 1](*var_list)
return pos[d3 - 1] == m.poses[n, l, d3]
m.pose_constraint = Constraint(m.N, m.L, m.D3, rule=pose_constraint)
def backwards_euler_pos(m, n, p): # position
if n > 1:
# return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n-1,p] + m.h**2 * m.ddx[n-1,p]/2
return m.x[n, p] == m.x[n - 1, p] + m.h * m.dx[n, p]
else:
return Constraint.Skip
m.integrate_p = Constraint(m.N, m.P, rule=backwards_euler_pos)
def backwards_euler_vel(m, n, p): # velocity
if n > 1:
return m.dx[n, p] == m.dx[n - 1, p] + m.h * m.ddx[n, p]
else:
return Constraint.Skip
m.integrate_v = Constraint(m.N, m.P, rule=backwards_euler_vel)
m.angs = ConstraintList()
for n in range(1, N):
for i in range(3, 3 * len(positions)):
m.angs.add(expr=(abs(m.x[n, i]) <= np.pi / 2))
# MODEL
def constant_acc(m, n, p):
if n > 1:
return m.ddx[n, p] == m.ddx[n - 1, p] + m.slack_model[n, p]
else:
return Constraint.Skip
m.constant_acc = Constraint(m.N, m.P, rule=constant_acc)
# MEASUREMENT
def measurement_constraints(m, n, c, l, d2):
#project
K, D, R, t = K_arr[c - 1], D_arr[c - 1], R_arr[c - 1], t_arr[c - 1]
x, y, z = m.poses[n, l, 1], m.poses[n, l, 2], m.poses[n, l, 3]
if (markers[l - 1] == "neck"):
return Constraint.Skip
else:
return proj_funcs[d2 - 1](
x, y, z, K, D, R,
t) - m.meas[n, c, l, d2] - m.slack_meas[n, c, l, d2] == 0
m.measurement = Constraint(m.N,
m.C,
m.L,
m.D2,
rule=measurement_constraints)
def obj(m):
slack_model_err = 0.0
slack_meas_err = 0.0
for n in range(1, N + 1):
#Model Error
for p in range(1, P + 1):
slack_model_err += m.model_err_weight[p] * m.slack_model[n,
p]**2
#Measurement Error
for l in range(1, L + 1):
for c in range(1, C + 1):
for d2 in range(1, D2 + 1):
slack_meas_err += redescending_loss(
m.meas_err_weight[n, c, l] *
m.slack_meas[n, c, l, d2], 3, 10, 20)
return slack_meas_err + slack_model_err
m.obj = Objective(rule=obj)
return (m, pose_to_3d)
def declare_constraints(model):
model.z_th_sum = Constraint(model.HP, model.ST, rule=z_th_sum_rule)
model.th_lower = Constraint(model.HP, model.ST, rule=th_lower_rule)
model.th_upper = Constraint(model.HP, model.ST, rule=th_upper_rule)
model.var_delta_fh_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=var_delta_fh_sum_rule)
model.var_delta_fh_upper = Constraint(var_delta_fh_index,
rule=var_delta_fh_upper_rule)
model.z_thx_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=z_thx_sum_rule)
model.thx_lower = Constraint(model.HP,
model.CP,
model.ST,
rule=thx_lower_rule)
model.thx_upper = Constraint(model.HP,
model.CP,
model.ST,
rule=thx_upper_rule)
model.var_delta_fhx_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=var_delta_fhx_sum_rule)
model.var_delta_fhx_upper = Constraint(var_delta_fhx_index,
rule=var_delta_fhx_upper_rule)
model.z_tc_sum = Constraint(model.CP, model.ST, rule=z_tc_sum_rule)
model.tc_lower = Constraint(model.CP, model.ST, rule=tc_lower_rule)
model.tc_upper = Constraint(model.CP, model.ST, rule=tc_upper_rule)
model.var_delta_fc_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=var_delta_fc_sum_rule)
model.var_delta_fc_upper = Constraint(var_delta_fc_index,
rule=var_delta_fc_upper_rule)
model.z_tcx_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=z_tcx_sum_rule)
model.tcx_lower = Constraint(model.HP,
model.CP,
model.ST,
rule=tcx_lower_rule)
model.tcx_upper = Constraint(model.HP,
model.CP,
model.ST,
rule=tcx_upper_rule)
model.var_delta_fcx_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=var_delta_fcx_sum_rule)
model.var_delta_fcx_upper = Constraint(var_delta_fcx_index,
rule=var_delta_fcx_upper_rule)
# Overall heat balance
model.overall_heat_balance_hot = Constraint(
model.HP, rule=overall_heat_balance_hot_rule)
model.overall_heat_balance_cold = Constraint(
model.CP, rule=overall_heat_balance_cold_rule)
model.energy_balance_hot = Constraint(
model.HP,
model.ST,
rule=energy_balance_hot_rule,
doc="Energy exchanged by hot stream i in stage k")
model.energy_balance_cold = Constraint(
model.CP,
model.ST,
rule=energy_balance_cold_rule,
doc="Energy exchanged by cold stream j in stage k")
model.energy_balance_cu = Constraint(
model.HP,
rule=energy_balance_cu_rule,
doc="Energy exchanged by hot stream i with the cold utility")
model.energy_balance_hu = Constraint(
model.CP,
rule=energy_balance_hu_rule,
doc="Energy exchanged by cold stream j with the hot utility")
# Inlet temperatures
model.hot_inlet = Constraint(model.HP, rule=hot_inlet_rule)
model.cold_inlet = Constraint(model.CP, rule=cold_inlet_rule)
# Mass balance
model.mass_balance_hot = Constraint(model.HP,
model.ST,
rule=mass_balance_hot_rule)
model.mass_balance_cold = Constraint(model.CP,
model.ST,
rule=mass_balance_cold_rule)
# Monotonicity
model.decreasing_hot = Constraint(model.HP,
model.ST,
rule=decreasing_hot_rule)
model.decreasing_cold = Constraint(model.CP,
model.ST,
rule=decreasing_cold_rule)
model.hot_upper_bound = Constraint(model.HP, rule=hot_upper_bound_rule)
model.cold_lower_bound = Constraint(model.CP, rule=cold_lower_bound_rule)
# Heat load big M
model.q_big_m = Constraint(model.HP, model.CP, model.ST, rule=q_big_m_rule)
model.q_cu_big_m = Constraint(model.HP, rule=q_cu_big_m_rule)
model.q_hu_big_m = Constraint(model.CP, rule=q_hu_big_m_rule)
# Temperature approach big M
model.temp_app_in = Constraint(model.HP,
model.CP,
model.ST,
rule=temp_app_in_rule)
model.temp_app_out = Constraint(model.HP,
model.CP,
model.ST,
rule=temp_app_out_rule)
model.temp_app_cu = Constraint(model.HP, rule=temp_app_cu_rule)
model.temp_app_hu = Constraint(model.CP, rule=temp_app_hu_rule)
# Bilinear McCormick bounds
model.mccor_convex_h_in_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_h_in_1_rule)
model.mccor_convex_h_in_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_h_in_2_rule)
model.mccor_concave_h_in_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_h_in_1_rule)
model.mccor_concave_h_in_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_h_in_2_rule)
model.mccor_convex_h_out_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_h_out_1_rule)
model.mccor_convex_h_out_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_h_out_2_rule)
model.mccor_concave_h_out_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_h_out_1_rule)
model.mccor_concave_h_out_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_h_out_2_rule)
model.mccor_convex_c_in_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_c_in_1_rule)
model.mccor_convex_c_in_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_c_in_2_rule)
model.mccor_concave_c_in_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_c_in_1_rule)
model.mccor_concave_c_in_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_c_in_2_rule)
model.mccor_convex_c_out_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_c_out_1_rule)
model.mccor_convex_c_out_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_convex_c_out_2_rule)
model.mccor_concave_c_out_1 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_c_out_1_rule)
model.mccor_concave_c_out_2 = Constraint(model.HP,
model.CP,
model.ST,
rule=mccor_concave_c_out_2_rule)
# q-betas
model.z_q_beta_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=z_q_beta_sum_rule)
model.q_low = Constraint(model.HP, model.CP, model.ST, rule=q_low_rule)
model.q_high = Constraint(model.HP, model.CP, model.ST, rule=q_high_rule)
model.q_pow_beta = Constraint(model.HP,
model.CP,
model.ST,
rule=q_pow_beta_rule)
model.z_q_cu_beta_sum = Constraint(model.HP, rule=z_q_cu_beta_sum_rule)
model.q_cu_low = Constraint(model.HP, rule=q_cu_low_rule)
model.q_cu_high = Constraint(model.HP, rule=q_cu_high_rule)
model.q_cu_pow_beta = Constraint(model.HP, rule=q_cu_pow_beta_rule)
model.z_q_hu_beta_sum = Constraint(model.CP, rule=z_q_hu_beta_sum_rule)
model.q_hu_low = Constraint(model.CP, rule=q_hu_low_rule)
model.q_hu_high = Constraint(model.CP, rule=q_hu_high_rule)
model.q_hu_pow_beta = Constraint(model.CP, rule=q_hu_pow_beta_rule)
#Per heat exchanger energy balance
model.q_energy_balance_hot = Constraint(model.HP,
model.CP,
model.ST,
rule=q_energy_balance_hot_rule)
model.q_energy_balance_cold = Constraint(model.HP,
model.CP,
model.ST,
rule=q_energy_balance_cold_rule)
#Per mixer energy balance
model.mixer_energy_balance_hot = Constraint(
model.HP, model.ST, rule=mixer_energy_balance_hot_rule)
model.mixer_energy_balance_cold = Constraint(
model.CP, model.ST, rule=mixer_energy_balance_cold_rule)
# RecLMTD to the beta-th power
model.grad_reclmtd_beta = Constraint(reclmtd_beta_index,
rule=grad_reclmtd_beta_rule)
model.grad_reclmtd_cu_beta = Constraint(reclmtd_cu_beta_index,
rule=grad_reclmtd_cu_beta_rule)
model.grad_reclmtd_hu_beta = Constraint(reclmtd_hu_beta_index,
rule=grad_reclmtd_hu_beta_rule)
# Area to the beta-th powers
model.z_area_beta_q_sum = Constraint(model.HP,
model.CP,
model.ST,
rule=z_area_beta_q_sum_rule)
model.area_beta_q_lower = Constraint(model.HP,
model.CP,
model.ST,
rule=area_beta_q_lower_rule)
model.area_beta_q_upper = Constraint(model.HP,
model.CP,
model.ST,
rule=area_beta_q_upper_rule)
model.var_delta_reclmtd_beta_sum = Constraint(
model.HP, model.CP, model.ST, rule=var_delta_reclmtd_beta_sum_rule)
model.var_delta_reclmtd_beta_upper = Constraint(
z_area_beta_q_index, rule=var_delta_reclmtd_beta_upper_rule)
model.area_beta_mccor_convex_1 = Constraint(
model.HP, model.CP, model.ST, rule=area_beta_mccor_convex_1_rule)
model.area_beta_mccor_convex_2 = Constraint(
model.HP, model.CP, model.ST, rule=area_beta_mccor_convex_2_rule)
model.area_beta_mccor_concave_1 = Constraint(
model.HP, model.CP, model.ST, rule=area_beta_mccor_concave_1_rule)
model.area_beta_mccor_concave_2 = Constraint(
model.HP, model.CP, model.ST, rule=area_beta_mccor_concave_2_rule)
model.z_area_beta_q_cu_sum = Constraint(model.HP,
rule=z_area_beta_q_cu_sum_rule)
model.area_beta_q_cu_lower = Constraint(model.HP,
rule=area_beta_q_cu_lower_rule)
model.area_beta_q_cu_upper = Constraint(model.HP,
rule=area_beta_q_cu_upper_rule)
model.var_delta_reclmtd_cu_beta_sum = Constraint(
model.HP, rule=var_delta_reclmtd_cu_beta_sum_rule)
model.var_delta_reclmtd_cu_beta_upper = Constraint(
z_area_beta_q_cu_index, rule=var_delta_reclmtd_cu_beta_upper_rule)
model.area_cu_beta_mccor_convex_1 = Constraint(
model.HP, rule=area_cu_beta_mccor_convex_1_rule)
model.area_cu_beta_mccor_convex_2 = Constraint(
model.HP, rule=area_cu_beta_mccor_convex_2_rule)
model.area_cu_beta_mccor_concave_1 = Constraint(
model.HP, rule=area_cu_beta_mccor_concave_1_rule)
model.area_cu_beta_mccor_concave_2 = Constraint(
model.HP, rule=area_cu_beta_mccor_concave_2_rule)
model.z_area_beta_q_hu_sum = Constraint(model.CP,
rule=z_area_beta_q_hu_sum_rule)
model.area_beta_q_hu_lower = Constraint(model.CP,
rule=area_beta_q_hu_lower_rule)
model.area_beta_q_hu_upper = Constraint(model.CP,
rule=area_beta_q_hu_upper_rule)
model.var_delta_reclmtd_hu_beta_sum = Constraint(
model.CP, rule=var_delta_reclmtd_hu_beta_sum_rule)
model.var_delta_reclmtd_hu_beta_upper = Constraint(
z_area_beta_q_hu_index, rule=var_delta_reclmtd_hu_beta_upper_rule)
model.area_hu_beta_mccor_convex_1 = Constraint(
model.CP, rule=area_hu_beta_mccor_convex_1_rule)
model.area_hu_beta_mccor_convex_2 = Constraint(
model.CP, rule=area_hu_beta_mccor_convex_2_rule)
model.area_hu_beta_mccor_concave_1 = Constraint(
model.CP, rule=area_hu_beta_mccor_concave_1_rule)
model.area_hu_beta_mccor_concave_2 = Constraint(
model.CP, rule=area_hu_beta_mccor_concave_2_rule)
def calculate_variable_from_constraint(variable,
constraint,
eps=1e-8,
iterlim=1000,
linesearch=True,
alpha_min=1e-8):
"""Calculate the variable value given a specified equality constraint
This function calculates the value of the specified variable
necessary to make the provided equality constraint feasible
(assuming any other variables values are fixed). The method first
attempts to solve for the variable value assuming it appears
linearly in the constraint. If that doesn't converge the constraint
residual, it falls back on Newton's method using exact (symbolic)
derivatives.
Notes
-----
This is an unconstrained solver and is NOT guaranteed to respect the
variable bounds or domain. The solver may leave the variable value
in an infeasible state (outside the declared bounds or domain bounds).
Parameters:
-----------
variable: :py:class:`_VarData`
The variable to solve for
constraint: :py:class:`_ConstraintData` or relational expression or `tuple`
The equality constraint to use to solve for the variable value.
May be a `ConstraintData` object or any valid argument for
``Constraint(expr=<>)`` (i.e., a relational expression or 2- or
3-tuple)
eps: `float`
The tolerance to use to determine equality [default=1e-8].
iterlim: `int`
The maximum number of iterations if this method has to fall back
on using Newton's method. Raises RuntimeError on iteration
limit [default=1000]
linesearch: `bool`
Decides whether or not to use the linesearch (recommended).
[default=True]
alpha_min: `float`
The minimum fractional step to use in the linesearch [default=1e-8].
Returns:
--------
None
"""
# Leverage all the Constraint logic to process the incoming tuple/expression
if not isinstance(constraint, _ConstraintData):
constraint = Constraint(expr=constraint,
name=type(constraint).__name__)
constraint.construct()
body = constraint.body
lower = constraint.lb
upper = constraint.ub
if lower != upper:
raise ValueError("Constraint must be an equality constraint")
if variable.value is None:
# Note that we use "skip_validation=True" here as well, as the
# variable domain may not admit the calculated initial guesses,
# and we want to bypass that check.
if variable.lb is None:
if variable.ub is None:
# no variable values, and no lower or upper bound - set
# initial value to 0.0
variable.set_value(0, skip_validation=True)
else:
# no variable value or lower bound - set to 0 or upper
# bound whichever is lower
variable.set_value(min(0, variable.ub), skip_validation=True)
elif variable.ub is None:
# no variable value or upper bound - set to 0 or lower
# bound, whichever is higher
variable.set_value(max(0, variable.lb), skip_validation=True)
else:
# we have upper and lower bounds
if variable.lb <= 0 and variable.ub >= 0:
# set the initial value to 0 if bounds bracket 0
variable.set_value(0, skip_validation=True)
else:
# set the initial value to the midpoint of the bounds
variable.set_value((variable.lb + variable.ub) / 2.0,
skip_validation=True)
# store the initial value to use later if necessary
orig_initial_value = variable.value
# solve the common case where variable is linear with coefficient of 1.0
x1 = value(variable)
# Note: both the direct (linear) calculation and Newton's method
# below rely on a numerically valid initial starting point.
# While we have strategies for dealing with hitting numerically
# invalid (e.g., sqrt(-1)) conditions below, if the initial point is
# not valid, we will allow that exception to propagate up
try:
residual_1 = value(body)
except:
logger.error(
"Encountered an error evaluating the expression at the "
"initial guess.\n\tPlease provide a different initial guess.")
raise
variable.set_value(x1 - (residual_1 - upper), skip_validation=True)
residual_2 = value(body, exception=False)
# If we encounter an error while evaluating the expression at the
# linear intercept calculated assuming the derivative was 1. This
# is most commonly due to nonlinear expressions (like sqrt())
# becoming invalid/complex. We will skip the rest of the
# "shortcuts" that assume the expression is linear and move directly
# to using Newton's method.
if residual_2 is not None and type(residual_2) is not complex:
# if the variable appears linearly with a coefficient of 1, then we
# are done
if abs(residual_2 - upper) < eps:
# Re-set the variable value to trigger any warnings WRT the
# final variable state
variable.set_value(variable.value)
return
# Assume the variable appears linearly and calculate the coefficient
x2 = value(variable)
slope = float(residual_1 - residual_2) / (x1 - x2)
intercept = (residual_1 - upper) - slope * x1
if slope:
variable.set_value(-intercept / slope, skip_validation=True)
body_val = value(body, exception=False)
if body_val is not None and abs(body_val - upper) < eps:
# Re-set the variable value to trigger any warnings WRT
# the final variable state
variable.set_value(variable.value)
return
# Variable appears nonlinearly; solve using Newton's method
#
# restore initial value
variable.set_value(orig_initial_value, skip_validation=True)
expr = body - upper
expr_deriv = differentiate(expr,
wrt=variable,
mode=differentiate.Modes.sympy)
if type(expr_deriv) in native_numeric_types and expr_deriv == 0:
raise ValueError("Variable derivative == 0, cannot solve for variable")
if abs(value(expr_deriv)) < 1e-12:
raise RuntimeError(
'Initial value for variable results in a derivative value that is '
'very close to zero.\n\tPlease provide a different initial guess.')
iter_left = iterlim
fk = residual_1 - upper
while abs(fk) > eps and iter_left:
iter_left -= 1
if not iter_left:
raise RuntimeError(
"Iteration limit (%s) reached; remaining residual = %s" %
(iterlim, value(expr)))
# compute step
xk = value(variable)
try:
fk = value(expr)
if type(fk) is complex:
raise ValueError(
"Complex numbers are not allowed in Newton's method.")
except:
# We hit numerical problems with the last step (possible if
# the line search is turned off)
logger.error(
"Newton's method encountered an error evaluating the "
"expression.\n\tPlease provide a different initial guess "
"or enable the linesearch if you have not.")
raise
fpk = value(expr_deriv)
if abs(fpk) < 1e-12:
raise RuntimeError(
"Newton's method encountered a derivative that was too "
"close to zero.\n\tPlease provide a different initial guess "
"or enable the linesearch if you have not.")
pk = -fk / fpk
alpha = 1.0
xkp1 = xk + alpha * pk
variable.set_value(xkp1, skip_validation=True)
# perform line search
if linesearch:
c1 = 0.999 # ensure sufficient progress
while alpha > alpha_min:
# check if the value at xkp1 has sufficient reduction in
# the residual
fkp1 = value(expr, exception=False)
# HACK for Python3 support, pending resolution of #879
# Issue #879 also pertains to other checks for "complex"
# in this method.
if type(fkp1) is complex:
# We cannot perform computations on complex numbers
fkp1 = None
if fkp1 is not None and fkp1**2 < c1 * fk**2:
# found an alpha value with sufficient reduction
# continue to the next step
fk = fkp1
break
alpha /= 2.0
xkp1 = xk + alpha * pk
variable.set_value(xkp1, skip_validation=True)
if alpha <= alpha_min:
residual = value(expr, exception=False)
if residual is None or type(residual) is complex:
residual = "{function evaluation error}"
raise RuntimeError(
"Linesearch iteration limit reached; remaining "
"residual = %s." % (residual, ))
#
# Re-set the variable value to trigger any warnings WRT the final
# variable state
variable.set_value(variable.value)
def build_model(project_dir, dlc_thresh=0.5) -> Tuple[ConcreteModel, Dict]:
#SYMBOLIC ROTATION MATRIX FUNCTIONS
print("Generate and load data...")
L = 14 #number of joints in the cheetah model
# defines arrays ofa angles, velocities and accelerations
phi = [sp.symbols(f"\\phi_{{{l}}}") for l in range(L)]
theta = [sp.symbols(f"\\theta_{{{l}}}") for l in range(L)]
psi = [sp.symbols(f"\\psi_{{{l}}}") for l in range(L)]
#ROTATIONS
# head
RI_0 = rot_z(psi[0]) @ rot_x(phi[0]) @ rot_y(theta[0])
R0_I = RI_0.T
# neck
RI_1 = rot_z(psi[1]) @ rot_x(phi[1]) @ rot_y(theta[1]) @ RI_0
R1_I = RI_1.T
# front torso
RI_2 = rot_y(theta[2]) @ RI_1
R2_I = RI_2.T
# back torso
RI_3 = rot_z(psi[3])@ rot_x(phi[3]) @ rot_y(theta[3]) @ RI_2
R3_I = RI_3.T
# tail base
RI_4 = rot_z(psi[4]) @ rot_y(theta[4]) @ RI_3
R4_I = RI_4.T
# tail mid
RI_5 = rot_z(psi[5]) @ rot_y(theta[5]) @ RI_4
R5_I = RI_5.T
#l_shoulder
RI_6 = rot_y(theta[6]) @ RI_2
R6_I = RI_6.T
#l_front_knee
RI_7 = rot_y(theta[7]) @ RI_6
R7_I = RI_7.T
#r_shoulder
RI_8 = rot_y(theta[8]) @ RI_2
R8_I = RI_8.T
#r_front_knee
RI_9 = rot_y(theta[9]) @ RI_8
R9_I = RI_9.T
#l_hip
RI_10 = rot_y(theta[10]) @ RI_3
R10_I = RI_10.T
#l_back_knee
RI_11 = rot_y(theta[11]) @ RI_10
R11_I = RI_11.T
#r_hip
RI_12 = rot_y(theta[12]) @ RI_3
R12_I = RI_12.T
#r_back_knee
RI_13 = rot_y(theta[13]) @ RI_12
R13_I = RI_13.T
# defines the position, velocities and accelerations in the inertial frame
x, y, z = sp.symbols("x y z")
dx, dy, dz = sp.symbols("\\dot{x} \\dot{y} \\dot{z}")
ddx, ddy, ddz = sp.symbols("\\ddot{x} \\ddot{y} \\ddot{z}")
# SYMBOLIC CHEETAH POSE POSITIONS
p_head = sp.Matrix([x, y, z])
p_l_eye = p_head + R0_I @ sp.Matrix([0, 0.03, 0])
p_r_eye = p_head + R0_I @ sp.Matrix([0, -0.03, 0])
p_nose = p_head + R0_I @ sp.Matrix([0.055, 0, -0.055])
p_neck_base = p_head + R1_I @ sp.Matrix([-0.28, 0, 0])
p_spine = p_neck_base + R2_I @ sp.Matrix([-0.37, 0, 0])
p_tail_base = p_spine + R3_I @ sp.Matrix([-0.37, 0, 0])
p_tail_mid = p_tail_base + R4_I @ sp.Matrix([-0.28, 0, 0])
p_tail_tip = p_tail_mid + R5_I @ sp.Matrix([-0.36, 0, 0])
p_l_shoulder = p_neck_base + R2_I @ sp.Matrix([-0.04, 0.08, -0.10])
p_l_front_knee = p_l_shoulder + R6_I @ sp.Matrix([0, 0, -0.24])
p_l_front_ankle = p_l_front_knee + R7_I @ sp.Matrix([0, 0, -0.28])
p_r_shoulder = p_neck_base + R2_I @ sp.Matrix([-0.04, -0.08, -0.10])
p_r_front_knee = p_r_shoulder + R8_I @ sp.Matrix([0, 0, -0.24])
p_r_front_ankle = p_r_front_knee + R9_I @ sp.Matrix([0, 0, -0.28])
p_l_hip = p_tail_base + R3_I @ sp.Matrix([0.12, 0.08, -0.06])
p_l_back_knee = p_l_hip + R10_I @ sp.Matrix([0, 0, -0.32])
p_l_back_ankle = p_l_back_knee + R11_I @ sp.Matrix([0, 0, -0.25])
p_r_hip = p_tail_base + R3_I @ sp.Matrix([0.12, -0.08, -0.06])
p_r_back_knee = p_r_hip + R12_I @ sp.Matrix([0, 0, -0.32])
p_r_back_ankle = p_r_back_knee + R13_I @ sp.Matrix([0, 0, -0.25])
positions = sp.Matrix([
p_l_eye.T, p_r_eye.T, p_nose.T,
p_neck_base.T, p_spine.T,
p_tail_base.T, p_tail_mid.T, p_tail_tip.T,
p_l_shoulder.T, p_l_front_knee.T, p_l_front_ankle.T,
p_r_shoulder.T, p_r_front_knee.T, p_r_front_ankle.T,
p_l_hip.T, p_l_back_knee.T, p_l_back_ankle.T,
p_r_hip.T, p_r_back_knee.T, p_r_back_ankle.T
])
# ========= LAMBDIFY SYMBOLIC FUNCTIONS ========
func_map = {"sin":sin, "cos":cos, "ImmutableDenseMatrix":np.array}
sym_list = [x, y, z, *phi, *theta, *psi]
pose_to_3d = sp.lambdify(sym_list, positions, modules=[func_map])
pos_funcs = []
for i in range(positions.shape[0]):
lamb = sp.lambdify(sym_list, positions[i,:], modules=[func_map])
pos_funcs.append(lamb)
scene_path = os.path.join(project_dir, "scene_sba.json")
K_arr, D_arr, R_arr, t_arr, _ = utils.load_scene(scene_path)
D_arr = D_arr.reshape((-1,4))
# markers = misc.get_markers()
markers = dict(enumerate([
"l_eye", "r_eye", "nose",
"neck_base", "spine",
"tail_base", "tail1", "tail2",
"l_shoulder", "l_front_knee", "l_front_ankle",
"r_shoulder", "r_front_knee", "r_front_ankle",
"l_hip", "l_back_knee", "l_back_ankle",
"r_hip", "r_back_knee", "r_back_ankle"
]))
df_paths = sorted(glob.glob(os.path.join(project_dir, '*.h5')))
points_2d_df = utils.create_dlc_points_2d_file(df_paths)
def get_meas_from_df(n, c, l, d):
n_mask = points_2d_df["frame"]== n-1
l_mask = points_2d_df["marker"]== markers[l-1]
c_mask = points_2d_df["camera"]== c-1
d_idx = {1:"x", 2:"y"}
val = points_2d_df[n_mask & l_mask & c_mask]
return val[d_idx[d]].values[0]
def get_likelihood_from_df(n, c, l):
n_mask = points_2d_df["frame"]== n-1
l_mask = points_2d_df["marker"]== markers[l-1]
c_mask = points_2d_df["camera"]== c-1
val = points_2d_df[n_mask & l_mask & c_mask]
return val["likelihood"].values[0]
# Parameters
h = 1/120 #timestep
end_frame = 165
start_frame = 50
N = end_frame-start_frame # N > start_frame !!
P = 3 + len(phi)+len(theta)+len(psi)
L = len(pos_funcs)
C = len(K_arr)
D2 = 2
D3 = 3
W = 2
proj_funcs = [pt3d_to_x2d, pt3d_to_y2d]
# measurement standard deviation
R = np.array([
1.24,
1.18,
1.2,
2.08,
2.04,
2.52,
2.73,
1.83,
3.4,
2.91,
2.85,
# 2.27, # l_front_paw
3.47,
2.75,
2.69,
# 2.24, # r_front_paw
3.53,
2.69,
2.49,
# 2.34, # l_back_paw
3.26,
2.76,
2.33,
# 2.4, # r_back_paw
])
R_pw = np.repeat(7, len(R))
Q = np.array([ # model parameters variance
4.0,
7.0,
5.0,
13.0,
32.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
9.0,
18.0,
43.0,
53.0,
90.0,
118.0,
247.0,
186.0,
194.0,
164.0,
295.0,
243.0,
334.0,
149.0,
26.0,
12.0,
0.0,
34.0,
43.0,
51.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
])**2
triangulate_func = calib.triangulate_points_fisheye
points_2d_filtered_df = points_2d_df[points_2d_df['likelihood']>dlc_thresh]
points_3d_df = calib.get_pairwise_3d_points_from_df(points_2d_filtered_df, K_arr, D_arr, R_arr, t_arr, triangulate_func)
# estimate initial points
nose_pts = points_3d_df[points_3d_df["marker"]=="nose"][["x", "y", "z", "frame"]].values
x_slope, x_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,0])
y_slope, y_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,1])
z_slope, z_intercept, *_ = stats.linregress(nose_pts[:,3], nose_pts[:,2])
frame_est = np.arange(N)
x_est = frame_est*x_slope + x_intercept
y_est = frame_est*y_slope + y_intercept
z_est = frame_est*z_slope + z_intercept
print(x_est.shape)
psi_est = np.arctan2(y_slope, x_slope)
print("Build optimisation problem - Start")
m = ConcreteModel(name = "Skeleton")
# ===== SETS =====
m.N = RangeSet(N) #number of timesteps in trajectory
m.P = RangeSet(P) #number of pose parameters (x, y, z, phi_1..n, theta_1..n, psi_1..n)
m.L = RangeSet(L) #number of labels
m.C = RangeSet(C) #number of cameras
m.D2 = RangeSet(D2) #dimensionality of measurements
m.D3 = RangeSet(D3) #dimensionality of measurements
m.W = RangeSet(W) # Number of pairwise terms to include.
def init_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n+start_frame, c, l)
if likelihood > dlc_thresh:
return 1/R[l-1]
else:
return 0
m.meas_err_weight = Param(m.N, m.C, m.L, initialize=init_meas_weights, mutable=True) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_pw_meas_weights(model, n, c, l):
likelihood = get_likelihood_from_df(n+start_frame, c, l)
if likelihood <= dlc_thresh:
return 1/R_pw[l-1]
else:
return 0
m.meas_pw_err_weight = Param(m.N, m.C, m.L, initialize=init_pw_meas_weights, mutable=True) # IndexError: index 0 is out of bounds for axis 0 with size 0
def init_model_weights(m, p):
if Q[p-1] != 0.0:
return 1/Q[p-1]
else:
return 0
m.model_err_weight = Param(m.P, initialize=init_model_weights)
m.h = h
def init_measurements_df(m, n, c, l, d2):
return get_meas_from_df(n+start_frame, c, l, d2)
m.meas = Param(m.N, m.C, m.L, m.D2, initialize=init_measurements_df)
# resultsfilename='C://Users//user-pc//Desktop//pwpoints.pickle'
# with open(resultsfilename, 'rb') as f:
# data=pickle.load(f)
pw_data = {}
for cam in range(C):
pw_data[cam] = load_data(f"/Users/zico/msc/dev/FYP/data/09_03_2019/lily/run/cam{cam+1}-predictions.pickle")
index_dict = {"nose":23, "r_eye":0, "l_eye":1, "neck_base":24, "spine":6, "tail_base":22, "tail1":11,
"tail2":12, "l_shoulder":13,"l_front_knee":14,"l_front_ankle":15,"r_shoulder":2,
"r_front_knee":3, "r_front_ankle":4,"l_hip":17,"l_back_knee":18, "l_back_ankle":19,
"r_hip":7,"r_back_knee":8,"r_back_ankle":9}
pair_dict = {"r_eye":[23, 24], "l_eye":[23, 24], "nose":[6, 24], "neck_base":[6, 23], "spine":[22, 24], "tail_base":[6, 11], "tail1":[6, 22],
"tail2":[11, 22], "l_shoulder":[6, 24],"l_front_knee":[6, 24],"l_front_ankle":[6, 24],"r_shoulder":[6, 24],
"r_front_knee":[6, 24], "r_front_ankle":[6, 24],"l_hip":[6, 22],"l_back_knee":[6, 22], "l_back_ankle":[6, 22],
"r_hip":[6, 22],"r_back_knee":[6, 22],"r_back_ankle":[6, 22]}
def init_pw_measurements(m, n, c, l, d2, w):
pw_values = pw_data[c-1][n+start_frame]
marker = markers[l-1]
base = pair_dict[marker][w-1]
val = pw_values['pose'][d2-1::3]
val_pw = pw_values['pws'][:,:,:,d2-1]
return val[base]+val_pw[0,base,index_dict[marker]]
m.pw_meas = Param(m.N, m.C, m.L, m.D2, m.W, initialize=init_pw_measurements, within=Any)
"""
def init_pw_measurements2(m, n, c, l, d2):
val=0
if n-1 >= 20 and n-1 < 30:
fn = 10*(c-1)+(n-20)-1
x=data[fn]['pose'][0::3]
y=data[fn]['pose'][1::3]
xpw=data[fn]['pws'][:,:,:,0]
ypw=data[fn]['pws'][:,:,:,1]
marker = markers[l-1]
if "ankle" in marker:
base = pair_dict[marker][1]
if d2==1:
val=x[base]+xpw[0,base,index_dict[marker]]
elif d2==2:
val=y[base]+ypw[0,base,index_dict[marker]]
#sum/=len(pair_dict[marker])
return val
else:
return(0.0)
m.pw_meas2 = Param(m.N, m.C, m.L, m.D2, initialize=init_pw_measurements2, within=Any)
"""
# ===== VARIABLES =====
m.x = Var(m.N, m.P) #position
m.dx = Var(m.N, m.P) #velocity
m.ddx = Var(m.N, m.P) #acceleration
m.poses = Var(m.N, m.L, m.D3)
m.slack_model = Var(m.N, m.P)
m.slack_meas = Var(m.N, m.C, m.L, m.D2, initialize=0.0)
m.slack_pw_meas = Var(m.N, m.C, m.L, m.D2, m.W, initialize=0.0)
# ===== VARIABLES INITIALIZATION =====
init_x = np.zeros((N-start_frame, P))
init_x[:,0] = x_est[start_frame: start_frame+N] #x
init_x[:,1] = y_est[start_frame: start_frame+N] #y
init_x[:,2] = z_est[start_frame: start_frame+N] #z
init_x[:,31] = psi_est #yaw - psi
init_dx = np.zeros((N, P))
init_ddx = np.zeros((N, P))
for n in m.N:
for p in m.P:
if n<len(init_x): #init using known values
m.x[n,p].value = init_x[n-1,p-1]
m.dx[n,p].value = init_dx[n-1,p-1]
m.ddx[n,p].value = init_ddx[n-1,p-1]
else: #init using last known value
m.x[n,p].value = init_x[-1,p-1]
m.dx[n,p].value = init_dx[-1,p-1]
m.ddx[n,p].value = init_ddx[-1,p-1]
#init pose
var_list = [m.x[n,p].value for p in range(1, P+1)]
for l in m.L:
[pos] = pos_funcs[l-1](*var_list)
for d3 in m.D3:
m.poses[n,l,d3].value = pos[d3-1]
# ===== CONSTRAINTS =====
# 3D POSE
def pose_constraint(m,n,l,d3):
#get 3d points
var_list = [m.x[n,p] for p in range(1, P+1)]
[pos] = pos_funcs[l-1](*var_list)
return pos[d3-1] == m.poses[n,l,d3]
m.pose_constraint = Constraint(m.N, m.L, m.D3, rule=pose_constraint)
def backwards_euler_pos(m,n,p): # position
if n > 1:
# return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n-1,p] + m.h**2 * m.ddx[n-1,p]/2
return m.x[n,p] == m.x[n-1,p] + m.h*m.dx[n,p]
else:
return Constraint.Skip
m.integrate_p = Constraint(m.N, m.P, rule = backwards_euler_pos)
def backwards_euler_vel(m,n,p): # velocity
if n > 1:
return m.dx[n,p] == m.dx[n-1,p] + m.h*m.ddx[n,p]
else:
return Constraint.Skip
m.integrate_v = Constraint(m.N, m.P, rule = backwards_euler_vel)
# MODEL
def constant_acc(m, n, p):
if n > 1:
return m.ddx[n,p] == m.ddx[n-1,p] + m.slack_model[n,p]
else:
return Constraint.Skip
m.constant_acc = Constraint(m.N, m.P, rule = constant_acc)
# MEASUREMENT
def measurement_constraints(m, n, c, l, d2):
#project
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.meas[n, c, l, d2] - m.slack_meas[n, c, l, d2] ==0
m.measurement = Constraint(m.N, m.C, m.L, m.D2, rule = measurement_constraints)
def pw_measurement_constraints(m, n, c, l, d2, w):
#project
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.pw_meas[n, c, l, d2, w] - m.slack_pw_meas[n, c, l, d2, w] ==0.0
m.pw_measurement = Constraint(m.N, m.C, m.L, m.D2, m.W, rule = pw_measurement_constraints)
"""
def pw_measurement_constraints2(m, n, c, l, d2):
#project
if n-1 >= 20 and n-1 < 30 and "ankle" in markers[l-1]:
K, D, R, t = K_arr[c-1], D_arr[c-1], R_arr[c-1], t_arr[c-1]
x, y, z = m.poses[n,l,1], m.poses[n,l,2], m.poses[n,l,3]
return proj_funcs[d2-1](x, y, z, K, D, R, t) - m.pw_meas2[n, c, l, d2] - m.slack_meas[n, c, l, d2] ==0.0
else:
return(Constraint.Skip)
m.pw_measurement2 = Constraint(m.N, m.C, m.L, m.D2, rule = pw_measurement_constraints2)
"""
#===== POSE CONSTRAINTS (Note 1 based indexing for pyomo!!!!...@#^!@#&) =====
#Head
def head_psi_0(m,n):
return abs(m.x[n,4]) <= np.pi/6
m.head_psi_0 = Constraint(m.N, rule=head_psi_0)
def head_theta_0(m,n):
return abs(m.x[n,18]) <= np.pi/6
m.head_theta_0 = Constraint(m.N, rule=head_theta_0)
#Neck
def neck_phi_1(m,n):
return abs(m.x[n,5]) <= np.pi/6
m.neck_phi_1 = Constraint(m.N, rule=neck_phi_1)
def neck_theta_1(m,n):
return abs(m.x[n,19]) <= np.pi/6
m.neck_theta_1 = Constraint(m.N, rule=neck_theta_1)
def neck_psi_1(m,n):
return abs(m.x[n,33]) <= np.pi/6
m.neck_psi_1 = Constraint(m.N, rule=neck_psi_1)
#Front torso
def front_torso_theta_2(m,n):
return abs(m.x[n,20]) <= np.pi/6
m.front_torso_theta_2 = Constraint(m.N, rule=front_torso_theta_2)
#Back torso
def back_torso_theta_3(m,n):
return abs(m.x[n,21]) <= np.pi/6
m.back_torso_theta_3 = Constraint(m.N, rule=back_torso_theta_3)
# --- Back torso phi constraint - uncomment if needed! ---
def back_torso_phi_3(m,n):
return abs(m.x[n,7]) <= np.pi/6
m.back_torso_phi_3 = Constraint(m.N, rule=back_torso_phi_3)
def back_torso_psi_3(m,n):
return abs(m.x[n,35]) <= np.pi/6
m.back_torso_psi_3 = Constraint(m.N, rule=back_torso_psi_3)
#Tail base
def tail_base_theta_4(m,n):
return abs(m.x[n,22]) <= np.pi/1.5
m.tail_base_theta_4 = Constraint(m.N, rule=tail_base_theta_4)
def tail_base_psi_4(m,n):
return abs(m.x[n,36]) <= np.pi/1.5
m.tail_base_psi_4 = Constraint(m.N, rule=tail_base_psi_4)
#Tail base
def tail_mid_theta_5(m,n):
return abs(m.x[n,23]) <= np.pi/1.5
m.tail_mid_theta_5 = Constraint(m.N, rule=tail_mid_theta_5)
def tail_mid_psi_5(m,n):
return abs(m.x[n,37]) <= np.pi/1.5
m.tail_mid_psi_5 = Constraint(m.N, rule=tail_mid_psi_5)
#Front left leg
def l_shoulder_theta_6(m,n):
return abs(m.x[n,24]) <= np.pi/2
m.l_shoulder_theta_6 = Constraint(m.N, rule=l_shoulder_theta_6)
def l_front_knee_theta_7(m,n):
return abs(m.x[n,25] + np.pi/2) <= np.pi/2
m.l_front_knee_theta_7 = Constraint(m.N, rule=l_front_knee_theta_7)
#Front right leg
def r_shoulder_theta_8(m,n):
return abs(m.x[n,26]) <= np.pi/2
m.r_shoulder_theta_8 = Constraint(m.N, rule=r_shoulder_theta_8)
def r_front_knee_theta_9(m,n):
return abs(m.x[n,27] + np.pi/2) <= np.pi/2
m.r_front_knee_theta_9 = Constraint(m.N, rule=r_front_knee_theta_9)
#Back left leg
def l_hip_theta_10(m,n):
return abs(m.x[n,28]) <= np.pi/2
m.l_hip_theta_10 = Constraint(m.N, rule=l_hip_theta_10)
def l_back_knee_theta_11(m,n):
return abs(m.x[n,29] - np.pi/2) <= np.pi/2
m.l_back_knee_theta_11 = Constraint(m.N, rule=l_back_knee_theta_11)
#Back right leg
def r_hip_theta_12(m,n):
return abs(m.x[n,30]) <= np.pi/2
m.r_hip_theta_12 = Constraint(m.N, rule=r_hip_theta_12)
def r_back_knee_theta_13(m,n):
return abs(m.x[n,31] - np.pi/2) <= np.pi/2
m.r_back_knee_theta_13 = Constraint(m.N, rule=r_back_knee_theta_13)
# ======= OBJECTIVE FUNCTION =======
def obj(m):
slack_model_err = 0.0
slack_meas_err = 0.0
slack_pw_meas_err = 0.0
for n in m.N:
#Model Error
for p in m.P:
slack_model_err += m.model_err_weight[p] * m.slack_model[n, p] ** 2
#Measurement Error
for l in m.L:
for c in m.C:
for d2 in m.D2:
slack_meas_err += misc.redescending_loss(m.meas_err_weight[n, c, l] * m.slack_meas[n, c, l, d2], 3, 7, 20)
for w in m.W:
slack_meas_err += misc.redescending_loss(m.meas_pw_err_weight[n, c, l] * m.slack_pw_meas[n, c, l, d2, w], 3, 7, 20)
return slack_meas_err + slack_model_err
m.obj = Objective(rule = obj)
print("Build optimisation problem - End")
return m, pose_to_3d
def apply(self, **kwds):
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug("Calling ConnectorExpander")
instance = kwds['instance']
blockList = list(instance.block_data_objects(active=True))
noConnectors = True
for b in blockList:
if b.component_map(Connector):
noConnectors = False
break
if noConnectors:
return
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" Connectors found!")
#
# At this point, there are connectors in the model, so we must
# look for constraints that involve connectors and expand them.
#
#options = kwds['options']
#model = kwds['model']
# In general, blocks should be relatively self-contained, so we
# should build the connectors from the "bottom up":
blockList.reverse()
# Expand each constraint involving a connector
for block in blockList:
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" block: " + block.name)
CCC = {}
for name, constraint in itertools.chain\
( iteritems(block.component_map(Constraint)),
iteritems(block.component_map(ConstraintList)) ):
cList = []
CCC[name+'.expanded'] = cList
for idx, c in iteritems(constraint._data):
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (looking at constraint %s[%s])", name, idx)
connectors = []
self._gather_connectors(c.body, connectors)
if len(connectors) == 0:
continue
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (found connectors in constraint)")
# Validate that all connectors match
errors, ref, skip = self._validate_connectors(connectors)
if errors:
logger.error(
( "Connector mismatch: errors detected when "
"constructing constraint %s\n " %
(name + (idx and '[%s]' % idx or '')) ) +
'\n '.join(reversed(errors)) )
raise ValueError(
"Connector mismatch in constraint %s" % \
name + (idx and '[%s]' % idx or ''))
if __debug__ and logger.isEnabledFor(logging.DEBUG): #pragma:nocover
logger.debug(" (connectors valid)")
# Fill in any empty connectors
for conn in connectors:
if conn.vars:
continue
for var in ref.vars:
if var in skip:
continue
v = Var()
block.add_component(conn.local_name + '.auto.' + var, v)
conn.vars[var] = v
v.construct()
# OK - expand this constraint
self._expand_constraint(block, name, idx, c, ref, skip, cList)
# Now deactivate the original constraint
c.deactivate()
for name, exprs in iteritems(CCC):
cList = ConstraintList()
block.add_component( name, cList )
cList.construct()
for expr in exprs:
cList.add(expr)
# Now, go back and implement VarList aggregators
for block in blockList:
for conn in itervalues(block.component_map(Connector)):
for var, aggregator in iteritems(conn.aggregators):
c = Constraint(expr=aggregator(block, var))
block.add_component(
conn.local_name + '.' + var.local_name + '.aggregate', c)
c.construct()
