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