axial_symmetric_pt.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2015, 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 #include "descartes_trajectory/axial_symmetric_pt.h"
00020 #include "descartes_core/utils.h"
00021 #include "descartes_trajectory_test/cartesian_robot.h"
00022 #include <gtest/gtest.h>
00023 
00024 using namespace descartes_trajectory;
00025 using namespace descartes_trajectory_test;
00026 
00027 // Does it have a default constructor
00028 TEST(AxialSymPt, construction)
00029 {
00030   AxialSymmetricPt def();
00031 }
00032 
00033 TEST(AxialSymPt, discretization_count)
00034 {
00035   const double SEARCH_DISC = M_PI / 2.0;
00036   CartesianRobot robot (10.0, 2*M_PI);
00037 
00038   Eigen::Affine3d pose (Eigen::Affine3d::Identity());
00039 
00040   AxialSymmetricPt z_point (pose, SEARCH_DISC, AxialSymmetricPt::Z_AXIS);
00041   AxialSymmetricPt x_point (pose, SEARCH_DISC, AxialSymmetricPt::X_AXIS);
00042   AxialSymmetricPt y_point (pose, SEARCH_DISC, AxialSymmetricPt::Y_AXIS);
00043 
00044   EigenSTL::vector_Affine3d solutions;
00045   std::vector<std::vector<double> > joint_solutions;
00046 
00047   const unsigned EXPECTED_POSES = (2.0 * M_PI / SEARCH_DISC) + 1;
00048 
00049   z_point.getCartesianPoses(robot, solutions);
00050   EXPECT_EQ(EXPECTED_POSES, solutions.size());
00051   z_point.getJointPoses(robot, joint_solutions);
00052   EXPECT_EQ(EXPECTED_POSES, joint_solutions.size());
00053 
00054   x_point.getCartesianPoses(robot, solutions);
00055   EXPECT_EQ(EXPECTED_POSES, solutions.size());
00056   x_point.getJointPoses(robot, joint_solutions);
00057   EXPECT_EQ(EXPECTED_POSES, joint_solutions.size());
00058 
00059   y_point.getCartesianPoses(robot, solutions);
00060   EXPECT_EQ(EXPECTED_POSES, solutions.size());
00061   y_point.getJointPoses(robot, joint_solutions);
00062   EXPECT_EQ(EXPECTED_POSES, joint_solutions.size());
00063 }
00064 
00065 bool approxEqual(double a, double b, double tol)
00066 {
00067   return std::fmod(std::fabs(a - b), M_PI) <= tol;
00068 }
00069 
00070 TEST(AxialSymPt, discretization_values)
00071 {
00072   const double SEARCH_DISC = M_PI / 2.0;
00073   CartesianRobot robot (10.0, 2*M_PI);
00074 
00075   Eigen::Affine3d pose (Eigen::Affine3d::Identity());
00076 
00077   AxialSymmetricPt z_point (pose, SEARCH_DISC, AxialSymmetricPt::Z_AXIS);
00078   AxialSymmetricPt x_point (pose, SEARCH_DISC, AxialSymmetricPt::X_AXIS);
00079   AxialSymmetricPt y_point (pose, SEARCH_DISC, AxialSymmetricPt::Y_AXIS);
00080 
00081   const double ANGLE_TOL = 0.001;
00082   // 
00083   EigenSTL::vector_Affine3d solutions;
00084   z_point.getCartesianPoses(robot, solutions);
00085   for (std::size_t i = 0; i < solutions.size(); ++i)
00086   {
00087     Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0,1,2);
00088     EXPECT_TRUE(approxEqual(0.0, rpy(0), ANGLE_TOL));
00089     EXPECT_TRUE(approxEqual(0.0, rpy(1), ANGLE_TOL));
00090     EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(2), ANGLE_TOL));
00091   }
00092 
00093   x_point.getCartesianPoses(robot, solutions);
00094   for (std::size_t i = 0; i < solutions.size(); ++i)
00095   {
00096     Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0,1,2);
00097     EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(0), ANGLE_TOL));
00098     EXPECT_TRUE(approxEqual(0.0, rpy(1), ANGLE_TOL));
00099     EXPECT_TRUE(approxEqual(0.0, rpy(2), ANGLE_TOL));
00100   }
00101 
00102   y_point.getCartesianPoses(robot, solutions);
00103   for (std::size_t i = 0; i < solutions.size(); ++i)
00104   {
00105     Eigen::Vector3d rpy = solutions[i].rotation().eulerAngles(0,1,2);
00106     EXPECT_TRUE(approxEqual(0.0, rpy(0), ANGLE_TOL));
00107     EXPECT_TRUE(approxEqual(-M_PI + SEARCH_DISC * i, rpy(1), ANGLE_TOL));
00108     EXPECT_TRUE(approxEqual(0.0, rpy(2), ANGLE_TOL));
00109   }
00110 }
00111 
00112 


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