Commit 0e9022be by Nicolas Mansard Committed by Nicolas Mansard

[appendix] IVIGIT.

parent a08f120a
 import time import numpy as np p0 = np.random.rand(3) p1 = np.random.rand(3) for t in np.arange(0, 1, .01): p = p0 * (1 - t) + p1 * t viz.applyConfiguration('world/box', list(p) + list(quat.coeffs())) time.sleep(.01)
 from time import sleep import numpy as np from numpy.linalg import norm from pinocchio import SE3, AngleAxis, Quaternion def _lerp(p0, p1, t): return (1 - t) * p0 + t * p1 def slerp(q0, q1, t): assert (t >= 0 and t <= 1) a = AngleAxis(q0.inverse() * q1) return Quaternion(AngleAxis(a.angle * t, a.axis)) def nlerp(q0, q1, t): q0 = q0.coeffs() q1 = q1.coeffs() lerp = _lerp(q0, q1, t) lerp /= norm(lerp) return Quaternion(lerp[3], *list(lerp[:3])) q0 = Quaternion(SE3.Random().rotation) q1 = Quaternion(SE3.Random().rotation) viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs())) sleep(.1) for t in np.arange(0, 1, .01): q = nlerp(q0, q1, t) viz.applyConfiguration('world/box', [0, 0, 0] + list(q.coeffs())) sleep(.01) sleep(.1) viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs()))