Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <moveit/robot_model/robot_model.h>
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/robot_state/transforms.h>
00040 #include <urdf_parser/urdf_parser.h>
00041 #include <fstream>
00042 #include <gtest/gtest.h>
00043
00044 class LoadPlanningModelsPr2 : public testing::Test
00045 {
00046 protected:
00047 virtual void SetUp()
00048 {
00049 srdf_model_.reset(new srdf::Model());
00050
00051 std::string xml_string;
00052 std::fstream xml_file("pr2_description/urdf/robot.xml", std::fstream::in);
00053 if (xml_file.is_open())
00054 {
00055 while (xml_file.good())
00056 {
00057 std::string line;
00058 std::getline(xml_file, line);
00059 xml_string += (line + "\n");
00060 }
00061 xml_file.close();
00062 urdf_model_ = urdf::parseURDF(xml_string);
00063 urdf_ok_ = urdf_model_;
00064 }
00065 else
00066 urdf_ok_ = false;
00067 srdf_ok_ = srdf_model_->initFile(*urdf_model_, "pr2_description/srdf/robot.xml");
00068 };
00069
00070 virtual void TearDown()
00071 {
00072 }
00073
00074 protected:
00075 boost::shared_ptr<urdf::ModelInterface> urdf_model_;
00076 boost::shared_ptr<srdf::Model> srdf_model_;
00077 bool urdf_ok_;
00078 bool srdf_ok_;
00079 };
00080
00081 TEST_F(LoadPlanningModelsPr2, InitOK)
00082 {
00083 ASSERT_TRUE(urdf_ok_);
00084 ASSERT_EQ(urdf_model_->getName(), "pr2_test");
00085
00086 robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
00087 robot_state::RobotState ks(kmodel);
00088 ks.setToRandomValues();
00089 ks.setToDefaultValues();
00090
00091 robot_state::Transforms tf(kmodel->getModelFrame());
00092
00093 Eigen::Affine3d t1;
00094 t1.setIdentity();
00095 t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
00096 tf.setTransform(t1, "some_frame_1");
00097
00098 Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
00099 tf.setTransform(t2, "some_frame_2");
00100
00101 Eigen::Affine3d t3;
00102 t3.setIdentity();
00103 t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
00104 tf.setTransform(t3, "some_frame_3");
00105
00106 EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
00107 EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
00108 EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));
00109
00110 Eigen::Affine3d x;
00111 x.setIdentity();
00112 tf.transformPose(ks, "some_frame_2", x, x);
00113
00114 EXPECT_TRUE(t2.translation() == x.translation());
00115 EXPECT_TRUE(t2.rotation() == x.rotation());
00116
00117 tf.transformPose(ks, kmodel->getModelFrame(), x, x);
00118 EXPECT_TRUE(t2.translation() == x.translation());
00119 EXPECT_TRUE(t2.rotation() == x.rotation());
00120
00121 x.setIdentity();
00122 tf.transformPose(ks, "r_wrist_roll_link", x, x);
00123
00124 EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
00125 EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
00126 EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
00127 }
00128
00129 int main(int argc, char** argv)
00130 {
00131 testing::InitGoogleTest(&argc, argv);
00132 return RUN_ALL_TESTS();
00133 }