Ejemplo n.º 1
0
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());

}
Ejemplo n.º 2
0
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));
}
Ejemplo n.º 3
0
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));
}
Ejemplo n.º 5
0
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());
}
Ejemplo n.º 6
0
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());




}
Ejemplo n.º 7
0
 void SetRobotActiveDOFs() {
   m_prob->GetRAD()->SetRobotActiveDOFs();
 }
Ejemplo n.º 8
0
 py::list GetDOFIndices() {
   vector<int> inds = m_prob->GetRAD()->GetJointIndices();
   return toPyList(inds);
 }
Ejemplo n.º 9
0
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);
}
Ejemplo n.º 10
0
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);
}
Ejemplo n.º 11
0
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);
}
Ejemplo n.º 12
0
 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();
 }
Ejemplo n.º 13
0
 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);
 }