robot_state/test/test_transforms.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 the 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 
39 #include <moveit/robot_state/transforms.h>
40 #include <urdf_parser/urdf_parser.h>
41 #include <fstream>
42 #include <gtest/gtest.h>
43 
44 class LoadPlanningModelsPr2 : public testing::Test
45 {
46 protected:
47  virtual void SetUp()
48  {
49  srdf_model_.reset(new srdf::Model());
50 
51  std::string xml_string;
52  std::fstream xml_file("pr2_description/urdf/robot.xml", std::fstream::in);
53  if (xml_file.is_open())
54  {
55  while (xml_file.good())
56  {
57  std::string line;
58  std::getline(xml_file, line);
59  xml_string += (line + "\n");
60  }
61  xml_file.close();
62  urdf_model_ = urdf::parseURDF(xml_string);
64  }
65  else
66  urdf_ok_ = false;
67  srdf_ok_ = srdf_model_->initFile(*urdf_model_, "pr2_description/srdf/robot.xml");
68  };
69 
70  virtual void TearDown()
71  {
72  }
73 
74 protected:
75  urdf::ModelInterfaceSharedPtr urdf_model_;
77  bool urdf_ok_;
78  bool srdf_ok_;
79 };
80 
82 {
83  ASSERT_TRUE(urdf_ok_);
84  ASSERT_EQ(urdf_model_->getName(), "pr2_test");
85 
86  robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
87  robot_state::RobotState ks(kmodel);
88  ks.setToRandomValues();
89  ks.setToDefaultValues();
90 
91  robot_state::Transforms tf(kmodel->getModelFrame());
92 
93  Eigen::Affine3d t1;
94  t1.setIdentity();
95  t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
96  tf.setTransform(t1, "some_frame_1");
97 
98  Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
99  tf.setTransform(t2, "some_frame_2");
100 
101  Eigen::Affine3d t3;
102  t3.setIdentity();
103  t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
104  tf.setTransform(t3, "some_frame_3");
105 
106  EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
107  EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
108  EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));
109 
110  Eigen::Affine3d x;
111  x.setIdentity();
112  tf.transformPose(ks, "some_frame_2", x, x);
113 
114  EXPECT_TRUE(t2.translation() == x.translation());
115  EXPECT_TRUE(t2.rotation() == x.rotation());
116 
117  tf.transformPose(ks, kmodel->getModelFrame(), x, x);
118  EXPECT_TRUE(t2.translation() == x.translation());
119  EXPECT_TRUE(t2.rotation() == x.rotation());
120 
121  x.setIdentity();
122  tf.transformPose(ks, "r_wrist_roll_link", x, x);
123 
124  EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
125  EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
126  EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
127 }
128 
129 int main(int argc, char** argv)
130 {
131  testing::InitGoogleTest(&argc, argv);
132  return RUN_ALL_TESTS();
133 }
urdf::ModelInterfaceSharedPtr urdf_model_
robot_model::RobotModelPtr kmodel
int main(int argc, char **argv)
#define EXPECT_NEAR(a, b, prec)
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
TEST_F(LoadPlanningModelsPr2, InitOK)
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
#define EXPECT_FALSE(args)
def xml_string(rootXml, addHeader=True)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
double x
#define EXPECT_TRUE(args)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Sep 15 2018 02:51:06