vdbg().arrow(findcurve_basis[0:3,3], findcurve_basis[0:3,ax])
q_findcurve = augmatmultiply(findcurve_into, q)
- dq_findcurve = augmatmultiply(findcurve_into, dq, augwith=0)
+ dq_findcurve = -augmatmultiply(findcurve_into, dq, augwith=0)
findcurve_target = np.hstack((q_findcurve, dq_findcurve))
findcurve_start = (sqrt(start_s), sqrt(start_t), start_la,