Program Listing for File abstract_ik.hpp

↰ Return to documentation for file (include/bitbots_splines/abstract_ik.hpp)

#ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_
#define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_

#include <moveit/robot_model/robot_model.h>

#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>

namespace bitbots_splines {

typedef std::pair<std::vector<std::string>, std::vector<double>> JointGoals;

inline JointGoals joint_goals_update(const JointGoals &goals, const std::vector<std::string> &names,
                                     const std::vector<double> &values, rclcpp::Logger logger) {
  JointGoals result = goals;

  if (names.size() != values.size()) {
    RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes");
  }

  for (size_t i = 0; i < goals.first.size(); ++i) {
    for (size_t j = 0; j < names.size() && j < values.size(); ++j) {
      if (result.first.at(i) == names.at(j)) {
        result.second.at(i) = values.at(j);
      }
    }
  }

  return result;
}

inline JointGoals joint_goals_update_diff(const JointGoals &goals, const std::vector<std::string> &names,
                                          const std::vector<double> &diffs, rclcpp::Logger logger) {
  JointGoals result = goals;

  if (names.size() != diffs.size()) {
    RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes");
  }

  for (size_t i = 0; i < goals.first.size(); ++i) {
    for (size_t j = 0; j < names.size() && j < diffs.size(); ++j) {
      if (result.first.at(i) == names.at(j)) {
        result.second.at(i) += diffs.at(j);
      }
    }
  }

  return result;
}

template <typename Positions>
class AbstractIK {
  virtual void init(moveit::core::RobotModelPtr kinematic_model) = 0;
  virtual JointGoals calculate(const Positions &positions) = 0;
  virtual void reset() = 0;
};
}  // namespace bitbots_splines

#endif  // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_