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()); }