Nix pendant to pr 465

binding/python/mc_rbdyn/mc_rbdyn.pxd

@@ -89,11 +89,14 @@ cdef class CylindricalSurface(Surface):
 cdef CylindricalSurface CylindricalSurfaceFromPtr(c_mc_rbdyn.CylindricalSurface*)
 
 cdef class Contact(object):
+  cdef shared_ptr[c_mc_rbdyn.Contact] ptr
   cdef c_mc_rbdyn.Contact * impl
   cdef cppbool own_impl__
 
 cdef Contact ContactFromC(const c_mc_rbdyn.Contact &, cppbool copy=?)
 
+cdef Contact ContactFromPtr(shared_ptr[c_mc_rbdyn.Contact], cppbool copy=?)
+
 cdef class ContactVector(object):
   cdef vector[c_mc_rbdyn.Contact] * v
   cdef cppbool own_impl__

binding/python/mc_rbdyn/mc_rbdyn.pyx

@@ -1191,6 +1191,18 @@ cdef Contact ContactFromC(const c_mc_rbdyn.Contact& c, cppbool copy=True):
   else:
     ret.own_impl__ = False
     ret.impl = &(c_mc_rbdyn.const_cast_contact(c))
+    ret.ptr = shared_ptr[c_mc_rbdyn.Contact]()
+  return ret
+
+cdef Contact ContactFromPtr(shared_ptr[c_mc_rbdyn.Contact] ptr, cppbool copy=True):
+  cdef Contact ret = Contact(skip_alloc = True)
+  if copy:
+    ret.impl = new c_mc_rbdyn.Contact(deref(ptr.get()))
+    ret.own_impl__ = True
+  else:
+    ret.ptr = ptr
+    ret.impl = ptr.get()
+    ret.own_impl__ = False
   return ret
 
 cdef class ContactVector(object):

binding/python/mc_solver/c_mc_solver.pxd

@@ -74,7 +74,7 @@ cdef extern from "<mc_solver/TasksQPSolver.h>" namespace "mc_solver":
   cdef cppclass QPSolver:
     void addConstraintSet(const ConstraintSet&)
     void removeConstraintSet(const ConstraintSet&)
-    const vector[c_mc_rbdyn.Contact] & contacts()
+    const vector[shared_ptr[c_mc_rbdyn.Contact]] & contacts()
     void setContacts(const vector[c_mc_rbdyn.Contact]&)
     void addTask(c_qp.Task *)
     void addTask(c_mc_tasks.MetaTask *)

binding/python/mc_solver/mc_solver.pyx

@@ -174,7 +174,7 @@ cdef class QPSolver(object):
   def contacts(self):
     ret = []
     for i in range(self.impl.contacts().size()):
-      ret.append(mc_rbdyn.ContactFromC(self.impl.contacts()[i]))
+      ret.append(mc_rbdyn.ContactFromPtr(self.impl.contacts()[i], copy=False))
     return ret
   def addTask(self, mc_tasks.MetaTask task):
     self.impl.addTask(task.mt_base)

include/mc_control/Contact.h

@@ -31,8 +31,10 @@ struct MC_CONTROL_DLLAPI Contact
           const std::string & r1Surface,
           const std::string & r2Surface,
           double friction = mc_rbdyn::Contact::defaultFriction,
-          const Eigen::Vector6d & dof = Eigen::Vector6d::Ones())
-  : r1(r1), r2(r2), r1Surface(r1Surface), r2Surface(r2Surface), friction(friction), dof(dof)
+          const Eigen::Vector6d & dof = Eigen::Vector6d::Ones(),
+          std::optional<mc_rbdyn::FeasiblePolytope> feasiblePolytope = std::nullopt)
+  : r1(r1), r2(r2), r1Surface(r1Surface), r2Surface(r2Surface), friction(friction), dof(dof),
+    feasiblePolytope(feasiblePolytope)
   {
   }
 
@@ -42,6 +44,7 @@ struct MC_CONTROL_DLLAPI Contact
   std::string r2Surface;
   mutable double friction;
   mutable Eigen::Vector6d dof;
