MILDRobotModelWithExactIK.hpp

Go to the documentation of this file.

#pragma once

#include <boost/tuple/tuple.hpp>
#include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
#include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
#include "robot_model_services/typedef.hpp"
#include "geometry_msgs/Pose.h"
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include "robot_model_services/rating/IKRatingModule.h"
#include "robot_model_services/helper/DebugHelper.hpp"

namespace robot_model_services {
    typedef std::tuple<double,double> PTUConfig;    //Pan and tilt angle of the ptu
    class MILDRobotModelWithExactIK : public MILDRobotModel {
        private:

         //Maximum number of iterations for the IK angle iteration method
         const unsigned int IKVisualizationMaximunIterationCount = 6;

         float mInverseKinematicIterationAccuracy;

         IKRatingModulePtr ikRatingModule;

         double h_tilt;
         double viewTriangleZPlane_angleAlpha;
         double viewTriangleZPlane_angleGamma;
         double viewTriangleZPlane_sideA;
         double viewTriangleZPlane_sideB;
         double viewTriangleXYPlane_sideC;
         unsigned int mPanAngleSamplingStepsPerIteration;
         unsigned int mIKVisualizationLastIterationCount = 0;
         double mTiltAngleOffset;
         double mPanAngleOffset;
         double mViewPointDistance;

         bool mVisualizeIK;

         Eigen::Affine3d tiltToCameraEigen;

         Eigen::Affine3d panToTiltEigen;
         Eigen::Affine3d tiltToPanEigen;

         Eigen::Affine3d baseToPanEigen;
         Eigen::Affine3d panToBaseEigen;

         double x_product;

         bool tfParametersInitialized;

         //For Tests only
         int mNumberIKCalls;
         double mnTotalIKTime;

         bool setUpTFParameters();
         double acosClamped(double value);
         double getBaseAngleFromBaseFrame(Eigen::Affine3d &baseFrame);
         double getPanAngleFromPanJointPose(Eigen::Affine3d &panJointFrame, MILDRobotStatePtr &robotState);
         bool getTiltAngleAndTiltBasePointProjected(Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector,  Eigen::Vector3d &target_view_center_point, double &tilt, Eigen::Vector3d &tilt_base_point_projected);
         Eigen::Affine3d getTiltJointFrame(Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector,  Eigen::Vector3d &tilt_base_point);
         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);
         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);
         void visualizeIKCameraTarget(Eigen::Vector3d &target_view_center_point, Eigen::Vector3d &target_camera_point);
         void resetIKVisualization();
         void visualizeIKPoint(Eigen::Vector3d &point, Eigen::Vector4d &colorRGBA, std::string ns, int id);
         void visualizeIKArrowLarge(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id);
         void visualizeIKArrowSmall(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id);
         void visualizeIKArrow(Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, Eigen::Vector3d &scaleParameters, int id);

    public:

        MILDRobotModelWithExactIK();

        virtual ~MILDRobotModelWithExactIK();


                RobotStatePtr calculateRobotState(const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation);

        PTUConfig calculateCameraPoseCorrection(const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation);

        void setViewPointDistance(float viewPointDistance);

        ros::Publisher vis_pub;
        };

    typedef boost::shared_ptr<MILDRobotModelWithExactIK> MILDRobotModelWithExactIKPtr;
}