def armijo_search(self, X, U, Kproj, dX, dU):
"""
Perform an Armijo line search from the trajectory X,U along
the tangent trajectory dX, dU. Returns the tuple (nX, nU,
nCost).
"""
cost0 = self.calc_cost(X, U)
dcost0 = self.calc_dcost(X, U, dX, dU)
for m in range(0, self.armijo_max_iterations):
lam = self.armijo_beta**m
max_cost = cost0 + self.armijo_alpha * lam * dcost0
bX = X + lam * dX
bU = U + lam * dU
(result, nX, nU) = self.armijo_simulate(bX, bU, Kproj)
if not result:
self.monitor.armijo_simulation_failure(m, nX, nU, nX, bU)
continue
cost1 = self.calc_cost(nX, nU)
self.monitor.armijo_evaluation(m, nX, nU, bX, bU, cost1, max_cost)
if cost1 < max_cost:
return self.armijo_search_return(nX, nU, cost1)
else:
self.monitor.armijo_search_failure(X, U, dX, dU, cost0, dcost0,
Kproj)
raise trep.ConvergenceError("Armijo Failed to Converge")
def step(self,
t2,
u1=tuple(),
k2=tuple(),
max_iterations=200,
q2_hint=None,
lambda1_hint=None):
"""
Step the integrator forward to time t2 . This solves the DEL
equation. The result will be available in gmvi.t2, gmvi.q2,
gmvi.p2.
"""
u1 = np.array(u1)
k2 = np.array(k2)
assert u1.shape == (self.system.nu, )
assert k2.shape == (self.nk, )
# Advance the integrator
self.q1 = self.q2
self.p1 = self.p2
self.u1 = u1
self._q2[self.nd:] = k2
self.t1 = self.t2
self.t2 = t2
if q2_hint is not None:
self._q2[:self.nd] = q2_hint[:self.nd]
if lambda1_hint is not None:
self.lambda1 = lambda1_hint
try:
return self._solve_DEL(max_iterations)
except ValueError: # Catch singular derivatives.
raise trep.ConvergenceError("Singular derivative of DEL at t=%s" %
t2)
def continuous_plastic_impact(sys, surfaces, q1, dq1):
"""
Fully continuous impact update. q1 = q(t-), dq1 = qdot(t-)
"""
p = len(surfaces)
nd = sys.nQd
sys.q = q1
sys.dq = dq1
p1 = np.array([sys.L_ddq(q1) for q1 in sys.dyn_configs])
Dh = np.reshape(
np.array([[c.h_dq(q) for q in sys.dyn_configs]
for c in sys.constraints]), (sys.nc, sys.nQd))
Dphi = np.array([[surf.phi_dq(q) for q in sys.dyn_configs]
for surf in surfaces])
Dh2 = np.append(Dh, Dphi).reshape(sys.nc + p, nd)
Dh2T = np.transpose(Dh2)
def func(x):
sys.dqd = x[:nd]
lam = x[nd:]
p2 = np.array([sys.L_ddq(q1) for q1 in sys.dyn_configs])
f1 = p2 - p1 - np.dot(Dh2T, lam)
f2 = np.dot(Dh2, sys.dqd)
return np.hstack((f1, f2))
def fprime(x):
sys.dqd = x[:nd]
lam = x[nd:]
Df11 = np.array([[sys.L_ddqddq(q1, q2) for q2 in sys.dyn_configs]
for q1 in sys.dyn_configs])
Df1 = np.hstack((Df11, -Dh2T))
Df2 = np.hstack((Dh2, np.zeros((sys.nc + p, sys.nc + p))))
Df = np.vstack((Df1, Df2))
return Df
dq_guess = dq1[:nd]
x0 = np.hstack((dq_guess, np.zeros(p + sys.nc)))
(x, infodict, ier, mesg) = \
scipy.optimize.fsolve(func, x0, full_output=True, fprime=fprime)
if (ier != 1) and np.linalg.norm(func(x)) > (1e-10):
print mesg
raise trep.ConvergenceError("Continuous plastic impact update failed\
to converge.")
sys.dqd = x[:nd]
lam = x[nd:]
return sys.dqd
def solve_release_time(mvi, con):
""" Solves for the state of the system at the time of release of constraint con.
This is when the continuous-time Lagrange multiplier is zero.
This method modifies lambda1 and state 2.
"""
nd, m = mvi.nd, mvi.nc
# If the first state is already close enough to release, step integrator back
# and use the state as the release state.
if abs(mvi.lambda1c[con.index]) < LAMBDA_TOL:
mvi.set_single_state(2, mvi.t1, mvi.q1, mvi.p1)
mvi.lambda1 = mvi.lambda0
return {'t': mvi.t2, 'q': mvi.q2, 'p': mvi.p2, 'lambda1': mvi.lambda1}
set_system_state(mvi, 1)
Dh1T = np.transpose(Dh(mvi))
D2h1 = D2h(mvi)
t_guess, q_guess = linear_interpolate_release(mvi, con.index)
# No impacts at state 1 - solve normal release time problem.
if not mvi._state1_impacts:
x0 = np.hstack((q_guess[:nd], mvi.lambda0, t_guess))
# The root-solving problem is to find (DEL, h(qr, tr), lambda_{cont}) = 0.
def func(x):
mvi.t2 = x[-1]
mvi.lambda1 = x[nd:-1]
mvi.q2 = np.append(x[:nd], mvi.kin_config(x[-1]))
mvi.set_midpoint()
f1 = mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1)
set_system_state(mvi, 2)
f2 = h(mvi)
f3 = mvi.system.lambda_(constraint=con)
return np.hstack((f1, f2, f3))
def fprime(x):
mvi.t2 = x[-1]
mvi.q2 = np.append(x[:nd], mvi.kin_config(x[-1]))
mvi.lambda1 = x[(nd):(nd + m)]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df13 = D4D1L2(mvi) + D4fm2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df23 = D2h(mvi).reshape((mvi.nc, 1))
lam_dq = (mvi.system.lambda_dq(constraint=con) +
mvi.system.lambda_ddq(constraint=con) /
(mvi.t2 - mvi.t1))[:nd]
lam_dt = -1.0 / (mvi.t2 - mvi.t1) * np.dot(
mvi.system.dq[:nd],
mvi.system.lambda_ddq(constraint=con)[:nd])
Df1 = np.hstack((Df11, -Dh1T, Df13))
Df2 = np.hstack((Df21, np.zeros((mvi.nc, mvi.nc)), Df23))
Df3 = np.hstack((lam_dq, np.zeros(m), lam_dt))
return np.vstack((Df1, Df2, Df3))
# Impacts have occurred at state 1 - solve impact update with additional unknown
# release time.
else:
impact_surfaces = mvi._state1_impacts
p = len(impact_surfaces)
tau = mvi.tau2 # Is set by most recent impact solver.
x0 = np.hstack((q_guess[:nd], mvi.lambda1, t_guess,
np.dot(D2h1, mvi.lambda1) - tau))
def func(x):
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
Ec = x[-1]
mvi.set_midpoint()
f1 = mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1)
f3 = tau + D3L2(mvi) - np.dot(D2h1, mvi.lambda1) + Ec
set_system_state(mvi, 2)
f2 = h(mvi)
f4 = mvi.system.lambda_(constraint=con)
return np.hstack((f1, f2, f3, f4))
def fprime(x):
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
Ec = x[-1]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df13 = D4D1L2(mvi) + D4fm2(mvi)
Df31 = D2D3L2(mvi)
Df33 = D4D3L2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df23 = D2h(mvi).reshape((m, 1))
Df32 = -D2h(mvi)
Df41 = D2LAM2(mvi, con)
Df43 = D4LAM2(mvi, con)
Df1 = np.hstack((Df11, -Dh1T, Df13, np.zeros((nd, 1))))
Df2 = np.hstack((Df21, np.zeros((m, m)), Df23, np.zeros((m, 1))))
Df3 = np.hstack((Df31, Df32, Df33, (1.0, )))
Df4 = np.hstack((Df41, np.zeros(m), Df43, np.zeros(1)))
return np.vstack((Df1, Df2, Df3, Df4))
(x, infodict, ier, mesg) = scipy.optimize.fsolve(func,
x0,
full_output=True)
if ier != 1:
print mesg
raise trep.ConvergenceError("Release solver failed to converge.")
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
mvi.set_midpoint()
mvi.p2 = D2L2(mvi)
return {'t': mvi.t2, 'q': mvi.q2, 'p': mvi.p2, 'lambda1': mvi.lambda1}
def solve_impact_time(mvi, surface):
"""
Finds the time and configuration at impact across one surface.
When passed to this function, mvi should have following states:
1 - state just before impact
2 - invalid configuration to be corrected
This method modifies only state 2 and lambda1.
"""
mvi._surface = surface
nd, m = mvi.nd, mvi.nc
set_system_state(mvi, 1)
Dh1T = np.transpose(Dh(mvi))
D2h1 = D2h(mvi)
# Here, check if the impact occurs exactly at t1. If
# | phi | < tolerance, set the impact state at 1. The root-solver
# can't find this because it will cause a divide by zero error
# (t2-t1 = 0).
if abs(phi(mvi)) < mvi._surface.tolerance:
mvi._s2 = (mvi.t1, mvi.q1, mvi.p1)
mvi.lambda1 = mvi.lambda0
return {'t': mvi.t2, 'q': mvi.q2, 'p': mvi.p2, 'lambda1': mvi.lambda1}
t_guess, q_guess = linear_interpolate_impact(mvi)
# First, if no impacts have occurred at state 1, we use the normal
# VI equations plus the condition phi(qi) = 0.
# f = [DEL(qi, ti), h(qi), phi(qi)].
if not mvi._state1_impacts:
x0 = np.hstack((q_guess[:nd], mvi.lambda1, t_guess))
def func(x):
mvi.t2 = x[-1]
mvi.q2 = np.append(x[:nd], mvi.kin_config(x[-1]))
mvi.lambda1 = x[nd:(nd + m)]
mvi.set_midpoint()
f1 = mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1)
set_system_state(mvi, 2)
f2 = h(mvi)
f3 = phi(mvi)
return np.hstack((f1, f2, f3))
def fprime(x):
mvi.t2 = x[-1]
mvi.q2 = np.append(x[:nd], mvi.kin_config(x[-1]))
mvi.lambda1 = x[(nd):(nd + m)]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df13 = D4D1L2(mvi) + D4fm2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df31 = Dphi(mvi)
Df23 = D2h(mvi).reshape((mvi.nc, 1))
Df1 = np.hstack((Df11, -Dh1T, Df13))
Df2 = np.hstack((Df21, np.zeros((m, m)), Df23))
Df3 = np.hstack((Df31, np.zeros(m + 1)))
return np.vstack((Df1, Df2, Df3))
# Otherwise, we need to use the modified impact time solver that
# also applies the plastic impact map. Keep in mind that here the
# impact surfaces have already been added to the system, i.e., they
# are included in h. f = [DEL(qi, ti), h(qi), IMPACT_MAP(qi, ti)]
else:
impact_surfaces = mvi._state1_impacts
p = len(impact_surfaces)
tau = mvi._tau1
x0 = np.hstack((q_guess[:nd], mvi.lambda1, t_guess,
np.dot(D2h1, mvi.lambda1) - tau))
def func(x):
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
Ec = x[-1]
mvi.set_midpoint()
f1 = mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1)
f3 = tau + D3L2(mvi) - np.dot(D2h1, mvi.lambda1) + Ec
set_system_state(mvi, 2)
f2 = h(mvi)
f4 = phi(mvi)
return np.hstack((f1, f2, f3, f4))
def fprime(x):
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
Ec = x[-1]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df13 = D4D1L2(mvi) + D4fm2(mvi)
Df31 = D2D3L2(mvi)
Df33 = D4D3L2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df23 = D2h(mvi).reshape((m, 1))
Df32 = -D2h(mvi)
Df41 = Dphi(mvi)
Df1 = np.hstack((Df11, -Dh1T, Df13, np.zeros((nd, 1))))
Df2 = np.hstack((Df21, np.zeros((m, m)), Df23, np.zeros((m, 1))))
Df3 = np.hstack((Df31, Df32, Df33, (1.0, )))
Df4 = np.hstack((Df41, np.zeros(m + 2)))
return np.vstack((Df1, Df2, Df3, Df4))
# Call the root-solver to find the time of impact.
try:
(x, infodict, ier, mesg) = \
scipy.optimize.fsolve(func, x0, full_output=True, fprime=fprime)
except ZeroDivisionError:
raise Exception("Divide by zero in impact time solver.")
if ier != 1:
print mesg
raise trep.ConvergenceError("Find impact failed to converge.")
mvi.t2 = x[nd + m]
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
mvi.set_midpoint()
mvi.p2 = D2L2(mvi)
return {'t': mvi.t2, 'q': mvi.q2, 'p': mvi.p2, 'lambda1': mvi.lambda1}
def discrete_plastic_impact(mvi, impact_surfaces=[]):
"""
Finds the configuration after impact.
when passed to this function, mvi should have following states
1 - state just before impact
2 - impact state
plastic_impact updates so that state2 is post-impact, state1 is impact
"""
p = len(impact_surfaces)
nd, m = mvi.nd, mvi.nc
mvi._s2 = (mvi.t2, mvi.q2, mvi.p2)
mvi.lambda0 = mvi.lambda1
tau = mvi._tau1
# It is always possible that an impact happens very close to the
# time step t2. If this is the case, then we need to take a whole new
# step to ensure the impact map is computed. This step is assumed to
# be the same size as the previous one. Not ideal, but definitely
# needed.
if abs(mvi.t2 - mvi.t_end) < TIME_TOL:
mvi.t_end = mvi.t_start + 2 * mvi.current_dt
mvi.t2 = mvi.t_end
set_system_state(mvi, 1)
Dh1T = np.transpose(Dh(mvi))
D2h1 = D2h(mvi)
# If there are impact surfaces, apply normal plastic impact update.
if p != 0:
Dphi1 = Dphi(mvi, surfaces=impact_surfaces)
x0 = np.hstack((mvi.q2[:nd], mvi.lambda1, np.dot(Dphi1, mvi.p1),
np.dot(D2h1, mvi.lambda1) - tau))
def func(x):
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
lambda_c = x[(nd + m):-1]
Ec = x[-1]
set_system_state(mvi, 2)
f2 = h(mvi)
f4 = phi(mvi, surfaces=impact_surfaces)
mvi.set_midpoint()
f1 = (mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1) -
np.dot(np.transpose(Dphi1), lambda_c))
f3 = tau + D3L2(mvi) - np.dot(D2h1, mvi.lambda1) + Ec
return np.hstack((f1, f2, f3, f4))
def fprime(x):
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:nd + m]
lambda_c = x[(nd + m):-1]
Ec = x[-1]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df31 = D2D3L2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df41 = Dphi(mvi, surfaces=impact_surfaces)
Df32 = -D2h(mvi)
Df1 = np.column_stack(
(Df11, -Dh1T, -np.transpose(Dphi1), np.zeros((nd, 1))))
Df2 = np.hstack((Df21, np.zeros((mvi.nc, mvi.nc + p + 1))))
Df3 = np.hstack((Df31, Df32, np.zeros(p), (1.0, )))
Df4 = np.hstack((Df41, np.zeros((p, mvi.nc + p + 1))))
return np.vstack((Df1, Df2, Df3, Df4))
# Because of multiple events, it is possible we need to solve the
# impact update after the constraints have already been added to the
# system. This is the same problem, except lambda_c is a part of lambda
# already.
else:
x0 = np.hstack(
(mvi.q2[:nd], mvi.lambda1, np.dot(D2h1, mvi.lambda1) - tau))
def func(x):
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
Ec = x[-1]
set_system_state(mvi, 2)
f2 = h(mvi)
mvi.set_midpoint()
f1 = mvi.p1 + D1L2(mvi) + fm2(mvi) - np.dot(Dh1T, mvi.lambda1)
f3 = tau + D3L2(mvi) - np.dot(D2h1, mvi.lambda1) + Ec
return np.hstack((f1, f2, f3))
def fprime(x):
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:nd + m]
Ec = x[-1]
mvi.set_midpoint()
Df11 = D2D1L2(mvi) + D2fm2(mvi)
Df31 = D2D3L2(mvi)
set_system_state(mvi, 2)
Df21 = Dh(mvi)
Df32 = -D2h(mvi)
Df1 = np.column_stack((Df11, -Dh1T, np.zeros((nd, 1))))
Df2 = np.hstack((Df21, np.zeros((m, m + 1))))
Df3 = np.hstack((Df31, Df32, (1.0, )))
return np.vstack((Df1, Df2, Df3))
(x, infodict, ier, mesg) = \
scipy.optimize.fsolve(func, x0, full_output=True, fprime=fprime)
if (ier != 1):
if np.linalg.norm(func(x)) < mvi.tolerance:
pass
else:
print mesg
raise trep.ConvergenceError("Discrete plastic impact update failed\
to converge.")
mvi.q2 = np.append(x[:nd], mvi.kin_config(mvi.t2))
mvi.lambda1 = x[nd:(nd + m)]
mvi.set_midpoint()
mvi.p2 = D2L2(mvi)
mvi.tau2 = D4L2(mvi)
return {
't': mvi.t2,
'q': mvi.q2,
'lambda1': mvi.lambda1,
'lambdac': x[(nd + m):-1],
'Ec': x[-1]
}