+  mutable std::optional<mc_rbdyn::FeasiblePolytope> feasiblePolytope;
 
   bool operator==(const Contact & rhs) const
   {

include/mc_control/MCController.h

@@ -264,15 +264,19 @@ struct MC_CONTROL_DLLAPI MCController
 
   /** Add a contact between two robots
    *
-   * No effect if the contact is already present.
+   * No effect if the contact is already present and hasn't changed.
    *
+   * Appropriate constraints will be updated/recreated as needed if contact properties changed (dof, friction,
+   * feasibility polytopes, etc)
+   *
+   * \param c Contact to add
+   * \param show When true print information
    */
-  void addContact(const Contact & c);
+  void addContact(const Contact & c, bool show = true);
 
   /** Remove a contact between two robots
    *
-   * No effect if the contact is already absent.
-   *
+   * No effect if the contact does not exist
    */
   void removeContact(const Contact & c);
 

include/mc_rbdyn/Contact.h

@@ -8,9 +8,12 @@
 #include <mc_rbdyn/api.h>
 #include <mc_rtc/Configuration.h>
 
+#include <mc_tvm/fwd.h>
+
 #include <Tasks/QPContacts.h>
 
 #include <SpaceVecAlg/SpaceVecAlg>
+#include <optional>
 
 #include <memory>
 
@@ -43,12 +46,18 @@ MC_RBDYN_DLLAPI std::vector<sva::PTransformd> computePoints(const mc_rbdyn::Surf
                                                             const mc_rbdyn::Surface & envSurface,
                                                             const sva::PTransformd & X_es_rs);
 
+struct FeasiblePolytope
+{
+  Eigen::MatrixXd planeNormals;
+  Eigen::VectorXd planeConstants;
+};
+
 struct ContactImpl;
 
 struct MC_RBDYN_DLLAPI Contact
 {
 public:
-  constexpr static int nrConeGen = 4;
+  constexpr static int nrConeGen = 4; // FIXME: use it in tvmqpsolver...
   constexpr static double defaultFriction = 0.7;
   constexpr static unsigned int nrBilatPoints = 4;
 
@@ -124,6 +133,23 @@ struct MC_RBDYN_DLLAPI Contact
   /** Set the contact friction */
   void friction(double friction);
 
+  void feasiblePolytopeR1(const FeasiblePolytope & polytope);
+  const std::optional<FeasiblePolytope> & feasiblePolytopeR1() const noexcept;
+
+  void feasiblePolytopeR2(const FeasiblePolytope & polytope);
+  const std::optional<FeasiblePolytope> & feasiblePolytopeR2() const noexcept;
+
+  /** Get the TVM polytope associated to robot 1 of this contact
+   *
+   * FIXME Returns a non-const reference from a const method because it is most often used to register dependencies
+   * between TVM nodes which require non-const objects
+   */
+  mc_tvm::FeasiblePolytope & tvmPolytopeR1() const;
+
+  /** Get the TVM polytope associated to robot 2 of this contact
+   */
+  mc_tvm::FeasiblePolytope & tvmPolytopeR2() const;
+
   std::pair<std::string, std::string> surfaces() const;
 
   sva::PTransformd X_0_r1s(const mc_rbdyn::Robot & robot) const;
@@ -164,6 +190,20 @@ struct MC_RBDYN_DLLAPI Contact
                                       const sva::PTransformd & X_b1_b2,
                                       const std::vector<sva::PTransformd> & points) const;
 
+  //! If present superseeds friction cone constraints
+  // feasible polytope for r1 of contact
+  std::optional<FeasiblePolytope> feasiblePolytopeR1_;
+
+  // feasible polytope for r2 of contact
+  std::optional<FeasiblePolytope> feasiblePolytopeR2_;
+
+  // Secures threaded access to the contact object
+  mutable std::mutex contactMutex_;
+
+  /* mutable to allow initialization in const method */
+  mutable mc_tvm::PolytopePtr tvm_polytopeR1_;
+  mutable mc_tvm::PolytopePtr tvm_polytopeR2_;
+
 public:
   static mc_rbdyn::Contact load(const mc_rbdyn::Robots & robots, const mc_rtc::Configuration & config);
 

include/mc_rbdyn/Robot.h

@@ -632,7 +632,12 @@ struct MC_RBDYN_DLLAPI Robot
   const Eigen::Vector3d & zmpTarget() const;
 
   /** Compute and returns the mass of the robot */
-  inline double mass() const noexcept { return mass_; }
+  inline double mass() const noexcept
+  {
+    double mass = 0.;
+    for(const auto & b : mb().bodies()) { mass += b.inertia().mass(); }
+    return mass;
+  }
 
   /** @name Joint sensors
    *

include/mc_rbdyn/fwd.h

@@ -30,4 +30,7 @@ using ConstFramePtr = std::shared_ptr<const Frame>;
 
 struct ForceSensor;
 
+struct Contact;
+using ContactPtr = std::shared_ptr<Contact>;
+
 } // namespace mc_rbdyn

include/mc_rtc/logging.h

@@ -52,36 +52,114 @@ void critical(Args &&... args)
   details::cerr().critical(std::forward<Args>(args)...);
 }
 
+/**
+ * Print an error message
+ *
+ * Displayed in red on terminals with color support
+ *
+ * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string
+ */
 template<typename... Args>
 void error(Args &&... args)
 {
   details::cerr().error(std::forward<Args>(args)...);
 }
 
+/**
+ * Print optional error message
+ */
+template<typename... Args>
+void error(bool print, Args &&... args)
+{
+  if(print) { error(std::forward<Args>(args)...); }
+}
+
+/**
+ * Print a warning message
+ *
+ * Displayed in yellow on terminals with color support
+ *
+ * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string
+ */
 template<typename... Args>
 void warning(Args &&... args)
 {
   details::cerr().warn(std::forward<Args>(args)...);
 }
 
+/**
+ * Print optional warning message
+ */
+template<typename... Args>
+void warning(bool print, Args &&... args)
+{
+  if(print) { warning(std::forward<Args>(args)...); }
+}
+
+/**
+ * Print an information message
+ *
+ * Displayed in blue on terminals with color support
+ *
+ * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string
+ */
 template<typename... Args>
 void info(Args &&... args)
 {
   details::info().info(std::forward<Args>(args)...);
 }
 
+/**
+ * Print optional information message
+ */
+template<typename... Args>
+void info(bool print, Args &&... args)
+{
+  if(print) { info(std::forward<Args>(args)...); }
+}
+
+/**
+ * Print a success message
+ *
+ * Displayed in green on terminals with color support
+ *
+ * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string
+ */
 template<typename... Args>
 void success(Args &&... args)
 {
   details::success().info(std::forward<Args>(args)...);
 }
 
+/**
+ * Print optional success message
+ */
+template<typename... Args>
+void success(bool print, Args &&... args)
+{
+  if(print) { success(std::forward<Args>(args)...); }
+}
+
+/**
+ * Displays a system notification
+ *
+ * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string
+ */
 template<typename... Args>
 void notify(Args &&... args)
 {
   details::notify(fmt::format(std::forward<Args>(args)...));
 }
 
+/**
+ * Displays an optional system notification (libnotify, WinToast, etc.)
+ */
+template<typename... Args>
+void notify(bool print, Args &&... args)
+{
+  if(print) { notify(std::forward<Args>(args)...); }
+}
+
 } // namespace log
 
 } // namespace mc_rtc

