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); }
std::complex<double> spectrum::correlation(const spectrum& other) const { spectrum spec1(*this), spec2(other); spec1 /= spec1.abs(); spec2 /= spec2.abs(); return spec1 ^ spec2; }