start_kappa = 0
start_gamma = 1
+ tilt = atan(mu)
+ tilt_basis = np.array([
+ 1, 0, 0, 0,
+ 0, cos(tilt), -sin(tilt), 0,
+ 0, sin(tilt), cos(tilt), 0,
+ 0, 0, 0, 1,
+ ])
+ findcurve_basis = dPQplane_basis * tilt_basis
+ findcurve_into = np.linalg.inv(findcurve_basis)
+
+ q_findcurve = unaugment(findcurve_into, augment(q))
+ dq_findcurve = unaugment(findcurve_into, augment0(dq))
+
+ findcurve_target = np.concatenate(q_findcurve, dq_findcurve)
+ findcurve_start = (sqrt(start_s), sqrt(start_t), start_la,
+ start_mu, start_gamma, start_kappa)
+
+
# we work in two additional coordinate systems:
# for both these: