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(xs) qds_spline = path(xs, 1) qdds_spline = path(xs, 2) # 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_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_1waypoints(): "The case where there is only one waypoint." pi = SplineInterpolator([0], [[1, 2, 3]]) assert pi.dof == 3 npt.assert_allclose(pi.path_interval, np.r_[0, 0]) npt.assert_allclose(pi(0), np.r_[1, 2, 3]) npt.assert_allclose(pi(0, 1), np.r_[0, 0, 0]) npt.assert_allclose(pi([0, 0]), [[1, 2, 3], [1, 2, 3]]) npt.assert_allclose(pi([0, 0], 1), [[0, 0, 0], [0, 0, 0]])
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(ss).shape == (len(ss), ) assert pi(ss, 1).shape == (len(ss), ) assert pi(ss, 2).shape == (len(ss), ) assert pi(0).shape == () npt.assert_allclose(pi.path_interval, path_interval)
def test_1waypoints(self): "The case where there is only one waypoint." pi = SplineInterpolator([0], [[1, 2, 3]]) assert pi.dof == 3 npt.assert_allclose(pi.get_path_interval(), np.r_[0, 0]) npt.assert_allclose(pi.eval(0), np.r_[1, 2, 3]) npt.assert_allclose(pi.evald(0), np.r_[0, 0, 0]) npt.assert_allclose(pi.eval([0, 0]), [[1, 2, 3], [1, 2, 3]]) npt.assert_allclose(pi.evald([0, 0]), [[0, 0, 0], [0, 0, 0]])
def test_scalar(self, 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.get_path_interval(), path_interval)
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]))
def given_scalar_spline_interpolator(): sswp, wp, ss, path_interval = [[0, 0.3, 0.5], [1, 2, 3], [0.0, 0.1, 0.2, 0.3, 0.5], [0, 0.5]] pi = SplineInterpolator(sswp, wp) # 1 + 2s + 3s^2 yield pi, path_interval