/*
* scan expecting any of the 2 given tokens
*/
void
scan2(tok_kind expect1, tok_kind expect2, token *tokp)
{
get_token(tokp);
if (tokp->kind != expect1 && tokp->kind != expect2) {
expected2(expect1, expect2);
}
}
TEST_F(TrajectoryLibraryTest, TestTiRollout) {
Trajectory traj("trajtest/ti/TI-test-TI-straight-pd-no-yaw-00000", true);
Eigen::VectorXd expected(12);
expected << 0,0,0,0,-0.19141,0,12.046,0,-2.3342,0,0,0;
Eigen::VectorXd output = traj.GetState(0);
EXPECT_APPROX_MAT(expected, output, TOLERANCE);
Eigen::VectorXd expected2(12);
expected2 << 26.135,0,9.2492e-09,0,-0.19141,0,12.046,0,-2.3342,0,7.8801e-13,0;
output = traj.GetState(2.13);
EXPECT_APPROX_MAT( expected2, output, TOLERANCE);
}