test_transforms.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45