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());
}
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));
}
TEST_F(PlanningTest, numerical_ik1) {
Json::Value root = readJsonFile(string(DATA_DIR) + "/numerical_ik1.json");
TrajOptProbPtr prob = ConstructProblem(root, env);
ASSERT_TRUE(!!prob);
BasicTrustRegionSQP opt(prob);
// opt.addCallback(boost::bind(&PlotCosts, boost::ref(prob->getCosts()),*prob->GetRAD(), prob->GetVars(), _1));
opt.initialize(DblVec(prob->GetNumDOF(), 0));
double tStart = GetClock();
opt.optimize();
RAVELOG_INFO("planning time: %.3f\n", GetClock()-tStart);
printf("finally:\n");
if (plotting) PlotCallback(*prob)(NULL, opt.x());
}
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());
}
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());
}
void SetRobotActiveDOFs() {
m_prob->GetRAD()->SetRobotActiveDOFs();
}
py::list GetDOFIndices() {
vector<int> inds = m_prob->GetRAD()->GetJointIndices();
return toPyList(inds);
}
void PyTrajOptProb::AddErrCost2(py::object f, py::object dfdx, py::list ijs, const string& typestr, const string& name) {
PenaltyType type = _GetPenaltyType(typestr);
VarVector vars = _GetVars(ijs, m_prob->GetVars());
CostPtr c(new CostFromErrFunc(VectorOfVectorPtr(new VectorFuncFromPy(f)), MatrixOfVectorPtr(new MatrixFuncFromPy(dfdx)), vars, VectorXd(), type, name));
m_prob->addCost(c);
}
void PyTrajOptProb::AddCost1(py::object f, py::list ijs, const string& name) {
VarVector vars = _GetVars(ijs, m_prob->GetVars());
CostPtr c(new CostFromFunc(ScalarOfVectorPtr(new ScalarFuncFromPy(f)), vars, "f"));
m_prob->addCost(c);
}
void PyTrajOptProb::AddConstraint2(py::object f, py::object dfdx, py::list ijs, const string& typestr, const string& name) {
ConstraintType type = _GetConstraintType(typestr);
VarVector vars = _GetVars(ijs, m_prob->GetVars());
ConstraintPtr c(new ConstraintFromFunc(VectorOfVectorPtr(new VectorFuncFromPy(f)), MatrixOfVectorPtr(new MatrixFuncFromPy(dfdx)), vars, type, name));
m_prob->addConstr(c);
}
void SetRobotActiveDOFs() {
RobotAndDOFPtr rad = boost::dynamic_pointer_cast<RobotAndDOF>(m_prob->GetRAD());
if (!rad) PRINT_AND_THROW("can only call SetRobotActiveDOFs on a robot");
rad->SetRobotActiveDOFs();
}
py::list GetDOFIndices() {
RobotAndDOFPtr rad = boost::dynamic_pointer_cast<RobotAndDOF>(m_prob->GetRAD());
if (!rad) PRINT_AND_THROW("can only call GetDOFIndices on a robot");
vector<int> inds = rad->GetJointIndices();
return toPyList(inds);
}
