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