include/mc_solver/QPSolver.h

@@ -218,7 +218,7 @@ struct MC_SOLVER_DLLAPI QPSolver
   virtual void setContacts(ControllerToken, const std::vector<mc_rbdyn::Contact> & contacts) = 0;
 
   /** Returns the current set of contacts */
-  inline const std::vector<mc_rbdyn::Contact> & contacts() const noexcept { return contacts_; }
+  inline const std::vector<std::shared_ptr<mc_rbdyn::Contact>> & contacts() const noexcept { return contacts_; }
 
   /** Returns the current set of constraints */
   inline const std::vector<mc_solver::ConstraintSet *> & constraints() const noexcept { return constraints_; }
@@ -285,7 +285,7 @@ struct MC_SOLVER_DLLAPI QPSolver
   std::shared_ptr<mc_rtc::Logger> logger() const;
 
   /** Set the GUI helper for this solver instance */
-  void gui(std::shared_ptr<mc_rtc::gui::StateBuilder> gui);
+  virtual void gui(std::shared_ptr<mc_rtc::gui::StateBuilder> gui);
   /** Access to the gui instance */
   std::shared_ptr<mc_rtc::gui::StateBuilder> gui() const;
 
@@ -303,7 +303,7 @@ struct MC_SOLVER_DLLAPI QPSolver
   double timeStep;
 
   /** Holds mc_rbdyn::Contact in the solver */
-  std::vector<mc_rbdyn::Contact> contacts_;
+  std::vector<std::shared_ptr<mc_rbdyn::Contact>> contacts_;
 
   /** Holds MetaTask currently in the solver */
   std::vector<mc_tasks::MetaTask *> metaTasks_;