TEST_F(LoadPlanningModelsPr2, StateSpaces)
{
  ompl_interface::ModelBasedStateSpaceSpecification spec1(kmodel_, "right_arm");
  ompl_interface::ModelBasedStateSpace ss1(spec1);
  ss1.setup();

  ompl_interface::ModelBasedStateSpaceSpecification spec2(kmodel_, "left_arm");
  ompl_interface::ModelBasedStateSpace ss2(spec2);
  ss2.setup();

  ompl_interface::ModelBasedStateSpaceSpecification spec3(kmodel_, "whole_body");
  ompl_interface::ModelBasedStateSpace ss3(spec3);
  ss3.setup();

  ompl_interface::ModelBasedStateSpaceSpecification spec4(kmodel_, "arms");
  ompl_interface::ModelBasedStateSpace ss4(spec4);
  ss4.setup();
  
  std::ofstream fout("ompl_interface_test_state_space_diagram2.dot");
  ompl::base::StateSpace::Diagram(fout);
}
Esempio n. 2
0
 std::complex<double> spectrum::correlation(const spectrum& other) const {
     spectrum spec1(*this), spec2(other);
     spec1 /= spec1.abs(); spec2 /= spec2.abs();
     return spec1 ^ spec2;
 }