def __init__(self,
name,
c0,
dc0,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=1000,
verb=0,
regularization=1e-5,
solver='qpoases'):
''' Constructor
@param c0 Initial CoM position
@param dc0 Initial CoM velocity
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
assert mass > 0.0, "Mass is not positive"
assert mu > 0.0, "Friction coefficient is not positive"
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(
dc0).squeeze().shape[0] == 3, "Com velocity vector has not size 3"
assert np.asarray(
contact_points).shape[1] == 3, "Contact points have not size 3"
assert np.asarray(
contact_normals).shape[1] == 3, "Contact normals have not size 3"
assert np.asarray(contact_points).shape[0] == np.asarray(
contact_normals
).shape[
0], "Number of contact points do not match number of contact normals"
self._name = name
self._maxIter = maxIter
self._verb = verb
self._c0 = np.asarray(c0).squeeze().copy()
self._dc0 = np.asarray(dc0).squeeze().copy()
self._mass = mass
self._g = np.asarray(g).squeeze().copy()
# self._regularization = regularization;
self._mu = mu
self._contact_points = np.asarray(contact_points).copy()
self._contact_normals = np.asarray(contact_normals).copy()
if (norm(self._dc0) != 0.0):
self._v = self._dc0 / norm(self._dc0)
else:
self._v = np.array([1.0, 0.0, 0.0])
self._com_acc_solver = ComAccLP(self._name + "_comAccLP", self._c0,
self._v, self._contact_points,
self._contact_normals, self._mu,
self._g, self._mass, maxIter, verb - 1,
regularization, solver)
self._equilibrium_solver = RobustEquilibriumDLP(name + "_robEquiDLP",
self._contact_points,
self._contact_normals,
self._mu,
self._g,
self._mass,
verb=verb - 1)
def set_contacts(self, contact_points, contact_normals, mu):
self._com_acc_solver.set_contacts(contact_points, contact_normals, mu)
self._equilibrium_solver = RobustEquilibriumDLP(self._name,
contact_points,
contact_normals,
mu,
self._g,
self._mass,
verb=self._verb)
class StabilityCriterion(object):
_name = ""
_maxIter = 0
_verb = 0
_com_acc_solver = None
_equilibrium_solver = None
_c0 = None
_dc0 = None
_v = None
_computationTime = 0.0
_outerIterations = 0
_innerIterations = 0
def __init__(self,
name,
c0,
dc0,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=1000,
verb=0,
regularization=1e-5,
solver='qpoases'):
''' Constructor
@param c0 Initial CoM position
@param dc0 Initial CoM velocity
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
assert mass > 0.0, "Mass is not positive"
assert mu > 0.0, "Friction coefficient is not positive"
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(
dc0).squeeze().shape[0] == 3, "Com velocity vector has not size 3"
assert np.asarray(
contact_points).shape[1] == 3, "Contact points have not size 3"
assert np.asarray(
contact_normals).shape[1] == 3, "Contact normals have not size 3"
assert np.asarray(contact_points).shape[0] == np.asarray(
contact_normals
).shape[
0], "Number of contact points do not match number of contact normals"
self._name = name
self._maxIter = maxIter
self._verb = verb
self._c0 = np.asarray(c0).squeeze().copy()
self._dc0 = np.asarray(dc0).squeeze().copy()
self._mass = mass
self._g = np.asarray(g).squeeze().copy()
# self._regularization = regularization;
self._mu = mu
self._contact_points = np.asarray(contact_points).copy()
self._contact_normals = np.asarray(contact_normals).copy()
if (norm(self._dc0) != 0.0):
self._v = self._dc0 / norm(self._dc0)
else:
self._v = np.array([1.0, 0.0, 0.0])
self._com_acc_solver = ComAccLP(self._name + "_comAccLP", self._c0,
self._v, self._contact_points,
self._contact_normals, self._mu,
self._g, self._mass, maxIter, verb - 1,
regularization, solver)
self._equilibrium_solver = RobustEquilibriumDLP(name + "_robEquiDLP",
self._contact_points,
self._contact_normals,
self._mu,
self._g,
self._mass,
verb=verb - 1)
def set_contacts(self, contact_points, contact_normals, mu):
self._com_acc_solver.set_contacts(contact_points, contact_normals, mu)
self._equilibrium_solver = RobustEquilibriumDLP(self._name,
contact_points,
contact_normals,
mu,
self._g,
self._mass,
verb=self._verb)
def can_I_stop(self, c0=None, dc0=None, T_0=0.5, MAX_ITER=1000):
''' Determine whether the system can come to a stop without changing contacts.
Keyword arguments:
c0 -- initial CoM position
dc0 -- initial CoM velocity
contact points -- a matrix containing the contact points
contact normals -- a matrix containing the contact normals
mu -- friction coefficient (either a scalar or an array)
mass -- the robot mass
T_0 -- an initial guess for the time to stop
Output: An object containing the following member variables:
is_stable -- boolean value
c -- final com position
dc -- final com velocity
ddc_min -- minimum feasible com acceleration at the final com position (to be used as a stability margin)
t -- time taken to reach the final com state
computation_time -- time taken to solve all the LPs
'''
#- Initialize: alpha=0, Dalpha=||dc0||
#- LOOP:
# - Find min com acc for current com pos (i.e. DDalpha_min)
# - If DDalpha_min>0: return False
# - Find alpha_max (i.e. value of alpha corresponding to right vertex of active inequality)
# - Initialize: t=T_0, t_ub=10, t_lb=0
# LOOP:
# - Integrate LDS until t: DDalpha = d/b - (a/d)*alpha
# - if(Dalpha(t)==0 && alpha(t)<=alpha_max): return (True, alpha(t)*v)
# - if(alpha(t)==alpha_max && Dalpha(t)>0): alpha=alpha(t), Dalpha=Dalpha(t), break
# - if(alpha(t)<alpha_max && Dalpha>0): t_lb=t, t=(t_ub+t_lb)/2
# - else t_ub=t, t=(t_ub+t_lb)/2
assert T_0 > 0.0, "Time is not positive"
if (c0 is not None):
assert np.asarray(c0).squeeze().shape[0] == 3, "CoM has not size 3"
self._c0 = np.asarray(c0).squeeze().copy()
if (dc0 is not None):
assert np.asarray(
dc0).squeeze().shape[0] == 3, "CoM velocity has not size 3"
self._dc0 = np.asarray(dc0).squeeze().copy()
if (norm(self._dc0) != 0.0):
self._v = self._dc0 / norm(self._dc0)
if ((c0 is not None) or (dc0 is not None)):
self._com_acc_solver.set_com_state(self._c0, self._v)
# Initialize: alpha=0, Dalpha=||dc0||
t_int = 0.0
alpha = 0.0
Dalpha = norm(self._dc0)
last_iteration = False
self._computationTime = 0.0
self._outerIterations = 0
self._innerIterations = 0
last_ddc_min = None
if (Dalpha == 0.0):
r = self._equilibrium_solver.compute_equilibrium_robustness(
self._c0)
return Bunch(is_stable=(r >= 0.0),
c=self._c0,
dc=self._dc0,
t=0.0,
computation_time=0.0,
ddc_min=None)
for iiii in range(MAX_ITER):
if (last_iteration):
if (self._verb > 0):
print "[%s] Algorithm converged to Dalpha=%.3f" % (
self._name, Dalpha)
return Bunch(is_stable=False,
c=self._c0 + alpha * self._v,
dc=Dalpha * self._v,
t=t_int,
computation_time=self._computationTime,
ddc_min=0.0)
(imode, DDalpha_min, a, alpha_min,
alpha_max) = self._com_acc_solver.compute_max_deceleration(alpha)
self._computationTime += self._com_acc_solver.getLpTime()
self._outerIterations += 1
if (imode == LP_status.INFEASIBLE):
# Linear Program was not feasible, suggesting there is no feasible CoM acceleration for the given CoM position
if (self._verb > 1):
from com_acc_LP import ComAccPP
self._comAccPP = None
try:
self._comAccPP = ComAccPP(self._name + "_comAccPP",
self._c0,
self._v,
self._contact_points,
self._contact_normals,
self._mu,
self._g,
self._mass,
verb=self._verb - 1)
(imode2, DDalpha_min2, a2, alpha_max2
) = self._comAccPP.compute_max_deceleration(alpha)
except Exception as e:
print "[%s] Error while trying to compute min com acc with ComAccPP. " % (
self._name) + str(e)
if (self._comAccPP is not None and imode2 == 0):
raise ValueError(
"[%s] Computing min com acc with PP gave different result than with LP: "
% (self._name) + str(DDalpha_min2))
return Bunch(is_stable=False,
c=self._c0 + alpha * self._v,
dc=Dalpha * self._v,
t=t_int,
computation_time=self._computationTime,
ddc_min=None)
if (imode != LP_status.OPTIMAL):
raise ValueError("[%s] Error while solving com acc LP: %s" %
(self._name, LP_status_string[imode]))
last_ddc_min = DDalpha_min
if (self._verb > 0):
print "[%s] DDalpha_min=%.3f, alpha=%.3f, Dalpha=%.3f, alpha_max=%.3f, a=%.3f" % (
self._name, DDalpha_min, alpha, Dalpha, alpha_max, a)
if (DDalpha_min > -EPS):
if (self._verb > 0):
print "[%s] Minimum com acceleration is positive, so I cannot stop" % (
self._name)
return Bunch(is_stable=False,
c=self._c0 + alpha * self._v,
dc=Dalpha * self._v,
t=t_int,
computation_time=self._computationTime,
ddc_min=DDalpha_min)
# DDalpha_min is the acceleration corresponding to the current value of alpha
# compute DDalpha_0, that is the acceleration corresponding to alpha=0
DDalpha_0 = DDalpha_min - a * alpha
# If DDalpha_min is not always negative on the current segment then update
# alpha_max to the point where DDalpha_min becomes zero
if (DDalpha_0 + a * alpha_max > 0.0):
alpha_max = -DDalpha_0 / a
last_iteration = True
if (self._verb > 0):
print "[%s] Updated alpha_max %.3f" % (self._name,
alpha_max)
# Initialize: t=T_0, t_ub=10, t_lb=0
t = T_0
t_ub = 10.0
# hypothesis: 10 seconds are always sufficient to stop
t_lb = 0.0
if (abs(a) < EPS):
# if the acceleration is constant over time I can compute directly the time needed
# to bring the velocity to zero:
# Dalpha(t) = Dalpha(0) + t*DDalpha = 0
# t = -Dalpha(0)/DDalpha
t_zero = -Dalpha / DDalpha_min
alpha_t = alpha + t_zero * Dalpha + 0.5 * t_zero * t_zero * DDalpha_min
if (alpha_t <= alpha_max + EPS):
if (self._verb > 0):
print "DDalpha_min is independent from alpha, algorithm converged to Dalpha=0"
return Bunch(is_stable=True,
c=self._c0 + alpha_t * self._v,
dc=0.0 * self._v,
t=t_int + t_zero,
computation_time=self._computationTime,
ddc_min=DDalpha_min)
# if alpha reaches alpha_max before the velocity is zero, then compute the time needed to reach alpha_max
# alpha(t) = alpha(0) + t*Dalpha(0) + 0.5*t*t*DDalpha = alpha_max
# t = (- Dalpha(0) +/- sqrt(Dalpha(0)^2 - 2*DDalpha(alpha(0)-alpha_max))) / DDalpha;
# where DDalpha = -d[i_DDalpha_min]
# Having two solutions, we take the smallest one because we want to find the first time
# at which alpha reaches alpha_max
delta = sqrt(Dalpha**2 - 2 * DDalpha_min * (alpha - alpha_max))
t = (-Dalpha + delta) / DDalpha_min
if (t < 0.0):
# If the smallest time at which alpha reaches alpha_max is negative print a WARNING because this should not happen
print "[%s] WARNING: Time is negative: t=%.3f, alpha=%.3f, Dalpha=%.3f, DDalpha_min=%.3f, alpha_max=%.3f" % (
self._name, t, alpha, Dalpha, DDalpha_min, alpha_max)
t = (-Dalpha - delta) / DDalpha_min
if (t < 0.0):
# If also the largest time is negative print an ERROR and return
raise ValueError(
"[%s] ERROR: Time is still negative: t=%.3f, alpha=%.3f, Dalpha=%.3f, DDalpha_min=%.3f, alpha_max=%.3f"
% (self._name, t, alpha, Dalpha, DDalpha_min,
alpha_max))
else:
# Try to compute the time at which the velocity will be zero:
# Dalpha(t) = omega*sh*alpha + ch*Dalpha + omega*sh*(DDalpha_0/a) = 0
# omega*th*alpha + Dalpha + omega*th*(DDalpha_0/a) = 0
# th*omega*(alpha + DDalpha_0/a) = - Dalpha
# tanh(omega*t) = - Dalpha / (omega*(alpha + DDalpha_0/a))
# omega*t = atanh(- Dalpha / (omega*(alpha + DDalpha_0/a)))
# t = atanh(- Dalpha / (omega*(alpha + DDalpha_0/a))) / omega
omega = sqrt(a + 0j)
tmp = -Dalpha / (omega * (alpha + (DDalpha_0 / a)))
if (abs(np.real(tmp)) < 1.0):
t = np.arctanh(tmp) / omega
if (np.imag(t) != 0.0):
raise ValueError(
"[%s] ERROR time to reach zero vel is imaginary: "
% self._name + str(t) + "a=%.3f" % (a))
t = np.real(t)
t_ub = t
# use this time as an upper bound for the search
else:
# Here I already know that I won't be able to stop, but I keep integrating to find the min velocity I can reach.
if (self._verb > 0):
print "[%s] INFO argument of arctanh greater than 1 (%.3f), meaning that zero vel will never be reached" % (
self._name, np.real(tmp))
#return Bunch(is_stable=False, c=self._c0+alpha*self._v, dc=Dalpha*self._v, t=t_int);
bisection_converged = False
for jjjj in range(MAX_ITER):
# Integrate LDS until t: DDalpha = a*alpha - d
if (abs(a) > EPS):
# if a!=0 then the acceleration is a linear function of the position and I need to use this formula to integrate
sh = np.sinh(omega * t)
ch = np.cosh(omega * t)
alpha_t = ch * alpha + sh * Dalpha / omega - (1.0 - ch) * (
DDalpha_0 / a)
Dalpha_t = omega * sh * alpha + ch * Dalpha + omega * sh * (
DDalpha_0 / a)
else:
# if a==0 then the acceleration is constant and I need to use this formula to integrate
alpha_t = alpha + t * Dalpha + 0.5 * t * t * DDalpha_0
Dalpha_t = Dalpha + t * DDalpha_0
if (np.imag(alpha_t) != 0.0):
raise ValueError("ERROR alpha is imaginary: " +
str(alpha_t))
if (np.imag(Dalpha_t) != 0.0):
raise ValueError("ERROR Dalpha is imaginary: " +
str(Dalpha_t))
alpha_t = np.real(alpha_t)
Dalpha_t = np.real(Dalpha_t)
if (self._verb > 1):
print "[%s] Bisection iter" % (
self._name
), jjjj, "alpha", alpha_t, "Dalpha", Dalpha_t, "t", t
if (abs(Dalpha_t) < EPS and alpha_t <= alpha_max + EPS):
if (self._verb > 0):
print "[%s] Algorithm converged to Dalpha=0" % self._name
self._innerIterations += jjjj
t_int += t
return Bunch(is_stable=True,
c=self._c0 + alpha_t * self._v,
dc=Dalpha_t * self._v,
t=t_int,
computation_time=self._computationTime,
ddc_min=DDalpha_0 + a * alpha_t)
if (abs(alpha_t - alpha_max) < EPS and Dalpha_t > 0):
alpha = alpha_max + EPS
Dalpha = Dalpha_t
t_int += t
bisection_converged = True
self._innerIterations += jjjj
break
if (alpha_t < alpha_max and Dalpha_t > 0):
t_lb = t
t = 0.5 * (t_ub + t_lb)
else:
t_ub = t
t = 0.5 * (t_ub + t_lb)
if (not bisection_converged):
raise ValueError(
"[%s] Bisection search did not converge in %d iterations" %
(self._name, MAX_ITER))
raise ValueError("[%s] Algorithm did not converge in %d iterations" %
(self._name, MAX_ITER))
def predict_future_state(self, t_pred, c0=None, dc0=None, MAX_ITER=1000):
''' Compute what the CoM state will be at the specified time instant if the system
applies maximum CoM deceleration parallel to the current CoM velocity
Keyword arguments:
t_pred -- Future time at which the prediction is made
c0 -- initial CoM position
dc0 -- initial CoM velocity
Output: An object with the following member variables:
t -- time at which the integration has stopped (equal to t_pred, unless something went wrong)
c -- final com position
dc -- final com velocity
'''
#- Initialize: alpha=0, Dalpha=||dc0||
#- LOOP:
# - Find min com acc for current com pos (i.e. DDalpha_min)
# - If DDalpha_min>0: return False
# - Find alpha_max (i.e. value of alpha corresponding to right vertex of active inequality)
# - Initialize: t=T_0, t_ub=10, t_lb=0
# LOOP:
# - Integrate LDS until t: DDalpha = d/b - (a/d)*alpha
# - if(Dalpha(t)==0 && alpha(t)<=alpha_max): return (True, alpha(t)*v)
# - if(alpha(t)==alpha_max && Dalpha(t)>0): alpha=alpha(t), Dalpha=Dalpha(t), break
# - if(alpha(t)<alpha_max && Dalpha>0): t_lb=t, t=(t_ub+t_lb)/2
# - else t_ub=t, t=(t_ub+t_lb)/2
assert t_pred > 0.0, "Prediction time is not positive"
if (c0 is not None):
assert np.asarray(
c0).squeeze().shape[0] == 3, "CoM position has not size 3"
self._c0 = np.asarray(c0).squeeze().copy()
if (dc0 is not None):
assert np.asarray(
dc0).squeeze().shape[0] == 3, "CoM velocity has not size 3"
self._dc0 = np.asarray(dc0).squeeze().copy()
if (norm(self._dc0) != 0.0):
self._v = self._dc0 / norm(self._dc0)
if ((c0 is not None) or (dc0 is not None)):
self._com_acc_solver.set_com_state(self._c0, self._dc0)
# Initialize: alpha=0, Dalpha=||dc0||
alpha = 0.0
Dalpha = norm(self._dc0)
Dalpha_min = Dalpha
# minimum velocity found while integrating
self._computationTime = 0.0
self._outerIterations = 0
self._innerIterations = 0
t_int = 0.0
# current time
t_min = 0.0
# time corresponding to minimum velocity
min_vel_reached = False
if (Dalpha == 0.0):
r = self._equilibrium_solver.compute_equilibrium_robustness(
self._c0)
if (r >= 0.0):
return Bunch(t=t_pred,
c=self._c0,
dc=self._dc0,
t_min=0.0,
dc_min=self._dc0)
raise ValueError(
"[%s] NOT IMPLEMENTED YET: initial velocity is zero but system is not in static equilibrium!"
% (self._name))
for iiii in range(MAX_ITER):
(imode, DDalpha_min, a, alpha_min,
alpha_max) = self._com_acc_solver.compute_max_deceleration(alpha)
self._computationTime += self._com_acc_solver.getLpTime()
self._outerIterations += 1
if (imode != 0):
# Linear Program was not feasible, suggesting there is no feasible CoM acceleration for the given CoM position
if (min_vel_reached):
return Bunch(t=t_int,
c=self._c0 + alpha * self._v,
dc=Dalpha * self._v,
t_min=t_min,
dc_min=Dalpha_min * self._v)
return Bunch(t=t_int,
c=self._c0 + alpha * self._v,
dc=Dalpha * self._v,
t_min=t_int,
dc_min=Dalpha * self._v)
if (self._verb > 0):
print "[%s] t=%.3f, DDalpha_min=%.3f, alpha=%.3f, Dalpha=%.3f, alpha_max=%.3f, a=%.3f" % (
self._name, t_int, DDalpha_min, alpha, Dalpha, alpha_max,
a)
# DDalpha_min is the acceleration corresponding to the current value of alpha
# compute DDalpha_0, that is the acceleration corresponding to alpha=0
DDalpha_0 = DDalpha_min - a * alpha
if (not min_vel_reached):
if (DDalpha_min >= 0.0):
min_vel_reached = True
Dalpha_min = Dalpha
t_min = t_int
elif (DDalpha_0 + a * alpha_max > 0.0):
# If we have not found yet the point of minimum velocity,
# if DDalpha_min is not always negative on the current segment then update
# alpha_max to the point where DDalpha_min becomes zero
alpha_max = -DDalpha_0 / a
if (self._verb > 0):
print "[%s] Updated alpha_max %.3f" % (self._name,
alpha_max)
# Initialize initial guess and bounds on integration time
t_ub = t_pred - t_int
t_lb = 0.0
t = t_ub
if (abs(a) < EPS):
# if the acceleration is constant over time I can compute directly the time needed
# to bring the velocity to zero:
# Dalpha(t) = Dalpha(0) + t*DDalpha = 0
# t = -Dalpha(0)/DDalpha
t = -Dalpha / DDalpha_min
alpha_t = alpha + t * Dalpha + 0.5 * t * t * DDalpha_min
if (alpha_t <= alpha_max + EPS):
if (self._verb > 0):
print "DDalpha_min is independent from alpha, algorithm converged to Dalpha=0"
# hypothesis: after I reach Dalpha=0 I can maintain it (i.e. DDalpha=0 is feasible)
return Bunch(t=t_pred,
c=self._c0 + alpha_t * self._v,
dc=0.0 * self._v,
t_min=t_int + t,
dc_min=0 * self._v)
# if alpha reaches alpha_max before the velocity is zero, then compute the time needed to reach alpha_max
# alpha(t) = alpha(0) + t*Dalpha(0) + 0.5*t*t*DDalpha = alpha_max
# t = (- Dalpha(0) +/- sqrt(Dalpha(0)^2 - 2*DDalpha(alpha(0)-alpha_max))) / DDalpha;
# where DDalpha = -d[i_DDalpha_min]
# Having two solutions, we take the smallest one because we want to find the first time
# at which alpha reaches alpha_max
delta = sqrt(Dalpha**2 - 2 * DDalpha_min * (alpha - alpha_max))
t = (-Dalpha + delta) / DDalpha_min
if (t < 0.0):
raise ValueError(
"[%s] ERROR: Time to reach alpha_max with constant acceleration is negative: t=%.3f, alpha=%.3f, Dalpha=%.3f, DDalpha_min=%.3f, alpha_max=%.3f"
%
(self._name, t, alpha, Dalpha, DDalpha_min, alpha_max))
# ensure we do not overpass the specified t_pred
t = min([t, t_ub])
# integrate until either of these conditions is true:
# - you reach t=t_pred (while not passing over alpha_max and maintaining Dalpha>=0)
# - you reach alpha_max (while not passing over t_pred and maintaining Dalpha>=0)
# - you reach Dalpha=0 (while not passing over alpha_max and t_pred)
bisection_converged = False
for jjjj in range(MAX_ITER):
# Integrate LDS until t: DDalpha = a*alpha - d
if (abs(a) > EPS):
# if a!=0 then the acceleration is a linear function of the position and I need to use this formula to integrate
omega = sqrt(a + 0j)
sh = np.sinh(omega * t)
ch = np.cosh(omega * t)
alpha_t = ch * alpha + sh * Dalpha / omega - (1.0 - ch) * (
DDalpha_0 / a)
Dalpha_t = omega * sh * alpha + ch * Dalpha + omega * sh * (
DDalpha_0 / a)
else:
# if a==0 then the acceleration is constant and I need to use this formula to integrate
alpha_t = alpha + t * Dalpha + 0.5 * t * t * DDalpha_0
Dalpha_t = Dalpha + t * DDalpha_0
if (np.imag(alpha_t) != 0.0):
raise ValueError("ERROR alpha is imaginary: " +
str(alpha_t))
if (np.imag(Dalpha_t) != 0.0):
raise ValueError("ERROR Dalpha is imaginary: " +
str(Dalpha_t))
alpha_t = np.real(alpha_t)
Dalpha_t = np.real(Dalpha_t)
if (self._verb > 1):
print "[%s] Bisection iter" % (
self._name
), jjjj, "alpha", alpha_t, "Dalpha", Dalpha_t, "t", t
if (alpha_t <= alpha_max + EPS and
(abs(Dalpha_t) < EPS or
(Dalpha_t > -EPS and abs(t + t_int - t_pred) < EPS))):
# if I did not pass over alpha_max and velocity is zero OR
# if I did not pass over alpha_max and velocity is positive and I reached t_pred
if (self._verb > 0):
print "[%s] Algorithm converged to Dalpha=%.3f" % (
self._name, Dalpha_t)
self._innerIterations += jjjj
if (min_vel_reached):
return Bunch(t=t_pred,
c=self._c0 + alpha_t * self._v,
dc=Dalpha_t * self._v,
t_min=t_min,
dc_min=Dalpha_min * self._v)
return Bunch(t=t_pred,
c=self._c0 + alpha_t * self._v,
dc=Dalpha_t * self._v,
t_min=t_pred,
dc_min=Dalpha_t * self._v)
if (abs(alpha_t - alpha_max) < EPS and Dalpha_t > 0):
alpha = alpha_max + EPS
Dalpha = Dalpha_t
t_int += t
bisection_converged = True
self._innerIterations += jjjj
break
if (alpha_t < alpha_max and Dalpha_t > 0):
t_lb = t
t = 0.5 * (t_ub + t_lb)
else:
t_ub = t
t = 0.5 * (t_ub + t_lb)
if (not bisection_converged):
raise ValueError(
"[%s] Bisection search did not converge in %d iterations" %
(self._name, MAX_ITER))
raise ValueError("[%s] Algorithm did not converge in %d iterations" %
(self._name, MAX_ITER))