contact_status.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONTACT_STATUS_HPP_
#define ROBOTOC_CONTACT_STATUS_HPP_
 
#include <vector>
#include <string>
#include <unordered_map>
#include <iostream>
 
#include "Eigen/Core"
 
#include "robotoc/robot/se3.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/utils/aligned_unordered_map.hpp"
 
 
namespace robotoc {
 
enum class ContactType {
  PointContact,
  SurfaceContact
};
 
 
class ContactStatus {
public:
  ContactStatus(const std::vector<ContactType>& contact_types, 
                const std::vector<std::string>& contact_frame_names,
                const double default_friction_coefficient=0.7);
 
  ContactStatus();
 
  ~ContactStatus() = default;
 
  ContactStatus(const ContactStatus&) = default;
 
  ContactStatus& operator=(const ContactStatus&) = default;
 
  ContactStatus(ContactStatus&&) noexcept = default;
 
  ContactStatus& operator=(ContactStatus&&) noexcept = default;
 
  bool operator==(const ContactStatus& other) const;
 
  bool operator!=(const ContactStatus& other) const;
 
  ContactType contactType(const int contact_index) const;
 
  const std::vector<ContactType>& contactTypes() const;
 
  const std::string& contactFrameName(const int contact_index) const;
 
  const std::vector<std::string>& contactFrameNames() const;
 
  bool isContactActive(const int contact_index) const;
 
  bool isContactActive(const std::string& contact_frame_name) const;
 
  const std::vector<bool>& isContactActive() const;
 
  bool hasActiveContacts() const;
 
  int dimf() const;
 
  int maxNumContacts() const;
 
  void activateContact(const int contact_index);
 
  void activateContact(const std::string& contact_frame_name);
 
  void deactivateContact(const int contact_index);
 
  void deactivateContact(const std::string& contact_frame_name);
 
  void activateContacts(const std::vector<int>& contact_indices);
 
  void activateContacts(const std::vector<std::string>& contact_frame_names);
 
  void deactivateContacts(const std::vector<int>& contact_indices);
 
  void deactivateContacts(const std::vector<std::string>& contact_frame_names);
 
  void setContactPlacement(const int contact_index, 
                           const Eigen::Vector3d& contact_position);
 
  void setContactPlacement(const std::string& contact_frame_name, 
                           const Eigen::Vector3d& contact_position);
 
  void setContactPlacement(const int contact_index, 
                           const Eigen::Vector3d& contact_position, 
                           const Eigen::Matrix3d& contact_rotation);
 
  void setContactPlacement(const std::string& contact_frame_name, 
                           const Eigen::Vector3d& contact_position, 
                           const Eigen::Matrix3d& contact_rotation);
 
  void setContactPlacement(const int contact_index, 
                           const SE3& contact_placement);
 
  void setContactPlacement(const std::string& contact_frame_name, 
                           const SE3& contact_placement);
 
  void setContactPlacements(
      const std::vector<Eigen::Vector3d>& contact_positions);
 
  void setContactPlacements(
      const std::unordered_map<std::string, Eigen::Vector3d>& contact_positions);
 
  void setContactPlacements(
      const std::vector<Eigen::Vector3d>& contact_positions,
      const std::vector<Eigen::Matrix3d>& contact_rotations);
 
  void setContactPlacements(
      const std::unordered_map<std::string, Eigen::Vector3d>& contact_positions,
      const std::unordered_map<std::string, Eigen::Matrix3d>& contact_rotations);
 
  void setContactPlacements(const aligned_vector<SE3>& contact_placements);
 
  void setContactPlacements(
      const aligned_unordered_map<std::string, SE3>& contact_placements);
 
  const SE3& contactPlacement(const int contact_index) const;
 
  const SE3& contactPlacement(const std::string& contact_frame_name) const;
 
  const Eigen::Vector3d& contactPosition(const int contact_index) const;
 
  const Eigen::Vector3d& contactPosition(
      const std::string& contact_frame_name) const;
 
  const Eigen::Matrix3d& contactRotation(const int contact_index) const;
 
  const Eigen::Matrix3d& contactRotation(
      const std::string& contact_frame_name) const;
 
  const aligned_vector<SE3>& contactPlacements() const;
 
  const std::vector<Eigen::Vector3d>& contactPositions() const;
 
  const std::vector<Eigen::Matrix3d>& contactRotations() const;
 
  void setFrictionCoefficient(const int contact_index, 
                              const double friction_coefficient);
 
  void setFrictionCoefficient(const std::string& contact_frame_name, 
                              const double friction_coefficient);
 
  void setFrictionCoefficients(const std::vector<double>& friction_coefficients);
 
  void setFrictionCoefficients(
      const std::unordered_map<std::string, double>& friction_coefficients);
 
  double frictionCoefficient(const int contact_index) const;
 
  double frictionCoefficient(const std::string& contact_frame_name) const;
 
  const std::vector<double>& frictionCoefficients() const;
 
  int findContactIndex(const std::string& contact_frame_name) const;
 
  void setRandom();
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const ContactStatus& contact_status);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  std::vector<ContactType> contact_types_;
  std::vector<std::string> contact_frame_names_;
  std::vector<bool> is_contact_active_;
  aligned_vector<SE3> contact_placements_;
  std::vector<Eigen::Vector3d> contact_positions_;
  std::vector<Eigen::Matrix3d> contact_rotations_;
  std::vector<double> friction_coefficients_;
  int dimf_, max_contacts_, max_num_contacts_;
  bool has_active_contacts_;
 
  void setHasActiveContacts();
 
};
 
} // namespace robotoc
 
#include "robotoc/robot/contact_status.hxx"
 
#endif // ROBOTOC_CONTACT_STATUS_HPP_