TEST_F(PlanningTest, arm_around_table) {
RAVELOG_DEBUG("TEST\n");
RobotBasePtr pr2 = GetRobot(*env);
ProblemConstructionInfo pci(env);
Json::Value root = readJsonFile(string(DATA_DIR) + "/arm_around_table.json");
pci.fromJson(root);
pci.rad->SetDOFValues(toDblVec(pci.init_info.data.row(0)));
TrajOptProbPtr prob = ConstructProblem(pci);
ASSERT_TRUE(!!prob);
BasicTrustRegionSQP opt(prob);
if (plotting) opt.addCallback(PlotCallback(*prob));
opt.initialize(trajToDblVec(prob->GetInitTraj()));
double tStart = GetClock();
opt.optimize();
RAVELOG_INFO("planning time: %.3f\n", GetClock()-tStart);
vector<Collision> collisions;
CollisionChecker::GetOrCreate(*env)->ContinuousCheckTrajectory(getTraj(opt.x(), prob->GetVars()), *pci.rad, collisions);
RAVELOG_INFO("number of continuous collisions: %i\n", collisions.size());
if (plotting) PlotCallback(*prob)(NULL, opt.x());
}
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());
}
TrajOptResultPtr OptimizeProblem(TrajOptProbPtr prob, bool plot) {
RobotBase::RobotStateSaver saver = prob->GetRAD()->Save();
BasicTrustRegionSQP opt(prob);
opt.max_iter_ = 40;
opt.min_approx_improve_frac_ = .001;
opt.merit_error_coeff_ = 20;
if (plot) opt.addCallback(PlotCallback(*prob));
// opt.addCallback(boost::bind(&PlotCosts, boost::ref(prob->getCosts()),boost::ref(*prob->GetRAD()), boost::ref(prob->GetVars()), _1));
opt.initialize(trajToDblVec(prob->GetInitTraj()));
opt.optimize();
return TrajOptResultPtr(new TrajOptResult(opt.results(), *prob));
}
TrajOptResultPtr OptimizeProblem(TrajOptProbPtr prob, bool plot) {
Configuration::SaverPtr saver = prob->GetRAD()->Save();
BasicTrustRegionSQP opt(prob);
opt.max_iter_ = 40;
opt.min_approx_improve_frac_ = .001;
opt.improve_ratio_threshold_ = .2;
opt.merit_error_coeff_ = 20;
if (plot) {
SetupPlotting(*prob, opt);
}
opt.initialize(trajToDblVec(prob->GetInitTraj()));
opt.optimize();
return TrajOptResultPtr(new TrajOptResult(opt.results(), *prob));
}
TEST_F(PlanningTest, arm_around_table) {
RAVELOG_DEBUG("TEST\n");
RobotBasePtr pr2 = GetRobot(*env);
ProblemConstructionInfo pci(env);
Json::Value root = readJsonFile(string(DATA_DIR) + "/arm_around_table.json");
pci.fromJson(root);
pci.rad->SetDOFValues(toDblVec(pci.init_info.data.row(0)));
TrajOptProbPtr prob = ConstructProblem(pci);
ASSERT_TRUE(!!prob);
BasicTrustRegionSQP opt(prob);
if (plotting) opt.addCallback(PlotCallback(*prob));
opt.initialize(trajToDblVec(prob->GetInitTraj()));
double tStart = GetClock();
opt.optimize();
RAVELOG_INFO("planning time: %.3f\n", GetClock()-tStart);
if (plotting) PlotCallback(*prob)(opt.x());
}
