contact-map.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/context.hpp"
 
namespace aligator {

template <typename _Scalar> struct ContactMapTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using PoseVec = StdVectorEigenAligned<Vector3s>;
 
  ContactMapTpl(const std::vector<std::string> &contact_names,
                const std::vector<bool> &contact_states,
                const PoseVec &contact_poses)
      : contact_names_(contact_names)
      , contact_states_(contact_states)
      , contact_poses_(contact_poses) {
    if (contact_states.size() != contact_poses.size()) {
      ALIGATOR_DOMAIN_ERROR(
          "Contact_states and contact_poses should have same size, "
          "currently ({:d} and {:d}).",
          contact_states.size(), contact_poses.size());
    }
    size_ = contact_states_.size();
  }
 
  void addContact(std::string_view name, const bool state,
                  const Vector3s &pose) {
    contact_names_.emplace_back(name);
    contact_states_.push_back(state);
    contact_poses_.push_back(pose);
    size_ += 1;
  }
 
  void removeContact(const size_t i) {
    if (size_ == 0) {
      ALIGATOR_RUNTIME_ERROR("ContactMap is empty!");
    } else {
      contact_names_.erase(contact_names_.begin() + long(i));
      contact_states_.erase(contact_states_.begin() + long(i));
      contact_poses_.erase(contact_poses_.begin() + long(i));
      size_ -= 1;
    }
  }
 
  bool getContactState(std::string_view name) const {
    auto it = std::find(contact_names_.begin(), contact_names_.end(), name);
    if (it == contact_names_.end()) {
      ALIGATOR_RUNTIME_ERROR("Contact name does not exist in this map!");
    }
 
    const auto index = size_t(it - contact_names_.begin());
    return contact_states_[index];
  }
 
  const Vector3s &getContactPose(std::string_view name) const {
    auto it = std::find(contact_names_.begin(), contact_names_.end(), name);
    if (it == contact_names_.end()) {
      ALIGATOR_RUNTIME_ERROR("Contact name does not exist in this map!");
    }
 
    const auto index = size_t(it - contact_names_.begin());
    return contact_poses_[index];
  }
 
  void setContactPose(std::string_view name, const Vector3s &ref) {
    auto it = std::find(contact_names_.begin(), contact_names_.end(), name);
    if (it == contact_names_.end()) {
      ALIGATOR_RUNTIME_ERROR("Contact name does not exist in this map!");
    }
 
    const auto index = size_t(it - contact_names_.begin());
    contact_poses_[index] = ref;
  }
 
  std::vector<std::string> contact_names_;
  std::vector<bool> contact_states_;
  PoseVec contact_poses_;
  std::size_t size_;
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct ContactMapTpl<context::Scalar>;
#endif
} // namespace aligator