robot_model_test.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  * http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #ifndef ROBOT_MODEL_TEST_HPP_
00020 #define ROBOT_MODEL_TEST_HPP_
00021 
00022 #include "descartes_core/pretty_print.hpp"
00023 #include "descartes_core/robot_model.h"
00024 #include "ros/console.h"
00025 #include <gtest/gtest.h>
00026 
00036 namespace descartes_trajectory_test
00037 {
00038 
00039 template <class T>
00040 descartes_core::RobotModelPtr CreateRobotModel();
00041 
00042 template <class T>
00043 class RobotModelTest : public ::testing::Test
00044 {
00045 public:
00046   RobotModelTest() : model_(CreateRobotModel<T>())
00047   {
00048     ROS_INFO("Instantiated RobotModelTest fixture(base) (parameterized)");
00049   }
00050 
00051   virtual void SetUp()
00052   {
00053     ROS_INFO("Setting up RobotModelTest fixture(base) (parameterized)");
00054     ASSERT_TRUE(static_cast<bool>(this->model_));
00055   }
00056 
00057   virtual void TearDown()
00058   {
00059     ROS_INFO("Tearing down RobotModelTest fixture(base) (parameterized)");
00060   }
00061 
00062   virtual ~RobotModelTest()
00063   {
00064     ROS_INFO("Desctructing RobotModelTest fixture(base) (parameterized)");
00065   }
00066 
00067   descartes_core::RobotModelPtr model_;
00068 };
00069 
00070 using namespace descartes_core;
00071 
00072 TYPED_TEST_CASE_P(RobotModelTest);
00073 
00074 const double TF_EQ_TOL = 0.001;
00075 const double JOINT_EQ_TOL = 0.001;
00076 
00077 
00078 TYPED_TEST_P(RobotModelTest, construction) {
00079   ROS_INFO_STREAM("Robot model test construction");
00080 }
00081 
00082 
00083 TYPED_TEST_P(RobotModelTest, getIK) {
00084   ROS_INFO_STREAM("Testing getIK");
00085   std::vector<double> fk_joint(6, 0.0);
00086   std::vector<double> ik_joint;
00087   Eigen::Affine3d ik_pose, fk_pose;
00088   EXPECT_TRUE(this->model_->getFK(fk_joint, ik_pose));
00089   EXPECT_TRUE(this->model_->getIK(ik_pose, fk_joint, ik_joint));
00090   //This doesn't always work, but it should.  The IKFast solution doesn't
00091   //return the "closets" solution.  Numeric IK does appear to do this.
00092   EXPECT_TRUE(this->model_->getFK(ik_joint, fk_pose));
00093   EXPECT_TRUE(ik_pose.matrix().isApprox(fk_pose.matrix(), TF_EQ_TOL));
00094   ROS_INFO_STREAM("getIK Test completed");
00095 }
00096 
00097 TYPED_TEST_P(RobotModelTest, getAllIK) {
00098   ROS_INFO_STREAM("Testing getAllIK");
00099   std::vector<double> fk_joint(6, 0.5);
00100   std::vector<std::vector<double> > joint_poses;
00101   Eigen::Affine3d ik_pose, fk_pose;
00102 
00103   EXPECT_TRUE(this->model_->getFK(fk_joint, ik_pose));
00104   EXPECT_TRUE(this->model_->getAllIK(ik_pose, joint_poses));
00105   ROS_INFO_STREAM("Get all IK returned " << joint_poses.size() << " solutions");
00106   std::vector<std::vector<double> >::iterator it;
00107   for (it = joint_poses.begin(); it != joint_poses.end(); ++it)
00108   {
00109     ROS_INFO_STREAM("GetIK joint solution: " << *it);
00110     EXPECT_TRUE(this->model_->getFK(*it, fk_pose));
00111     EXPECT_TRUE(ik_pose.matrix().isApprox(fk_pose.matrix(), TF_EQ_TOL));
00112   }
00113 }
00114 
00115 
00116 REGISTER_TYPED_TEST_CASE_P(RobotModelTest, construction, getIK, getAllIK);
00117 
00118 } //descartes_trajectory_test
00119 
00120 #endif // ROBOT_MODEL_TEST_HPP_


descartes_trajectory
Author(s): Jorge Nicho
autogenerated on Wed Aug 26 2015 11:21:27