avoid_obstacles.h

Go to the documentation of this file.

#ifndef AVOID_OBSTACLES_H_
#define AVOID_OBSTACLES_H_

#include "constrained_ik/constraint.h"
#include "constrained_ik/constrained_ik.h"
#include <industrial_collision_detection/collision_detection/collision_common.h>
#include <vector>
#include <algorithm>
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacsolver.hpp>

namespace constrained_ik
{
namespace constraints
{

class AvoidObstacles: public Constraint
{
protected:
  struct LinkAvoidance
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LinkAvoidance(std::string link_name);

    LinkAvoidance();

    virtual ~LinkAvoidance() {delete jac_solver_;}

    double weight_; 
    double min_distance_; 
    double avoidance_distance_; 
    double amplitude_; 
    int num_robot_joints_; 
    int num_obstacle_joints_; 
    std::string link_name_; 
    KDL::Chain avoid_chain_; 
    int num_inboard_joints_; 
    KDL::Vector link_point_; 
    KDL::ChainJntToJacSolver * jac_solver_; 
    KDL::Vector obstacle_point_; 
  };

  std::map<std::string, LinkAvoidance> links_; 
  std::vector<std::string> link_names_; 
  std::set<const robot_model::LinkModel *> link_models_; 
  double distance_threshold_; 
  LinkAvoidance* getLinkData(const std::string link_name)
  {
    std::map<std::string, LinkAvoidance>::iterator it;
    it = links_.find(link_name);
    if(it != links_.end())
    {
      return &(it->second);
    }
    else
    {
      ROS_WARN_STREAM("Failed to retrieve avoidance data for link: " << link_name);
      return NULL;
    }
  }

  const LinkAvoidance * const getLinkData(const std::string link_name) const
  {
    std::map<std::string, LinkAvoidance>::const_iterator it;
    it = links_.find(link_name);
    if(it != links_.end())
    {
      return &(it->second);
    }
    else
    {
      ROS_WARN_STREAM("Failed to retrieve avoidance data for link: " << link_name);
      return NULL;
    }
  }

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  struct AvoidObstaclesData: public ConstraintData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    const constraints::AvoidObstacles* parent_; 
    collision_detection::DistanceResult distance_res_; 
    collision_detection::DistanceMap distance_map_; 
    collision_detection::DistanceInfoMap distance_info_map_; 
    AvoidObstaclesData(const constrained_ik::SolverState &state, const constraints::AvoidObstacles* parent);
  };

  AvoidObstacles() {}

  void init(const Constrained_IK * ik) override;

  constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override;

  void loadParameters(const XmlRpc::XmlRpcValue &constraint_xml) override;

  virtual Eigen::MatrixXd calcJacobian(const AvoidObstaclesData &cdata, const LinkAvoidance &link) const;

  virtual Eigen::VectorXd calcError(const AvoidObstaclesData &cdata, const LinkAvoidance &link) const;

  virtual bool checkStatus(const AvoidObstaclesData &cdata, const LinkAvoidance &link) const;

  virtual double getWeight(const std::string &link_name) const
  {
    LinkAvoidance *link;
    if(getLinkData(link_name))
      return link->weight_;
    else
      return -1.0;
  }

  virtual void setWeight(const std::string &link_name, const double &weight)
  {
    LinkAvoidance* link = getLinkData(link_name);
    if(link)
      link->weight_ = weight;
  }

  virtual double getMinDistance(const std::string &link_name) const
  {
    const LinkAvoidance * const link = getLinkData(link_name);
    if(link)
      return link->min_distance_;
    else
      return -1.0;
  }

  virtual void setMinDistance(const std::string &link_name, const double &min_distance)
  {
    LinkAvoidance* link = getLinkData(link_name);
    if(link)
      link->min_distance_ = min_distance;
  }

  virtual double getAmplitude(const std::string &link_name) const
  {
    const LinkAvoidance * const link = getLinkData(link_name);
    if(link)
      return link->amplitude_;
    else
      return -1.0;
  }

  virtual void setAmplitude(const std::string &link_name, const double &amplitude)
  {
    LinkAvoidance* link = getLinkData(link_name);
    if(link)
      link->amplitude_ = amplitude;
  }
  
  virtual double getAvoidanceDistance(const std::string &link_name) const
  {
    const LinkAvoidance * const link = getLinkData(link_name);
    if(link)
      return link->avoidance_distance_;
    else
      return -1.0;
  }

  virtual void setAvoidanceDistance(const std::string &link_name, const double &avoidance_distance)
  {
    LinkAvoidance* link = getLinkData(link_name);
    if(link)
    {
      link->avoidance_distance_ = avoidance_distance;
      updateDistanceThreshold();
    }
  }
  
  virtual std::vector<std::string> getAvoidanceLinkNames() const
  {
    return link_names_;
  }

  virtual void setAvoidanceLinks(const std::vector<std::string> &link_names)
  {
    link_names_ = link_names;
    links_.clear();
    
    for (std::vector<std::string>::const_iterator it = link_names.begin(); it < link_names.end(); ++it)
    {
      links_.insert(std::make_pair(*it, LinkAvoidance(*it)));
      updateDistanceThreshold();
    }
  }

  virtual void addAvoidanceLink(const std::string &link_name)
  {
    if (std::find(link_names_.begin(), link_names_.end(), link_name) == link_names_.end())
    {
      links_.insert(std::make_pair(link_name, LinkAvoidance(link_name)));
      link_names_.push_back(link_name);
      updateDistanceThreshold();
    }
    else
    {
      ROS_WARN("Tried to add an avoidance link that already exist.");
    }
  }

  virtual void updateDistanceThreshold()
  {
    distance_threshold_ = 0;
    for (std::map<std::string, LinkAvoidance>::const_iterator it = links_.begin(); it != links_.end(); it++)
    {
      if (it->second.avoidance_distance_ > distance_threshold_)
      {
        distance_threshold_ = it->second.avoidance_distance_;
      }
    }
  }

};

} /* namespace constraints */
} /* namespace constrained_ik */
#endif /* AVOID_OBSTACLES_H_ */