TEST(cast, boxes) {
EnvironmentBasePtr env = RaveCreateEnvironment();
ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml"));
KinBodyPtr box = env->GetKinBody("box");
RobotBasePtr boxbot = env->GetRobot("boxbot");
RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));
rad->GetRobot()->SetActiveDOFs(rad->GetJointIndices(), DOF_X | DOF_Y, Vector());
Json::Value root = readJsonFile(string(DATA_DIR) + "/box_cast_test.json");
DblVec start_dofs; start_dofs += -1.9, 0;
rad->SetDOFValues(start_dofs);
TrajOptProbPtr prob = ConstructProblem(root, env);
TrajArray traj = prob->GetInitTraj();
//shouldn't be necessary:
#if 0
ASSERT_TRUE(!!prob);
double dist_pen = .02, coeff = 10;
prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(0), prob->GetVarRow(1))));
prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(1), prob->GetVarRow(2))));
CollisionCheckerPtr checker = CollisionChecker::GetOrCreate(*prob->GetEnv());
{
vector<Collision> collisions;
checker->ContinuousCheckTrajectory(traj, *rad, collisions);
}
vector<Collision> collisions;
cout << "traj: " << endl << traj << endl;
checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);
cout << collisions.size() << endl;
#endif
BasicTrustRegionSQP opt(prob);
if (plotting) opt.addCallback(PlotCallback(*prob));
opt.initialize(trajToDblVec(prob->GetInitTraj()));
opt.optimize();
if (plotting) PlotCallback(*prob)(NULL, opt.x());
}
TEST(continuous_collisions, boxes) {
EnvironmentBasePtr env = RaveCreateEnvironment();
ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml"));
KinBodyPtr box = env->GetKinBody("box");
RobotBasePtr boxbot = env->GetRobot("boxbot");
CollisionCheckerPtr checker = CreateCollisionChecker(env);
{
RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));
TrajArray traj(2,2);
traj << -1.9,0, 0,1.9;
vector<Collision> collisions;
checker->ContinuousCheckTrajectory(traj, rad, collisions);
ASSERT_EQ(collisions.size(), 1);
Collision col = collisions[0];
Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;
EXPECT_VECTOR_NEAR(robot_normal, Vector(-1, 0, 0), 1e-4);
}
#define TRAJ_TEST_BOILERPLATE\
RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));\
vector<Collision> collisions;\
checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);\
ASSERT_EQ(collisions.size(), 1);\
Collision col = collisions[0];\
Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;
{
TrajArray traj(2,2);
traj << -1.9,0, 0,1.9;
TRAJ_TEST_BOILERPLATE
EXPECT_VECTOR_NEAR(robot_normal, Vector(-1/sqrtf(2), 1/sqrtf(2), 0), 1e-4);
EXPECT_NEAR(col.time, .5, 1e-1);
}
{
TrajArray traj(2,2);
traj << 0, .9, 0,2;
TRAJ_TEST_BOILERPLATE
EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4);
EXPECT_NEAR(col.time, 0, 1e-6);
}
{
TrajArray traj(2,2);
traj << 0,2, 0,.9;
TRAJ_TEST_BOILERPLATE
EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4);
EXPECT_NEAR(col.time, 1, 1e-6);
}
{
RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y | DOF_Z, Vector()));
vector<Collision> collisions;
vector<DblVec> multi_joints;
multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.9,5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.8,-5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,-5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,-5)));
multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,-5)));
checker->MultiCastVsAll(*rad, rad->GetRobot()->GetLinks(), multi_joints, collisions);
ASSERT_EQ(collisions.size(), 1);
Collision col = collisions[0];
Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;
EXPECT_VECTOR_NEAR(robot_normal, Vector(1/sqrtf(2), -1/sqrtf(2), 0), 1e-4);
}
}