asr_robot_model_services: MILDRobotModelWithExactIK.hpp Source File

Go to the documentation of this file.
00001 
00020 #pragma once
00021 
00022 #include <boost/tuple/tuple.hpp>
00023 #include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
00024 #include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
00025 #include "robot_model_services/typedef.hpp"
00026 #include "geometry_msgs/Pose.h"
00027 #include <ros/ros.h>
00028 #include <tf/transform_listener.h>
00029 #include "robot_model_services/rating/IKRatingModule.h"
00030 #include "robot_model_services/helper/DebugHelper.hpp"
00031 
00032 namespace robot_model_services {
00040     typedef std::tuple<double,double> PTUConfig;    //Pan and tilt angle of the ptu
00041     class MILDRobotModelWithExactIK : public MILDRobotModel {
00042         private:
00043 
00044          //Maximum number of iterations for the IK angle iteration method
00045          const unsigned int IKVisualizationMaximunIterationCount = 6;
00046 
00047          float mInverseKinematicIterationAccuracy;
00048 
00052          IKRatingModulePtr ikRatingModule;
00053 
00057          double h_tilt;
00058          double viewTriangleZPlane_angleAlpha;
00059          double viewTriangleZPlane_angleGamma;
00060          double viewTriangleZPlane_sideA;
00061          double viewTriangleZPlane_sideB;
00062          double viewTriangleXYPlane_sideC;
00063          unsigned int mPanAngleSamplingStepsPerIteration;
00064          unsigned int mIKVisualizationLastIterationCount = 0;
00065          double mTiltAngleOffset;
00066          double mPanAngleOffset;
00067          double mViewPointDistance;
00068 
00072          bool mVisualizeIK;
00073 
00077          Eigen::Affine3d tiltToCameraEigen;
00078 
00082          Eigen::Affine3d panToTiltEigen;
00083          Eigen::Affine3d tiltToPanEigen;
00084 
00088          Eigen::Affine3d baseToPanEigen;
00089          Eigen::Affine3d panToBaseEigen;
00090 
00091          double x_product;
00092 
00096          bool tfParametersInitialized;
00097 
00098          //For Tests only
00099          int mNumberIKCalls;
00100          double mnTotalIKTime;
00101 
00105          bool setUpTFParameters();
00109          double acosClamped(double value);
00113          double getBaseAngleFromBaseFrame(Eigen::Affine3d &baseFrame);
00117          double getPanAngleFromPanJointPose(Eigen::Affine3d &panJointFrame, MILDRobotStatePtr &robotState);
00122          bool getTiltAngleAndTiltBasePointProjected(Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector,  Eigen::Vector3d &target_view_center_point, double &tilt, Eigen::Vector3d &tilt_base_point_projected);
00126          Eigen::Affine3d getTiltJointFrame(Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector,  Eigen::Vector3d &tilt_base_point);
00130          void visualizeIKcalculation(Eigen::Vector3d &base_point, Eigen::Vector3d &base_orientation, Eigen::Vector3d &pan_joint_point, Eigen::Vector3d & pan_rotated_point, Eigen::Vector3d &tilt_base_point, Eigen::Vector3d &tilt_base_point_projected, Eigen::Vector3d &cam_point, Eigen::Vector3d &actual_view_center_point);
00134          void visualizeCameraPoseCorrection(Eigen::Vector3d &base_point, Eigen::Vector3d &base_orientation, Eigen::Vector3d &pan_joint_point, Eigen::Vector3d & pan_rotated_point, Eigen::Vector3d &tilt_base_point, Eigen::Vector3d &cam_point, Eigen::Vector3d &actual_view_center_point);
00138          void visualizeIKCameraTarget(Eigen::Vector3d &target_view_center_point, Eigen::Vector3d &target_camera_point);
00142          void resetIKVisualization();
00146          void visualizeIKPoint(Eigen::Vector3d &point, Eigen::Vector4d &colorRGBA, std::string ns, int id);
00150          void visualizeIKArrowLarge(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id);
00154          void visualizeIKArrowSmall(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id);
00158          void visualizeIKArrow(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, Eigen::Vector3d &scaleParameters, int id);
00159 
00160     public:
00161 
00165         MILDRobotModelWithExactIK();
00166 
00170         virtual ~MILDRobotModelWithExactIK();
00171 
00172 
00176                 RobotStatePtr calculateRobotState(const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation);
00177 
00181         PTUConfig calculateCameraPoseCorrection(const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation);
00182 
00186         void setViewPointDistance(float viewPointDistance);
00187 
00188         ros::Publisher vis_pub;
00189         };
00190 
00191     typedef boost::shared_ptr<MILDRobotModelWithExactIK> MILDRobotModelWithExactIKPtr;
00192 }