def test_compute_rave_trajectory(robot_fixture, ss_waypoints, waypoints):
"""From the given spline trajectory, compute openrave trajectory."""
active_indices = robot_fixture.GetActiveDOFIndices()
path = SplineInterpolator(ss_waypoints, waypoints)
traj = path.compute_rave_trajectory(robot_fixture)
spec = traj.GetConfigurationSpecification()
xs = np.linspace(0, path.duration, 10)
# Interpolate with spline
qs_spline = path.eval(xs)
qds_spline = path.evald(xs)
qdds_spline = path.evaldd(xs)
# Interpolate with OpenRAVE
qs_rave = []
qds_rave = []
qdds_rave = []
for t in xs:
data = traj.Sample(t)
qs_rave.append(spec.ExtractJointValues(
data, robot_fixture, active_indices, 0))
qds_rave.append(spec.ExtractJointValues(
data, robot_fixture, active_indices, 1))
qdds_rave.append(spec.ExtractJointValues(
data, robot_fixture, active_indices, 2))
# Assert all close
npt.assert_allclose(qs_spline, qs_rave)
npt.assert_allclose(qds_spline, qds_rave)
npt.assert_allclose(qdds_spline, qdds_rave)
def test_scalar(sswp, wp, ss, path_interval):
"A scalar (dof=1) trajectory."
pi = SplineInterpolator(sswp, wp) # 1 + 2s + 3s^2
assert pi.dof == 1
assert pi.eval(ss).shape == (len(ss), )
assert pi.evald(ss).shape == (len(ss), )
assert pi.evaldd(ss).shape == (len(ss), )
assert pi.eval(0).shape == ()
npt.assert_allclose(pi.path_interval, path_interval)
def test_5_dof():
pi = SplineInterpolator([0, 1], np.random.rand(2, 5))
# [1 + 2s + 3s^2]
# [-2 + 3s + 4s^2 + 5s^3]
ss = np.linspace(0, 1, 10)
assert pi.dof == 5
assert pi.eval(ss).shape == (10, 5)
assert pi.evald(ss).shape == (10, 5)
assert pi.evaldd(ss).shape == (10, 5)
npt.assert_allclose(pi.path_interval, np.r_[0, 1])
def test_2waypoints(xs, ys, yd):
"There is only two waypoints. Linear interpolation is done between them."
pi = SplineInterpolator(xs, ys, bc_type='natural')
npt.assert_allclose(pi.path_interval, xs)
npt.assert_allclose(pi.evald((xs[0] + xs[1]) / 2), yd)
npt.assert_allclose(pi.evaldd(0), np.zeros_like(ys[0]))