def integrate(self, theta0):
"""
Integrate curve matching equations along the curve. Note that this
trajectory does not necessarily satisfy the natural boundary
conditions. For this, use `match` instead.
Parameters
----------
theta0 : scalar
Initial value for angle
Returns
-------
g : GroupTrajectory
Trajectory in SE(2) mapping c0 to c1.
"""
theta, omega, v, delta_theta, delta_omega, delta_v = \
integrate(self.c0, self.c1, theta0, self.m)
g = GroupTrajectory(self.num_points, self.h, self.m)
g.theta = theta
g.x = self.compute_displacements(theta)
g.omega = omega
g.v = v
g.delta_omega = delta_omega
g.delta_v = delta_v
return g
def solve_bvp(c0, c1, m, theta_guess, full_output=False, tol=1e-8, n_iter=20):
delta = -1
n = 0
theta_start = theta_guess
while (n < n_iter) and (abs(delta) > tol):
# ipdb.set_trace()
# Integrate forward in time
theta, omega, v, delta_theta, delta_omega, delta_v = integrate(c0, c1, theta_guess, m)
# Calculate adjustment for angle as the fraction numer/denom
numer = (
m * (1 + omega[-1] ** 2 / 4) * omega[-1]
+ omega[-1] / 4 * np.dot(v[-1, :], v[-1, :])
+ np.dot(v[-1, :], np.dot(J, c0[-1, :]))
- omega[-1] / 2 * np.dot(v[-1, :], c0[-1, :])
)
c, d = coefficients_first_variation(omega[-1], v[-1, :], c0[-1, :], m)
denom = c * delta_omega[-1] + np.dot(d, delta_v[-1, :])
if abs(denom) < 1e-7 and abs(numer) < 1e-7:
# For now, report error and finish up prematurely
# TODO investigate when this happens, and apply l'Hospital
print "Warning: ill-behaved update quotient in shooting method. " "Initial value for theta_guess: %d." % theta_start
return -1
# Update initial guess
n += 1
delta = numer / denom
theta_guess -= delta
theta_guess %= 2 * np.pi
out = theta, omega, v, delta_theta, delta_omega, delta_v
if full_output:
res = right_boundary_condition(theta[-1], omega[-1], v[-1, :], c0[-1, :], c1[-1, :], m)
out += (res, n)
return out
