ObstacleAvoidancePotential.cpp

Go to the documentation of this file.

/*
 * ObstacleAvoidancePotential.cpp
 *
 *  Created on: Dec 14, 2011
 *      Author: mriedel
 */

#include <obs_avoidance/ObstacleAvoidancePotential.hpp>

namespace TELEKYB_NAMESPACE {

ObstacleAvoidancePotentialOptions::ObstacleAvoidancePotentialOptions(const std::string& identifier_)
        : OptionContainer(identifier_)
{
        tObsAPUavSemiDims = addOption("tObsAPUavSemiDims",
                        "Semi-axes of an ellipsoid approximation the uav.", Vector3D(0.4,0.4,0.15), false, false);
//      tObsAPRepulMaxDist = addOption("tObsAPRepulMaxDist",
//                      "Max Distance to Obstacle Point, so that it is considered.", 0.6, false, false);
//      tObsAPRepulMinDist = addOption("tObsAPRepulMinDist",
//                      "Min Distance to Obstacle Point, so that it is considered. Afterwards it has a constant influence",
//                      0.1, false, false);
//      tObsAPRepulGain = addOption("tObsAPRepulGain",
//                      "Gain for Obstacle Potential", 0.5, false, false);
//      tObsAPSaturationVel = addOption<double>("tObsAPSaturationVel",
//                      "Velocity Saturation if Distance < tObsAPRepulMinDist", 4.0, false, false);
//      tObsAPSaturationAcc = addOption<double>("tObsAPSaturationAcc",
//                      "Acceleration Saturation if Distance < tObsAPRepulMinDist", 6.0, false, false);

}

ObstacleAvoidancePotential::ObstacleAvoidancePotential(const std::string& identifier_)
        : options(identifier_),
          obstaclePotentialGradient(NULL)
{
        // Add Default Options to RawOptionContainer
        std::string gradientIdentifier = identifier_ + "_gradient";
        telekyb::RawOptionsContainer::addOption(gradientIdentifier + "/tPotFuncZeroD","0.5");
        telekyb::RawOptionsContainer::addOption(gradientIdentifier + "/tPotFuncInfD","0.2");
        telekyb::RawOptionsContainer::addOption(gradientIdentifier + "/tPotFuncSatValue","4.0");
        telekyb::RawOptionsContainer::addOption(gradientIdentifier + "/tPotFuncGain","100.0");

        // Note: Reads Parameters from RawOptionContainer!
        obstaclePotentialGradient = new CoTanRepulsiveGradient(gradientIdentifier);

}

ObstacleAvoidancePotential::~ObstacleAvoidancePotential() {
        if (obstaclePotentialGradient) {
                delete obstaclePotentialGradient;
        }
}

double ObstacleAvoidancePotential::getEffectiveUAVDistance(const Position3D& relObstaclePositon) const {
        // The clearance of a uav is modelled as an ellipsoid
        // TODO this ellipsoid should take care of the uav attitude
        //Position3D relObstaclePositon = -positionDifference;
        double azimuth = R3Helper::azimuth(relObstaclePositon);
        double zenith  = R3Helper::zenith(relObstaclePositon);
        Position3D uavSemiDims = options.tObsAPUavSemiDims->getValue();
        double xClearance = uavSemiDims(0) * sin(zenith) * cos(azimuth);
        double yClearance = uavSemiDims(1) * sin(zenith) * sin(azimuth);
        double zClearance = uavSemiDims(2) * cos(zenith);
        double uavClearance = sqrt(xClearance*xClearance + yClearance*yClearance + zClearance*zClearance);

        return relObstaclePositon.norm() - uavClearance;
}

Velocity3D ObstacleAvoidancePotential::getObstacleVelocity(const Position3D& position, const std::vector< Position3D >& obstaclePoints) {
        Velocity3D accumVelocity(0.0,0.0,0.0);


        //int obsNum = input.obsPositions.size();
        for(unsigned int i = 0; i < obstaclePoints.size(); i++){
                Position3D positionDifference = position - obstaclePoints[i];
                double d = getEffectiveUAVDistance(-positionDifference);
                accumVelocity += obstaclePotentialGradient->getPotential(d) * positionDifference.normalized();
//              ROS_WARN("Distance: %f", d);
        }


//      if (accumVelocity.norm() > 0.01) {
//              ROS_WARN("Calculated Obstaclevelocity: (%f,%f,%f)", accumVelocity(0),accumVelocity(1),accumVelocity(2));
//      }
        return accumVelocity;
}

//Acceleration3D ObstacleAvoidancePotential::getObstacleAcceleration(const Position3D& position, const std::vector< Position3D >& obstaclePoints) {
//      Acceleration3D accumAcceleration(0.0,0.0,0.0);
//
//      for(unsigned int i = 0; i < obstaclePoints.size(); i++){
//              Position3D positionDifference = position - obstaclePoints[i];
//              double d = getEffectiveUAVDistance(-positionDifference);
//              accumAcceleration += obstaclePotentialHassian.getPotential(d) * positionDifference.normalized();
//      }
//
//
//      if (accumAcceleration.norm() > 0.01) {
//              ROS_WARN("Calculated ObstacleAcceleration: (%f,%f,%f)", accumAcceleration(0),accumAcceleration(1),accumAcceleration(2));
//      }
//      return accumAcceleration;
//
//}

} /* namespace telekyb */