test_state_space.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <moveit_resources/config.h>
40 
41 #include <urdf_parser/urdf_parser.h>
42 
43 #include <ompl/util/Exception.h>
45 #include <gtest/gtest.h>
46 #include <fstream>
47 #include <boost/filesystem/path.hpp>
48 
49 class LoadPlanningModelsPr2 : public testing::Test
50 {
51 protected:
52  virtual void SetUp()
53  {
54  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
55 
56  srdf_model_.reset(new srdf::Model());
57  std::string xml_string;
58  std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
59  if (xml_file.is_open())
60  {
61  while (xml_file.good())
62  {
63  std::string line;
64  std::getline(xml_file, line);
65  xml_string += (line + "\n");
66  }
67  xml_file.close();
68  urdf_model_ = urdf::parseURDF(xml_string);
69  }
70  srdf_model_->initFile(*urdf_model_, (res_path / "pr2_description/srdf/robot.xml").string());
71  robot_model_.reset(new moveit::core::RobotModel(urdf_model_, srdf_model_));
72  };
73 
74  virtual void TearDown()
75  {
76  }
77 
78 protected:
79  robot_model::RobotModelPtr robot_model_;
80  urdf::ModelInterfaceSharedPtr urdf_model_;
82  bool urdf_ok_;
83  bool srdf_ok_;
84 };
85 
87 {
88  ompl_interface::ModelBasedStateSpaceSpecification spec(robot_model_, "whole_body");
90  ss.setPlanningVolume(-1, 1, -1, 1, -1, 1);
91  ss.setup();
92  std::ofstream fout("ompl_interface_test_state_space_diagram1.dot");
93  ss.diagram(fout);
94  bool passed = false;
95  try
96  {
97  ss.sanityChecks();
98  passed = true;
99  }
100  catch (ompl::Exception& ex)
101  {
102  ROS_ERROR("Sanity checks did not pass: %s", ex.what());
103  }
104  EXPECT_TRUE(passed);
105 }
106 
108 {
109  ompl_interface::ModelBasedStateSpaceSpecification spec1(robot_model_, "right_arm");
111  ss1.setup();
112 
113  ompl_interface::ModelBasedStateSpaceSpecification spec2(robot_model_, "left_arm");
115  ss2.setup();
116 
117  ompl_interface::ModelBasedStateSpaceSpecification spec3(robot_model_, "whole_body");
119  ss3.setup();
120 
121  ompl_interface::ModelBasedStateSpaceSpecification spec4(robot_model_, "arms");
123  ss4.setup();
124 
125  std::ofstream fout("ompl_interface_test_state_space_diagram2.dot");
126  ompl::base::StateSpace::Diagram(fout);
127 }
128 
130 {
131  ompl_interface::ModelBasedStateSpaceSpecification spec(robot_model_, "right_arm");
133  ss.setPlanningVolume(-1, 1, -1, 1, -1, 1);
134  ss.setup();
135  std::ofstream fout("ompl_interface_test_state_space_diagram1.dot");
136  ss.diagram(fout);
137  bool passed = false;
138  try
139  {
140  ss.sanityChecks();
141  passed = true;
142  }
143  catch (ompl::Exception& ex)
144  {
145  ROS_ERROR("Sanity checks did not pass: %s", ex.what());
146  }
147  EXPECT_TRUE(passed);
148 
149  robot_state::RobotState kstate(robot_model_);
150  kstate.setToRandomPositions();
151  EXPECT_TRUE(kstate.distance(kstate) < 1e-12);
152  ompl::base::State* state = ss.allocState();
153  for (int i = 0; i < 10; ++i)
154  {
155  robot_state::RobotState kstate2(kstate);
156  EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
157  ss.copyToOMPLState(state, kstate);
158  kstate.setToRandomPositions(kstate.getRobotModel()->getJointModelGroup(ss.getJointModelGroupName()));
159  std::cout << (kstate.getGlobalLinkTransform("r_wrist_roll_link").translation() -
160  kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation())
161  << std::endl;
162  EXPECT_TRUE(kstate.distance(kstate2) > 1e-12);
163  ss.copyToRobotState(kstate, state);
164  std::cout << (kstate.getGlobalLinkTransform("r_wrist_roll_link").translation() -
165  kstate2.getGlobalLinkTransform("r_wrist_roll_link").translation())
166  << std::endl;
167  EXPECT_TRUE(kstate.distance(kstate2) < 1e-12);
168  }
169 
170  ss.freeState(state);
171 }
172 
173 int main(int argc, char** argv)
174 {
175  testing::InitGoogleTest(&argc, argv);
176  return RUN_ALL_TESTS();
177 }
urdf::ModelInterfaceSharedPtr urdf_model_
virtual void copyToRobotState(robot_state::RobotState &rstate, const ompl::base::State *state) const
Copy the data from an OMPL state to a set of joint states.
virtual void SetUp()
virtual void copyToOMPLState(ompl::base::State *state, const robot_state::RobotState &rstate) const
Copy the data from a set of joint states to an OMPL state.
virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
Set the planning volume for the possible SE2 and/or SE3 components of the state space.
virtual ompl::base::State * allocState() const
def xml_string(rootXml, addHeader=True)
virtual void freeState(ompl::base::State *state) const
srdf::ModelSharedPtr srdf_model_
#define EXPECT_TRUE(args)
#define ROS_ERROR(...)
TEST_F(LoadPlanningModelsPr2, StateSpace)
int main(int argc, char **argv)
void TearDown() override
const std::string & getJointModelGroupName() const


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:03:46