asr_robot_model_services: IKTest.cpp Source File

Go to the documentation of this file.
00001 
00020 #define BOOST_TEST_STATIC_LINK
00021 
00022 #include <boost/test/included/unit_test.hpp>
00023 #include "robot_model_services/test_cases/BaseTest.h"
00024 #include "robot_model_services/robot_model/impl/MILDRobotModelWithExactIK.hpp"
00025 #include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
00026 #include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
00027 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00028 
00029 using namespace robot_model_services;
00030 using namespace boost::unit_test;
00031 
00032 class IKTest : public BaseTest{
00033 public:
00034     IKTest() : BaseTest (false){
00035     }
00036 
00037     virtual ~IKTest() {}
00038 
00039     void cameraPoseTest() {
00040         std::vector<SimpleVector3> targetCameraPositions;
00041         std::vector<SimpleQuaternion> targetCameraOrientations;
00042 
00043         //Initialize Poses
00044         targetCameraPositions.push_back(SimpleVector3(0, 0, 0));
00045         targetCameraOrientations.push_back(ZXZ2Quaternion(0.0, 0.0, 0.0));
00046         //Added some poses where the X or Y direction of the camera is exactly zero
00047         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
00048         targetCameraOrientations.push_back(ZXZ2Quaternion(0, 0.0, 0 ));
00049         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
00050         targetCameraOrientations.push_back(ZXZ2Quaternion(90, 0.0, 0 ));
00051         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
00052         targetCameraOrientations.push_back(ZXZ2Quaternion(180, 0.0, 0 ));
00053         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
00054         targetCameraOrientations.push_back(ZXZ2Quaternion(270, 0.0, 0 ));
00055 
00056         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00057         targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 0.0, 0 ));
00058         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00059         targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 80.0, 0));
00060         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00061         targetCameraOrientations.push_back(ZXZ2Quaternion(-180, 100.0, 0 ));
00062         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00063         targetCameraOrientations.push_back(ZXZ2Quaternion(-200, 40.0, 0));
00064         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00065         targetCameraOrientations.push_back(ZXZ2Quaternion(-180, -30.0, 0));
00066         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
00067         targetCameraOrientations.push_back(ZXZ2Quaternion(-190, -50.0, 0));
00068         targetCameraPositions.push_back(SimpleVector3(-0.404993116856, -0.28920769691, 1.4));
00069         targetCameraOrientations.push_back(ZXZ2Quaternion(-120, -20.0, 0));
00070         targetCameraPositions.push_back(SimpleVector3(0.804993116856, 0.98920769691, 1.4));
00071         targetCameraOrientations.push_back(ZXZ2Quaternion(120, -20.0, 0));
00072         targetCameraPositions.push_back(SimpleVector3(0.004993116856, 2.08920769691, 1.5));
00073         targetCameraOrientations.push_back(ZXZ2Quaternion(20, -20.0, 0));
00074         targetCameraPositions.push_back(SimpleVector3(0.404993116856, 0.28920769691, 0.8));
00075         targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 0.0, 0));
00076         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.7));
00077         targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 0.0, 0 ));
00078         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -1.28920769691, 0.4));
00079         targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 80.0, 0));
00080         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -1.28920769691, 0.5));
00081         targetCameraOrientations.push_back(ZXZ2Quaternion(-180, 100.0, 0 ));
00082         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.0));
00083         targetCameraOrientations.push_back(ZXZ2Quaternion(-200, 40.0, 0));
00084         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.8));
00085         targetCameraOrientations.push_back(ZXZ2Quaternion(-180, -30.0, 0));
00086         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
00087         targetCameraOrientations.push_back(ZXZ2Quaternion(-190, -50.0, 0));
00088         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.1));
00089         targetCameraOrientations.push_back(ZXZ2Quaternion(-120, -20.0, 0));
00090         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.7));
00091         targetCameraOrientations.push_back(ZXZ2Quaternion(120, -20.0, 0));
00092         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.0));
00093         targetCameraOrientations.push_back(ZXZ2Quaternion(20, -20.0, 0));
00094         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 0.9));
00095         targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 0.0, 0));
00096         //a couple of very difficult test cases
00097         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 2.5));
00098         targetCameraOrientations.push_back(ZXZ2Quaternion(-20, -20.0, 0));
00099         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 0.0));
00100         targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 30.0, 0));
00101         targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, -1.0));
00102         targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 30.0, 0));
00103 
00104 
00105         ROS_INFO_STREAM("Initializing...");
00106 
00107         //Initialize robot model
00108         robot_model_services::MILDRobotModelWithExactIK *myRobotModel = new robot_model_services::MILDRobotModelWithExactIK();
00109         robot_model_services::MILDRobotModelWithExactIKPtr myRobotModelPtr(myRobotModel);
00110         double position_x = -0.113948535919;
00111         double position_y = -1.60322499275;
00112         double rotationAngle = -1.57;
00113 
00114         robot_model_services::MILDRobotState * startState = new robot_model_services::MILDRobotState(0,0,rotationAngle,position_x,position_y);
00115         robot_model_services::MILDRobotStatePtr startStatePtr(startState);
00116 
00117         //Publish current pose
00118         ros::NodeHandle n;
00119         ros::Publisher pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1000);
00120         geometry_msgs::PoseWithCovarianceStamped startPoseStamped = geometry_msgs::PoseWithCovarianceStamped();
00121         geometry_msgs::PoseWithCovariance startPose = geometry_msgs::PoseWithCovariance();
00122         SimpleQuaternion rotation = ZXZ2Quaternion(0, 0, rotationAngle*180/M_PI);
00123         startPose.pose.position.x = position_x;
00124         startPose.pose.position.y = position_y;
00125         startPose.pose.position.z = 0;
00126         startPose.pose.orientation.w = rotation.w();
00127         startPose.pose.orientation.x = rotation.x();
00128         startPose.pose.orientation.y = rotation.y();
00129         startPose.pose.orientation.z = rotation.z();
00130         startPoseStamped.pose = startPose;
00131         startPoseStamped.header.stamp = ros::Time::now();
00132         pose_pub.publish(startPoseStamped);
00133 
00134 
00135         myRobotModelPtr->setPanAngleLimits(-60, 60);
00136         myRobotModelPtr->setTiltAngleLimits(-180, 180);
00137         ROS_INFO_STREAM("Running test...");
00138         ros::spinOnce();
00139         for (unsigned int i = 0; i < targetCameraPositions.size(); i++)
00140         {
00141             ROS_INFO_STREAM("Testpose " << (i+1));
00142             SimpleVector3 currentPosition = targetCameraPositions[i];
00143             SimpleQuaternion currentOrientation = targetCameraOrientations[i];
00144             ROS_INFO_STREAM("Calculating inverse kinematics...");
00145             robot_model_services::RobotStatePtr newStatePtr = myRobotModelPtr->calculateRobotState(startStatePtr, currentPosition, currentOrientation);
00146             //ROS_INFO_STREAM("Calculating camera pose correction...");
00147             //robot_model_services::PTUConfig ptuConfig = myRobotModelPtr->calculateCameraPoseCorrection(startStatePtr, currentPosition, currentOrientation);
00148             //ROS_INFO_STREAM("Got pan = " << std::get<0>(ptuConfig)*180/M_PI << ", tilt = " << std::get<1>(ptuConfig)*180/M_PI);
00149             ros::spinOnce();
00150             waitForEnter();
00151             ros::Duration(2).sleep();
00152         }
00153         ROS_INFO_STREAM("All tests done.");
00154         }
00155 };
00156 
00157 test_suite* init_unit_test_suite( int argc, char* argv[] ) {
00158         ros::init(argc, argv, "nbv_test");
00159     ros::start();
00160 
00161         test_suite* evaluation = BOOST_TEST_SUITE("Evaluation NBV");
00162 
00163     boost::shared_ptr<IKTest> testPtr(new IKTest());
00164 
00165     evaluation->add(BOOST_CLASS_TEST_CASE(&IKTest::cameraPoseTest, testPtr));
00166 
00167     framework::master_test_suite().add(evaluation);
00168 
00169     return 0;
00170 }
00171