contact_sequence.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_CONTACT_SEQUENCE_HPP_
#define ROBOTOC_CONTACT_SEQUENCE_HPP_ 
 
#include <deque>
#include <iostream>
#include <memory>
#include <cassert>
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/se3.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/robot/impact_status.hpp"
#include "robotoc/planner/discrete_event.hpp"
 
 
namespace robotoc {
 
class ContactSequence {
public:
  ContactSequence(const Robot& robot, const int reserved_num_discrete_events=0);
 
  ContactSequence();
 
  ~ContactSequence() = default;
 
  ContactSequence(const ContactSequence&) = default;
 
  ContactSequence& operator=(const ContactSequence&) = default;
 
  ContactSequence(ContactSequence&&) noexcept = default;
 
  ContactSequence& operator=(ContactSequence&&) noexcept = default;
 
  void init(const ContactStatus& contact_status);
 
  void push_back(const DiscreteEvent& discrete_event, const double event_time,
                 const bool sto=false);
 
  void push_back(const ContactStatus& contact_status, 
                 const double switching_time, const bool sto=false);
 
  void pop_back();
 
  void pop_front();
 
  void setImpactTime(const int impact_index, const double impact_time);
 
  void setLiftTime(const int lift_index, const double lift_time);
 
  bool isSTOEnabledImpact(const int impact_index) const;
 
  bool isSTOEnabledLift(const int lift_index) const;
 
  bool isEventTimeConsistent() const;
 
  void setContactPlacements(
      const int contact_phase, 
      const std::vector<Eigen::Vector3d>& contact_positions);
 
  void setContactPlacements(
      const int contact_phase, 
      const std::vector<Eigen::Vector3d>& contact_positions,
      const std::vector<Eigen::Matrix3d>& contact_rotations);
 
  void setContactPlacements(const int contact_phase, 
                            const aligned_vector<SE3>& contact_placements);
 
  void setFrictionCoefficients(const int contact_phase, 
                               const std::vector<double>& friction_coefficients);
 
  int numContactPhases() const {
    return contact_statuses_.size();
  }
 
  int numDiscreteEvents() const {
    return (numContactPhases()-1);
  }
 
  int numImpactEvents() const {
    return impact_events_.size();
  }
 
  int numLiftEvents() const {
    return (numDiscreteEvents()-numImpactEvents());
  }
 
  const ContactStatus& contactStatus(const int contact_phase) const {
    assert(contact_phase >= 0);
    assert(contact_phase < numContactPhases());
    return contact_statuses_[contact_phase];
  }
 
  const ImpactStatus& impactStatus(const int impact_index) const {
    assert(impact_index >= 0);
    assert(impact_index < numImpactEvents());
    return impact_events_[impact_index].impactStatus();
  }
 
  double impactTime(const int impact_index) const {
    assert(impact_index >= 0);
    assert(impact_index < numImpactEvents());
    return impact_time_[impact_index];
  }
 
  double liftTime(const int lift_index) const {
    assert(lift_index >= 0);
    assert(lift_index < numLiftEvents());
    return lift_time_[lift_index];
  }
 
  DiscreteEventType eventType(const int event_index) const {
    assert(event_index >= 0);
    assert(event_index < numImpactEvents()+numLiftEvents());
    if (is_impact_event_[event_index]) return DiscreteEventType::Impact;
    else return DiscreteEventType::Lift;
  }
 
  const std::deque<double>& eventTimes() const {
    return event_time_;
  }
 
  void reserve(const int reserved_num_discrete_events);
 
  int reservedNumDiscreteEvents() const;
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const ContactSequence& contact_sequence);
 
  friend std::ostream& operator<<(
      std::ostream& os, 
      const std::shared_ptr<ContactSequence>& contact_sequence);
 
private:
  int reserved_num_discrete_events_;
  ContactStatus default_contact_status_;
  std::deque<ContactStatus> contact_statuses_;
  std::deque<DiscreteEvent> impact_events_;
  std::deque<int> event_index_impact_, event_index_lift_;
  std::deque<double> event_time_, impact_time_, lift_time_;
  std::deque<bool> is_impact_event_, sto_impact_, sto_lift_;
 
  void clear();
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_CONTACT_SEQUENCE_HPP_