CollisionsConstraint.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/ConstraintSet.h>
 
#include <mc_rbdyn/Collision.h>
 
#include <mc_rtc/gui/StateBuilder.h>
#include <mc_rtc/void_ptr.h>
 
namespace mc_solver
{
 
struct QPSolver;
 
struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
{
public:
  constexpr static double defaultDampingOffset = 0.1;
 
public:
  CollisionsConstraint(const mc_rbdyn::Robots & robots, unsigned int r1Index, unsigned int r2Index, double timeStep);
 
  bool removeCollision(QPSolver & solver, const std::string & b1Name, const std::string & b2Name);
 
  void removeCollisions(QPSolver & solver, const std::vector<mc_rbdyn::Collision> & cols);
 
  bool removeCollisionByBody(QPSolver & solver, const std::string & byName, const std::string & b2Name);
 
  void addCollision(QPSolver & solver, const mc_rbdyn::Collision & col);
 
  void addCollisions(QPSolver & solver, const std::vector<mc_rbdyn::Collision> & cols);
 
  bool hasCollision(const std::string & c1, const std::string & c2) const noexcept;
 
  void reset();
 
  inline bool automaticMonitor() const noexcept { return autoMonitor_; }
 
  inline void automaticMonitor(bool a) noexcept { autoMonitor_ = a; }
 
  void addToSolverImpl(QPSolver & solver) override;
 
  void update(QPSolver & solver) override;
 
  void removeFromSolverImpl(QPSolver & solver) override;
 
public:
  mc_rtc::void_ptr constraint_;
  unsigned int r1Index;
  unsigned int r2Index;
  std::vector<mc_rbdyn::Collision> cols;
 
private:
  /* Internal sauce to manage collisions */
  int collId;
  std::map<std::string, std::pair<int, mc_rbdyn::Collision>> collIdDict;
  std::string __keyByNames(const std::string & name1, const std::string & name2);
  int __createCollId(const mc_rbdyn::Collision & col);
  std::pair<int, mc_rbdyn::Collision> __popCollId(const std::string & name1, const std::string & name2);
  void __addCollision(mc_solver::QPSolver & solver, const mc_rbdyn::Collision & col);
 
  /* Internal management for collision display */
  bool autoMonitor_ = true;
  std::unordered_set<int> monitored_;
  std::shared_ptr<mc_rtc::gui::StateBuilder> gui_;
  std::vector<std::string> category_;
  void addMonitorButton(int collId, const mc_rbdyn::Collision & col);
  void toggleCollisionMonitor(int collId, const mc_rbdyn::Collision * col = nullptr);
};
 
} // namespace mc_solver