def getViolations(self,g,lbg,ubg,reportThreshold=0):
"""
Tests if g >= ubg + reportThreshold
g <= lbg - reportThreshold
Positive reportThreshold supresses barely active bounds
Negative reportThreshold reports not-quite-active bounds
"""
violations = {}
ubviols = g - ubg
lbviols = lbg - g
ubviolsIdx = np.where(C.logic_and(ubviols >= reportThreshold, ubg > lbg))[0]
lbviolsIdx = np.where(C.logic_and(lbviols >= reportThreshold, ubg > lbg))[0]
violations = {}
for k in ubviolsIdx:
(name,time,idx) = self._tags[k]
viol = ('ub',(time,idx),float(ubviols[k]))#,g[k],ubg[k])
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
for k in lbviolsIdx:
(name,time,idx) = self._tags[k]
viol = ('lb',(time,idx),float(lbviols[k]))#,g[k],lbg[k])
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
return violations
def boundsFeedback(self,x,lbx,ubx,reportThreshold=0):
"""
Tests if x >= ub + reportThreshold
x <= lb - reportThreshold
Positive reportThreshold supresses barely active bounds
Negative reportThreshold reports not-quite-active bounds
"""
violations = {}
ubviols = x - ubx
lbviols = lbx - x
ubviolsIdx = np.where(CS.logic_and(ubviols >= reportThreshold, ubx > lbx))[0]
lbviolsIdx = np.where(CS.logic_and(lbviols >= reportThreshold, ubx > lbx))[0]
violations = {}
for k in ubviolsIdx:
(name,time) = self.bndtags[k]
viol = ('ub',time,float(ubviols[k]))
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
for k in lbviolsIdx:
(name,time) = self.bndtags[k]
viol = ('lb',time,float(lbviols[k]))
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
return violations
def __call__(self, x, y):
"""
Evaluate the B-Spline at point (x, y).
The support of this function is the half-open interval [tx[0], tx[-1]) x [ty[0], ty[-1]).
:param x: The coordinate of the point at which to evaluate.
:param y: The ordinate of the point at which to evaluate.
:returns: The spline evaluated at the given point.
"""
z = 0.0
for i in range(len(self.__tx) - self.__kx - 1):
bx = if_else(
logic_and(x >= self.__tx[i],
x <= self.__tx[i + self.__kx + 1]),
self.basis(self.__tx, x, self.__kx, i), 0.0)
for j in range(len(self.__ty) - self.__ky - 1):
by = if_else(
logic_and(y >= self.__ty[j],
y <= self.__ty[j + self.__ky + 1]),
self.basis(self.__ty, y, self.__ky, j), 0.0)
z += self.__w[i *
(len(self.__ty) - self.__ky - 1) + j] * bx * by
return z
def getViolations(self,
g,
lbg,
ubg,
reportThreshold=0,
reportEqViolations=False):
"""
Tests if g >= ubg + reportThreshold
g <= lbg - reportThreshold
Positive reportThreshold supresses barely active bounds
Negative reportThreshold reports not-quite-active bounds
"""
violations = {}
ubviols = g - ubg
lbviols = lbg - g
ubviolsIdx = np.where(
C.logic_and(ubviols >= reportThreshold, ubg > lbg))[0]
lbviolsIdx = np.where(
C.logic_and(lbviols >= reportThreshold, ubg > lbg))[0]
eqviolsIdx = []
if reportEqViolations:
eqviolsIdx = np.where(
C.logic_and(C.fabs(ubviols) >= reportThreshold, ubg == lbg))[0]
violations = {}
for k in ubviolsIdx:
(name, time, idx) = self._tags[k]
viol = ('ub', (time, idx), float(ubviols[k])) #,g[k],ubg[k])
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
for k in lbviolsIdx:
(name, time, idx) = self._tags[k]
viol = ('lb', (time, idx), float(lbviols[k])) #,g[k],lbg[k])
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
for k in eqviolsIdx:
(name, time, idx) = self._tags[k]
viol = ('eq', (time, idx), float(ubviols[k])) #,g[k],lbg[k])
if name not in violations:
violations[name] = [viol]
else:
violations[name].append(viol)
return violations
def get_in_tangent_cone_function_multidim(self, cnstr):
"""Returns a casadi function for the SetConstraint instance when the
SetConstraint is multidimensional."""
if not isinstance(cnstr, SetConstraint):
raise TypeError("in_tangent_cone is only available for" +
" SetConstraint")
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
opt_var = [robot_vel_var]
opt_var_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
expr = cnstr.expression
set_min = cnstr.set_min
set_max = cnstr.set_max
dexpr = cs.jacobian(expr, time_var)
dexpr += cs.jtimes(expr, robot_var, robot_vel_var)
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
opt_var += [virtual_vel_var]
opt_var_names += ["virtual_vel_var"]
dexpr += cs.jtimes(expr, virtual_var, virtual_vel_var)
if input_var is not None:
list_vars += [input_var]
list_vars += ["input_var"]
le = expr - set_min
ue = expr - set_max
le_good = le >= 1e-12
ue_good = ue <= 1e-12
above = cs.dot(le_good - 1, le_good - 1) == 0
below = cs.dot(ue_good - 1, ue_good - 1) == 0
inside = cs.logic_and(above, below)
out_dir = (cs.sign(le) + cs.sign(ue)) / 2.0
# going_in = cs.dot(out_dir, dexpr) <= 0.0
same_signs = cs.sign(le) == cs.sign(ue)
corner = cs.dot(same_signs - 1, same_signs - 1) == 0
dists = (cs.norm_2(dexpr) + 1e-10) * cs.norm_2(out_dir)
corner_handler = cs.if_else(
cs.dot(out_dir, dexpr) < 0.0,
cs.fabs(cs.dot(-out_dir, dexpr)) / dists < cs.np.cos(cs.np.pi / 4),
False, True)
going_in = cs.if_else(corner, corner_handler,
cs.dot(out_dir, dexpr) < 0.0, True)
in_tc = cs.if_else(
inside, # Are we inside?
True, # Then true.
going_in, # If not, check if we're "going_in"
True)
return cs.Function("in_tc_" + cnstr.label.replace(" ", "_"),
list_vars + opt_var, [in_tc],
list_names + opt_var_names,
["in_tc_" + cnstr.label])
def logical_and(x1, x2):
"""
Compute the truth value of x1 AND x2 element-wise.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.logical_and.html
"""
if not is_casadi_type([x1, x2], recursive=True):
return _onp.logical_and(x1, x2)
else:
return _cas.logic_and(x1, x2)
def __call__(self, x):
"""
Evaluate the B-Spline at point x.
The support of this function is the half-open interval [t[0], t[-1]).
:param x: The point at which to evaluate.
:returns: The spline evaluated at the given point.
"""
y = 0.0
for i in range(len(self.__t) - self.__k - 1):
y += if_else(logic_and(x >= self.__t[i], x <= self.__t[i + self.__k + 1]), self.__w[
i] * self.basis(self.__t, x, self.__k, i), 0.0)
return y
def basis(self, t, x, k, i):
"""
Evaluate the B-Spline basis function using Cox-de Boor recursion.
:param x: Point at which to evaluate.
:param k: Order of the basis function.
:param i: Knot number.
:returns: The B-Spline basis function of the given order, at the given knot, evaluated at the given point.
"""
if k == 0:
return if_else(logic_and(t[i] <= x, x < t[i + 1]), 1.0, 0.0)
else:
if t[i] < t[i + k]:
a = (x - t[i]) / (t[i + k] - t[i]) * self.basis(t, x, k - 1, i)
else:
a = 0.0
if t[i + 1] < t[i + k + 1]:
b = (t[i + k + 1] - x) / (t[i + k + 1] - t[i + 1]) * \
self.basis(t, x, k - 1, i + 1)
else:
b = 0.0
return a + b
def from_dcm(cls, R):
assert R.shape == (3, 3)
b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2])
b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2])
b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2])
b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] + R[2, 2])
q1 = ca.SX(4, 1)
q1[0] = b1
q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1)
q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1)
q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1)
q2 = ca.SX(4, 1)
q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2)
q2[1] = b2
q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2)
q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2)
q3 = ca.SX(4, 1)
q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3)
q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3)
q3[2] = b3
q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3)
q4 = ca.SX(4, 1)
q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4)
q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4)
q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4)
q4[3] = b4
q = ca.if_else(
ca.trace(R) > 0, q1,
ca.if_else(ca.logic_and(R[0, 0] > R[1, 1], R[0, 0] > R[2, 2]), q2,
ca.if_else(R[1, 1] > R[2, 2], q3, q4)))
return q
def edge(self, c):
"""rising edge"""
return ca.logic_and(c, ca.logic_not(self.pre_cond(c)))
def ode_rhs(self):
"""Muscle Model ODE rhs.
Returns
----------
ode_rhs: list<cas.SX>
description
"""
#: Bandpass l_ce
#b, a = signal.butter(2, 50, 'low', analog=True)
#l_ce_filt = signal.lfilter(b, a, self._l_ce.sym)
l_ce_tol = cas.fmax(self._l_ce.sym, 0.0)
_stim = cas.fmax(0.01, cas.fmin(self._stim.sym, 1.))
#: Algrebaic Equation
l_mtc = self._l_slack.val + self._l_opt.val + self._delta_length.sym
l_se = l_mtc - l_ce_tol
#: Muscle Acitvation Dynamics
self._dA.sym = (
_stim - self._activation.sym)/GeyerMuscle.tau_act
#: Muscle Dynamics
#: Series Force
_f_se = (self._f_max.val * (
(l_se - self._l_slack.val) / (
self._l_slack.val * self.e_ref))**2) * (
l_se > self._l_slack.val)
#: Muscle Belly Force
_f_be_cond = self._l_opt.val * (1.0 - self.w)
_f_be = (
(self._f_max.val * (
(l_ce_tol - self._l_opt.val * (1.0 - self.w)) / (
self._l_opt.val * self.w / 2.0))**2)) * (
l_ce_tol <= _f_be_cond)
#: Force-Length Relationship
val = cas.fabs(
(l_ce_tol - self._l_opt.val) / (self._l_opt.val * self.w))
exposant = GeyerMuscle.c * val**3
_f_l = cas.exp(exposant)
#: Force Parallel Element
_f_pe_star = (self._f_max.val * (
(l_ce_tol - self._l_opt.val) / (self._l_opt.val * self.w))**2)*(
l_ce_tol > self._l_opt.val)
#: Force Velocity Inverse Relation
_f_v_eq = ((
self._f_max.val * self._activation.sym * _f_l) + _f_pe_star)
f_v_cond = cas.logic_and(
_f_v_eq < self.tol, _f_v_eq > -self.tol)
_f_v = cas.if_else(f_v_cond, 0.0, (_f_se + _f_be) / ((
self._f_max.val * self._activation.sym * _f_l) + _f_pe_star))
f_v = cas.fmax(0.0, cas.fmin(_f_v, 1.5))
self._v_ce.sym = cas.if_else(
f_v < 1.0, self._v_max.sym * self._l_opt.val * (
1.0 - f_v) / (1.0 + f_v * GeyerMuscle.K),
self._v_max.sym*self._l_opt.val * (f_v - 1.0) / (
7.56 * GeyerMuscle.K *
(f_v - GeyerMuscle.N) + 1.0 - GeyerMuscle.N
))
#: Active, Passive, Tendon Force Computation
_f_v_ce = cas.if_else(
self._v_ce.sym < 0.,
(self._v_max.sym*self._l_opt.val - self._v_ce.sym) /
(self._v_max.sym*self._l_opt.val + GeyerMuscle.K * self._v_ce.sym),
GeyerMuscle.N + (GeyerMuscle.N - 1) * (
self._v_max.sym*self._l_opt.val + self._v_ce.sym
) / (
7.56 * GeyerMuscle.K * self._v_ce.sym - self._v_max.sym*self._l_opt.val
))
self._a_force = self._activation.sym * _f_v_ce * _f_l * self._f_max.val
self._p_force = _f_pe_star*_f_v - _f_be
self._t_force = _f_se
self._alg_tendon_force.sym = self._z_tendon_force.sym - self._t_force
self._alg_active_force.sym = self._z_active_force.sym - self._a_force
self._alg_passive_force.sym = self._z_passive_force.sym - self._p_force
self._alg_v_ce.sym = self._z_v_ce.sym - self._v_ce.sym
self._alg_l_mtc.sym = self._z_l_mtc.sym - l_mtc
self._alg_dact.sym = self._z_dact.sym - self._dA.sym
return True
def edge(self, c):
"""rising edge"""
return ca.logic_and(c, ca.logic_not(self.pre_cond(c)))