def custom_dynamic(states: MX, controls: MX, parameters: MX,
nlp: NonLinearProgram) -> tuple:
"""
The dynamics of the system using an external force (see custom_dynamics for more explanation)
Parameters
----------
states: MX
The current states of the system
controls: MX
The current controls of the system
parameters: MX
The current parameters of the system
nlp: NonLinearProgram
A reference to the phase of the ocp
Returns
-------
The state derivative
"""
q = DynamicsFunctions.get(nlp.states["q"], states)
qdot = DynamicsFunctions.get(nlp.states["qdot"], states)
tau = DynamicsFunctions.get(nlp.controls["tau"], controls)
force_vector = MX.zeros(6)
force_vector[5] = 100 * q[0]**2
f_ext = biorbd.VecBiorbdSpatialVector()
f_ext.append(biorbd.SpatialVector(force_vector))
qddot = nlp.model.ForwardDynamics(q, qdot, tau, f_ext).to_mx()
return qdot, qddot
def test_blockdiag(self):
# Test blockdiag with DM
correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1],
[0, 0, 1, 1, 1], [0, 0, 1, 1, 1]])
a = DM.ones(2, 2)
b = DM.ones(3, 3)
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res))
# MX and DM mix
a = MX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = MX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and DM mix
a = SX.sym('a', 2, 2)
b = DM.ones(1, 1)
correct_res = SX.zeros(3, 3)
correct_res[:2, :2] = a
correct_res[2:, 2:] = b
res = blockdiag(a, b)
self.assertTrue(is_equal(res, correct_res, 30))
# SX and MX
a = SX.sym('a', 2, 2)
b = MX.sym('b', 2, 2)
self.assertRaises(ValueError, blockdiag, a, b)
def blockdiag(*matrices_list):
"""Receives a list of matrices and return a block diagonal.
:param DM|MX|SX matrices_list: list of matrices
"""
size_1 = sum([m.size1() for m in matrices_list])
size_2 = sum([m.size2() for m in matrices_list])
matrix_types = [type(m) for m in matrices_list]
if SX in matrix_types and MX in matrix_types:
raise ValueError(
"Can not mix MX and SX types. Types give: {}".format(matrix_types))
if SX in matrix_types:
matrix = SX.zeros(size_1, size_2)
elif MX in matrix_types:
matrix = MX.zeros(size_1, size_2)
else:
matrix = DM.zeros(size_1, size_2)
index_1 = 0
index_2 = 0
for m in matrices_list:
matrix[index_1:index_1 + m.size1(), index_2:index_2 + m.size2()] = m
index_1 += m.size1()
index_2 += m.size2()
return matrix
def contact_force_continuity(pen_node: PenaltyNode, idx_pre, idx_post):
final_contact_z = sum1(pen_node[0].nlp.contact_forces_func(pen_node[0].x[0], pen_node[0].u[0], pen_node[0].p)[idx_pre, :])
if idx_post:
starting_contact_z = sum1(pen_node[1].nlp.contact_forces_func(pen_node[1].x[0], pen_node[1].u[0], pen_node[1].p)[idx_post, :])
else:
starting_contact_z = MX.zeros(final_contact_z.shape)
return final_contact_z - starting_contact_z
def my_parameter_function(biorbd_model, value, extra_value):
new_gravity = MX.zeros(3, 1)
new_gravity[2] = value + extra_value
biorbd_model.setGravity(new_gravity)
def onestep_reachability(p_center,
ssm,
k_ff,
l_mu,
l_sigma,
q_shape=None,
k_fb=None,
c_safety=1.,
a=None,
b=None,
t_z_gp=None):
""" Overapproximate the reachable set of states under affine control law
given a system of the form:
x_{t+1} = \mathcal{N}(\mu(x_t,u_t), \Sigma(x_t,u_t)),
where x,\mu \in R^{n_s}, u \in R^{n_u} and \Sigma^{n_s \times n_s} are given bei the
gp predictive mean and variance respectively
we approximate the reachset of a set of inputs x_t \in \epsilon(p,Q)
describing an ellipsoid with center p and shape matrix Q
under the control low u_t = Kx_t + k
Parameters
----------
p_center: n_s x 1 array[float]
Center of state ellipsoid
gp: SimpleGPModel
The gp representing the dynamics
k_ff: n_u x 1 array[float]
The additive term of the controls
l_mu: 1d_array of size n_s
Set of Lipschitz constants on the Gradients of the mean function (per state
dimension)
l_sigma: 1d_array of size n_s
Set of Lipschitz constants of the predictive variance (per state dimension)
q_shape: np.ndarray[float], array of shape n_s x n_s, optional
Shape matrix of state ellipsoid
k_fb: n_u x n_s array[float], optional
The state feedback-matrix for the controls
c_safety: float, optional
The scaling of the semi-axes of the uncertainty matrix
corresponding to a level-set of the gaussian pdf.
Returns:
-------
p_new: n_s x 1 array[float]
Center of the overapproximated next state ellipsoid
Q_new: np.ndarray[float], array of shape n_s x n_s
Shape matrix of the overapproximated next state ellipsoid.
"""
n_s = np.shape(p_center)[0]
n_u = np.shape(k_ff)[0]
if t_z_gp is None:
t_z_gp = MX.eye(n_s)
if a is None:
a = MX.eye(n_s)
b = MX.zeros(n_s, n_u)
if q_shape is None: # the state is a point
u_p = k_ff
x_bar = mtimes(t_z_gp, p_center)
mu_new, pred_var, _ = ssm(x_bar.T, u_p.T)
p_lin = mtimes(a, p_center) + mtimes(b, u_p)
p_1 = p_lin + mu_new
rkhs_bound = c_safety * sqrt(pred_var)
q_1 = ellipsoid_from_rectangle(rkhs_bound)
return p_1, q_1, pred_var
else: # the state is a (ellipsoid) set
# compute the linearization centers
x_bar = mtimes(t_z_gp, p_center) # center of the state ellipsoid
u_bar = k_ff # u_bar = K*(u_bar-u_bar) + k = k
z_bar = vertcat(x_bar, u_bar)
# compute the zero and first order matrices
mu_0, sigm_0, jac_mu = ssm(x_bar.T, u_bar.T)
n_x_in = np.shape(t_z_gp)[0]
a_mu = jac_mu[:, :n_x_in]
a_mu = mtimes(a_mu, t_z_gp)
b_mu = jac_mu[:, n_x_in:]
# reach set of the affine terms
H = a + a_mu + mtimes(b_mu + b, k_fb)
p_0 = mu_0 + mtimes(a, p_center) + mtimes(b, u_bar)
Q_0 = mtimes(H, mtimes(q_shape, H.T))
ub_mean, ub_sigma = compute_remainder_overapproximations(
q_shape, k_fb, l_mu, l_sigma)
# computing the box approximate to the lagrange remainder
Q_lagrange_mu = ellipsoid_from_rectangle(ub_mean)
p_lagrange_mu = MX.zeros((n_s, 1))
b_sigma_eps = c_safety * (sqrt(sigm_0) + ub_sigma)
Q_lagrange_sigm = ellipsoid_from_rectangle(b_sigma_eps)
p_lagrange_sigm = MX.zeros((n_s, 1))
p_sum_lagrange, Q_sum_lagrange = sum_two_ellipsoids(
p_lagrange_sigm, Q_lagrange_sigm, p_lagrange_mu, Q_lagrange_mu)
p_1, q_1 = sum_two_ellipsoids(p_sum_lagrange, Q_sum_lagrange, p_0, Q_0)
return p_1, q_1, sigm_0
def _construct_upd_z_nlp(self):
# construct variables
self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct),
entry('z_ij', struct=self.q_ij_struct)])
var = struct_symMX(self._var_struct_updz)
z_i = self.q_i_struct(var['z_i'])
z_ij = self.q_ij_struct(var['z_ij'])
# construct parameters
self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct),
entry('x_j', struct=self.q_ij_struct),
entry('l_i', struct=self.q_i_struct),
entry('l_ij', struct=self.q_ij_struct),
entry('t'), entry('T'), entry('rho'),
entry('par', struct=self.par_struct)])
par = struct_symMX(self._par_struct_updz)
x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j'])
l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij'])
t, T, rho = par['t'], par['T'], par['rho']
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# construct constraints
constraints, lb, ub = [], [], []
for con in self.constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_i[label, name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_i.items():
if s.getName() == sym.getName():
c = substitute(c, sym, par['par', name])
constraints.append(c)
lb.append(con[1])
ub.append(con[2])
self.lb_updz, self.ub_updz = lb, ub
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label, name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name in q_ij.keys():
x = x_j[str(nghb), child.label, name]
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
# construct problem
prob, compile_time = self.father.create_nlp(var, par, obj,
constraints, self.options)
self.problem_upd_z = prob
def construct_upd_xz(self, problem=None):
# construct optifather & give reference to problem
self.father_updx = OptiFather(list(self.group.values()))
self.problem.father = self.father_updx
# define z_ij variables
init = self.q_ij_struct(0)
for nghb, q_ij in self.q_ij.items():
for child, q_j in q_ij.items():
for name, ind in q_j.items():
var = np.array(child._values[name])
v = var.T.flatten()[ind]
init[nghb.label, child.label, name, ind] = v
z_ij = self.define_variable('z_ij',
self.q_ij_struct.shape[0],
value=np.array(init.cat))
# define parameters
l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
# put them in the struct format
z_ij = self.q_ij_struct(z_ij)
l_ij = self.q_ij_struct(l_ij)
l_ji = self.q_ji_struct(l_ji)
# get (part of) variables
x_i = self._get_x_variables(symbolic=True)
# construct local copies of parameters
par = {}
for name, s in self.par_global.items():
par[name] = self.define_parameter(name, s.shape[0], s.shape[1])
if problem is None:
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t / T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline(x_i, tf, self.q_i)
self._transform_spline([z_ij, l_ij], tf, self.q_ij)
self._transform_spline(l_ji, tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
for nghb in self.q_ji.keys():
l = l_ji[str(nghb), child.label, name]
obj += mtimes(l.T, x)
for nghb, q_j in self.q_ij.items():
for child in q_j.keys():
for name in q_j[child].keys():
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj -= mtimes(l.T, z)
self.define_objective(obj)
# construct constraints
for con in self.global_constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = x_i[label][name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_global.items():
if s.name() == sym.name():
c = substitute(c, sym, par[name])
lb, ub = con[1], con[2]
self.define_constraint(c, lb, ub)
# construct problem
prob, buildtime = self.father_updx.construct_problem(
self.options, str(self._index), problem)
self.problem_upd_xz = prob
self.father_updx.init_transformations(
self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_dd()
return buildtime
def custom_mx_fail(pn):
if pn.u is None:
return None
u = pn.nlp.controls
return MX.zeros(u.shape), u.cx, MX.zeros(u.shape)
def configure_soft_contact_function(ocp, nlp):
"""
Configure the soft contact sphere
Parameters
----------
ocp: OptimalControlProgram
A reference to the ocp
nlp: NonLinearProgram
A reference to the phase
"""
global_soft_contact_force_func = MX.zeros(
nlp.model.nbSoftContacts() * 6, 1)
n = nlp.model.nbQ()
component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"]
for i_sc in range(nlp.model.nbSoftContacts()):
soft_contact = nlp.model.softContact(i_sc)
global_soft_contact_force_func[i_sc * 6:(i_sc + 1) * 6, :] = (
biorbd.SoftContactSphere(soft_contact).computeForceAtOrigin(
nlp.model, nlp.states.mx_reduced[:n],
nlp.states.mx_reduced[n:]).to_mx())
nlp.soft_contact_forces_func = Function(
"soft_contact_forces_func",
[
nlp.states.mx_reduced, nlp.controls.mx_reduced,
nlp.parameters.mx
],
[global_soft_contact_force_func],
["x", "u", "p"],
["soft_contact_forces"],
).expand()
for i_sc in range(nlp.model.nbSoftContacts()):
all_soft_contact_names = []
all_soft_contact_names.extend([
f"{nlp.model.softContactName(i_sc).to_string()}_{name}"
for name in component_list if nlp.model.softContactName(
i_sc).to_string() not in all_soft_contact_names
])
if "soft_contact_forces" in nlp.plot_mapping:
phase_mappings = nlp.plot_mapping["soft_contact_forces"]
else:
soft_contact_names_in_phase = [
f"{nlp.model.softContactName(i_sc).to_string()}_{name}"
for name in component_list if nlp.model.softContactName(
i_sc).to_string() not in all_soft_contact_names
]
phase_mappings = Mapping([
i for i, c in enumerate(all_soft_contact_names)
if c in soft_contact_names_in_phase
])
nlp.plot[
f"soft_contact_forces_{nlp.model.softContactName(i_sc).to_string()}"] = CustomPlot(
lambda t, x, u, p: nlp.soft_contact_forces_func(x, u, p)[
(i_sc * 6):((i_sc + 1) * 6), :],
plot_type=PlotType.INTEGRATED,
axes_idx=phase_mappings,
legend=all_soft_contact_names,
)
def construct_upd_xz(self, problem=None):
# construct optifather & give reference to problem
self.father_updx = OptiFather(self.group.values())
self.problem.father = self.father_updx
# define z_ij variables
init = self.q_ij_struct(0)
for nghb, q_ij in self.q_ij.items():
for child, q_j in q_ij.items():
for name, ind in q_j.items():
var = np.array(child._values[name])
v = var.T.flatten()[ind]
init[nghb.label, child.label, name, ind] = v
z_ij = self.define_variable(
'z_ij', self.q_ij_struct.shape[0], value=np.array(init.cat))
# define parameters
l_ij = self.define_parameter('l_ij', self.q_ij_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
# put them in the struct format
z_ij = self.q_ij_struct(z_ij)
l_ij = self.q_ij_struct(l_ij)
l_ji = self.q_ji_struct(l_ji)
# get (part of) variables
x_i = self._get_x_variables(symbolic=True)
# construct local copies of parameters
par = {}
for name, s in self.par_global.items():
par[name] = self.define_parameter(name, s.shape[0], s.shape[1])
if problem is None:
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline(x_i, tf, self.q_i)
self._transform_spline([z_ij, l_ij], tf, self.q_ij)
self._transform_spline(l_ji, tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
for nghb in self.q_ji.keys():
l = l_ji[str(nghb), child.label, name]
obj += mtimes(l.T, x)
for nghb, q_j in self.q_ij.items():
for child in q_j.keys():
for name in q_j[child].keys():
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj -= mtimes(l.T, z)
self.define_objective(obj)
# construct constraints
for con in self.global_constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = x_i[label][name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.name() in child.symbol_dict:
name = child.symbol_dict[sym.name()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_global.items():
if s.name() == sym.name():
c = substitute(c, sym, par[name])
lb, ub = con[1], con[2]
self.define_constraint(c, lb, ub)
# construct problem
prob, buildtime = self.father_updx.construct_problem(
self.options, str(self._index), problem)
self.problem_upd_xz = prob
self.father_updx.init_transformations(self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_dd()
return buildtime
def custom_mx_fail(pn):
if pn.u is None:
return None
return MX.zeros(pn.u.shape), pn.u, MX.zeros(pn.u.shape)
