SimpleIKRatingModule.cpp

Go to the documentation of this file.

#include "robot_model_services/rating/impl/SimpleIKRatingModule.h"
#include <ros/ros.h>

namespace robot_model_services {
    SimpleIKRatingModule::SimpleIKRatingModule() : IKRatingModule() {}
    SimpleIKRatingModule::~SimpleIKRatingModule() { }

    double SimpleIKRatingModule::getPanAngleRating(const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition, double sourceRotationBase, double targetRotationBase)
    {
        //Substract the difference between source and target rotation
        double rotDiff = fmod(targetRotationBase - sourceRotationBase, 2.0*M_PI);
        if (rotDiff <= 0) {rotDiff += 2.0*M_PI;}
        rotDiff = std::min(fabs(rotDiff), (double)(2.0f*M_PI-fabs(rotDiff)));
        double rating = pow(0.6, rotDiff);
        return rating;
    }
}