test_initializers.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <exotica_core/exotica_core.h>
31 #include <exotica_core/tools/test_helpers.h>
32 #include <gtest/gtest.h>
33 
34 using namespace exotica;
35 
36 static const std::string urdf_string = "<robot name=\"test_robot\"><link name=\"base\"><visual><geometry><cylinder length=\"0.3\" radius=\"0.2\"/></geometry><origin xyz=\"0 0 0.15\"/></visual><collision><geometry><cylinder length=\"0.3\" radius=\"0.2\"/></geometry><origin xyz=\"0 0 0.15\"/></collision></link><link name=\"link1\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.15\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.075\"/></visual><collision><geometry><cylinder length=\"0.15\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.075\"/></collision></link><link name=\"link2\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.35\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.175\"/></visual><collision><geometry><cylinder length=\"0.35\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.175\"/></collision></link><link name=\"link3\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.45\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.225\"/></visual><collision><geometry><cylinder length=\"0.45\" radius=\"0.05\"/></geometry><origin xyz=\"0 0 0.225\"/></collision></link><link name=\"endeff\"><inertial><mass value=\"0.2\"/><origin xyz=\"0 0 0.1\"/><inertia ixx=\"0.00381666666667\" ixy=\"0\" ixz=\"0\" iyy=\"0.0036\" iyz=\"0\" izz=\"0.00381666666667\"/></inertial><visual><geometry><cylinder length=\"0.05\" radius=\"0.1\"/></geometry><origin xyz=\"0 0 -0.025\"/></visual><collision><geometry><cylinder length=\"0.05\" radius=\"0.1\"/></geometry><origin xyz=\"0 0 -0.025\"/></collision></link><joint name=\"joint1\" type=\"revolute\"><parent link=\"base\"/><child link=\"link1\"/><origin xyz=\"0 0 0.3\" rpy=\"0 0 0\" /><axis xyz=\"0 0 1\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint2\" type=\"revolute\"><parent link=\"link1\"/><child link=\"link2\"/><origin xyz=\"0 0 0.15\" rpy=\"0 0 0\" /><axis xyz=\"0 1 0\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint3\" type=\"revolute\"><parent link=\"link2\"/><child link=\"link3\"/><origin xyz=\"0 0 0.35\" rpy=\"0 0 0\" /><axis xyz=\"0 1 0\" /><limit effort=\"200\" velocity=\"1.0\" lower=\"-0.4\" upper=\"0.4\"/><safety_controller k_position=\"30\" k_velocity=\"30\" soft_lower_limit=\"-0.4\" soft_upper_limit=\"0.4\"/></joint><joint name=\"joint4\" type=\"fixed\"><parent link=\"link3\"/><child link=\"endeff\"/><origin xyz=\"0 0 0.45\" rpy=\"0 0 0\" /></joint></robot>";
37 static const std::string srdf_string = "<robot name=\"test_robot\"><group name=\"arm\"><chain base_link=\"base\" tip_link=\"endeff\" /></group><virtual_joint name=\"world_joint\" type=\"fixed\" parent_frame=\"world_frame\" child_link=\"base\" /><group_state name=\"zero\" group=\"arm\"><joint name=\"joint1\" value=\"0\" /><joint name=\"joint2\" value=\"0.3\" /><joint name=\"joint3\" value=\"0.55\" /></group_state><disable_collisions link1=\"base\" link2=\"link1\" reason=\"Adjacent\" /><disable_collisions link1=\"endeff\" link2=\"link3\" reason=\"Adjacent\" /><disable_collisions link1=\"link1\" link2=\"link2\" reason=\"Adjacent\" /><disable_collisions link1=\"link2\" link2=\"link3\" reason=\"Adjacent\" /></robot>";
38 
39 int argc_;
40 char** argv_;
41 
42 bool testCore()
43 {
44  if (Setup::GetSolvers().size() == 0)
45  {
46  ADD_FAILURE() << "Failed to find any solvers.";
47  return false;
48  }
49  if (Setup::GetProblems().size() == 0)
50  {
51  ADD_FAILURE() << "Failed to find any problems.";
52  return false;
53  }
54  if (Setup::GetMaps().size() == 0)
55  {
56  ADD_FAILURE() << "Failed to find any maps.";
57  return false;
58  }
59  return true;
60 }
61 
62 bool testGenericInit()
63 {
64  Initializer scene("Scene", {{"Name", std::string("MyScene")}, {"JointGroup", std::string("arm")}});
65  Initializer map("exotica/EffPosition", {{"Name", std::string("Position")},
66  {"Scene", std::string("MyScene")},
67  {"EndEffector", std::vector<Initializer>({Initializer("Frame", {{"Link", std::string("endeff")}})})}});
68  Eigen::VectorXd W(3);
69  W << 3, 2, 1;
70  Initializer problem("exotica/UnconstrainedEndPoseProblem", {
71  {"Name", std::string("MyProblem")},
72  {"PlanningScene", scene},
73  {"Maps", std::vector<Initializer>({map})},
74  {"W", W},
75  });
76  Initializer solver("exotica/IKSolver", {
77  {"Name", std::string("MySolver")},
78  {"MaxIterations", 1},
79  {"MaxStep", 0.1},
80  {"C", 1e-3},
81  });
82  Server::Instance()->GetModel("robot_description", urdf_string, srdf_string);
83  PlanningProblemPtr any_problem = Setup::CreateProblem(problem);
84  MotionSolverPtr any_solver = Setup::CreateSolver(solver);
85  any_solver->SpecifyProblem(any_problem);
86  return true;
87 }
88 
89 bool testXMLInit()
90 {
91  std::string XMLstring = "<IKSolverDemoConfig><IKSolver Name=\"MySolver\"><MaxIterations>1</MaxIterations><MaxStep>0.1</MaxStep><C>1e-3</C></IKSolver><UnconstrainedEndPoseProblem Name=\"MyProblem\"><PlanningScene><Scene Name=\"MyScene\"><JointGroup>arm</JointGroup></Scene></PlanningScene><Maps><EffPosition Name=\"Position\"><Scene>MyScene</Scene><EndEffector><Frame Link=\"endeff\" /></EndEffector></EffPosition></Maps><W> 3 2 1 </W></UnconstrainedEndPoseProblem></IKSolverDemoConfig>";
92  Initializer solver, problem;
93  XMLLoader::Load(XMLstring, solver, problem, "", "", true);
94  PlanningProblemPtr any_problem = Setup::CreateProblem(problem);
95  MotionSolverPtr any_solver = Setup::CreateSolver(solver);
96  any_solver->SpecifyProblem(any_problem);
97  return true;
98 }
99 
100 bool testRos()
101 {
102  {
103  TEST_COUT << "Parsing EXOTica paths...";
104  std::string path1 = ros::package::getPath("exotica_core");
105  std::string path2 = ParsePath("{exotica_core}");
106  if (path1 != path2)
107  ADD_FAILURE() << "Failed when parsing paths:\n"
108  << path1 << "\n"
109  << path2;
110  }
111 
112  // Reset server
113  Server::Destroy();
114 
115  if (Server::IsRos()) ThrowPretty("ROS node initialized, but it shouldn't have been!");
116 
117  // Fail when ROS node has not been initialized
118  try
119  {
120  Server::Instance()->GetNodeHandle();
121  return false;
122  }
123  catch (Exception& e)
124  {
125  }
126 
127  if (Server::HasParam("/rosdistro")) ThrowPretty("ROS param retrieved, but shouldn't have!");
128  try
129  {
130  std::string param;
131  Server::GetParam("/rosdistro", param);
132  return false;
133  }
134  catch (Exception& e)
135  {
136  }
137 
138  // Load robot model into cache from a file
139  Server::Instance()->GetModel("robot_description", urdf_string, srdf_string);
140  // Load model from the cache
141  Server::Instance()->GetModel("robot_description");
142  // Reset server, deleting the model cache
143  Server::Destroy();
144  // Attempt to load model from the empty cache
145  try
146  {
147  // Fails because URDF/SRDF are not specified and ROS node is not running to load model from ROS params.
148  Server::Instance()->GetModel("robot_description");
149  return false;
150  }
151  catch (Exception& e)
152  {
153  }
154  return true;
155 }
156 
157 TEST(ExoticaTestInitializers, testRos)
158 {
159  try
160  {
161  EXPECT_TRUE(testRos());
162  }
163  catch (const std::exception& e)
164  {
165  ADD_FAILURE() << "Uncaught exception! " << e.what();
166  }
167 }
168 
169 TEST(ExoticaTestInitializers, testCore)
170 {
171  try
172  {
173  ros::init(argc_, argv_, "EXOTica_test_initializers");
174  EXPECT_TRUE(testCore());
175  Setup::Destroy();
176  }
177  catch (const std::exception& e)
178  {
179  ADD_FAILURE() << "Uncaught exception! " << e.what();
180  }
181 }
182 
183 TEST(ExoticaTestInitializers, testGenericInit)
184 {
185  try
186  {
187  EXPECT_TRUE(testGenericInit());
188  }
189  catch (const std::exception& e)
190  {
191  ADD_FAILURE() << "Uncaught exception! " << e.what();
192  }
193 }
194 
195 TEST(ExoticaTestInitializers, testXMLInit)
196 {
197  try
198  {
199  EXPECT_TRUE(testXMLInit());
200  }
201  catch (const std::exception& e)
202  {
203  ADD_FAILURE() << "Uncaught exception! " << e.what();
204  }
205 }
206 
207 int main(int argc, char** argv)
208 {
209  testing::InitGoogleTest(&argc, argv);
210  argc_ = argc;
211  argv_ = argv;
212  int ret = RUN_ALL_TESTS();
213  Setup::Destroy();
214  return ret;
215 }
urdf_string
static const std::string urdf_string
Definition: test_initializers.cpp:36
testGenericInit
bool testGenericInit()
Definition: test_initializers.cpp:62
param
bool param(const std::string &param_name, T &param_val, const T &default_val)
exotica::MotionSolverPtr
std::shared_ptr< exotica::MotionSolver > MotionSolverPtr
exotica_core.h
main
int main(int argc, char **argv)
Definition: test_initializers.cpp:207
argv_
char ** argv_
Definition: test_initializers.cpp:40
testCore
bool testCore()
Definition: test_initializers.cpp:42
srdf_string
static const std::string srdf_string
Definition: test_initializers.cpp:37
exotica::Setup::CreateSolver
static std::shared_ptr< exotica::MotionSolver > CreateSolver(const std::string &type, bool prepend=true)
exotica::Server::Destroy
static void Destroy()
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
test.test_dynamic_time_indexed_shooting_problem.problem
problem
Definition: test_dynamic_time_indexed_shooting_problem.py:201
size
std::size_t size(custom_string const &s)
ThrowPretty
#define ThrowPretty(m)
exotica::Server::GetParam
static bool GetParam(const std::string &name, T &param)
exotica::Server::Instance
static std::shared_ptr< Server > Instance()
exotica::Setup::Destroy
static void Destroy()
exotica::Server::HasParam
static bool HasParam(const std::string &name)
testXMLInit
bool testXMLInit()
Definition: test_initializers.cpp:89
TEST
TEST(ExoticaTestInitializers, testRos)
Definition: test_initializers.cpp:157
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
exotica::Setup::GetProblems
static std::vector< std::string > GetProblems()
exotica::ParsePath
std::string ParsePath(const std::string &path)
argc_
int argc_
Definition: test_initializers.cpp:39
test.test_dynamic_time_indexed_shooting_problem.scene
scene
Definition: test_dynamic_time_indexed_shooting_problem.py:202
exotica::Setup::GetMaps
static std::vector< std::string > GetMaps()
exotica::Setup::GetSolvers
static std::vector< std::string > GetSolvers()
exotica::Setup::CreateProblem
static std::shared_ptr< exotica::PlanningProblem > CreateProblem(const std::string &type, bool prepend=true)
exotica::PlanningProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
test_helpers.h
exotica::Server::IsRos
static bool IsRos()
EXPECT_TRUE
#define EXPECT_TRUE(args)
exotica::XMLLoader::Load
static void Load(std::string file_name, Initializer &solver, Initializer &problem, const std::string &solver_name="", const std::string &problem_name="", bool parsePathAsXML=false)
TEST_COUT
#define TEST_COUT
testRos
bool testRos()
Definition: test_initializers.cpp:100

exotica_examples
Author(s):
autogenerated on Sat Apr 10 2021 02:37:17