Merge pull request #253 from arntanguy/topic/DoFContact

[mc_control] Properly apply contact DoF when using the deprecated method

binding/python/mc_control/c_mc_control.pxd

@@ -7,7 +7,7 @@ from sva.c_sva cimport *
 from rbdyn.c_rbdyn cimport *
 cimport sch.c_sch as sch
 cimport tasks.qp.c_qp as c_qp
-from mc_rbdyn.c_mc_rbdyn cimport *
+from mc_rbdyn.c_mc_rbdyn cimport RobotModulePtr, Robot, Robots, Collision
 from mc_solver.c_mc_solver cimport *
 cimport mc_observers.c_mc_observers as c_mc_observers
 cimport mc_rtc.c_mc_rtc as c_mc_rtc
@@ -26,6 +26,30 @@ cdef extern from "<memory>" namespace "std" nogil:
     shared_ptr(T*)
     T* get()
 
+cdef extern from "<mc_control/Contact.h>" namespace "mc_control":
+  cdef cppclass Contact:
+    Contact()
+    Contact(const string&, const string&, const string&, const string&, double)
+    Contact(const string&, const string&, const string&, const string&, double, const Vector6d)
+    string r1
+    string r2
+    string r1Surface
+    string r2Surface
+    double friction
+    Vector6d dof
+
+  # Actually std::set<Contact, std::less<Contact>, Eigen::aligned_allocator<Contact>> but Cython only know std::set<T>
+  cdef cppclass ContactSet:
+    cppclass iterator:
+      Contact& operator*()
+      iterator operator++()
+      iterator operator--()
+      bint operator==(iterator)
+      bint operator!=(iterator)
+    iterator begin()
+    iterator end()
+
+
 cdef extern from "<mc_control/mc_controller.h>" namespace "mc_control":
   cdef cppclass ControllerResetData:
     const vector[vector[double]] & q
@@ -54,6 +78,18 @@ cdef extern from "<mc_control/mc_controller.h>" namespace "mc_control":
     c_mc_observers.ObserverPipeline & observerPipeline(const string&)
     vector[c_mc_observers.ObserverPipeline] & observerPipelines()
 
+    void addCollisions(const string&, const string&,
+                       const vector[Collision] &)
+    void removeCollisions(const string&, const string&)
+    void removeCollisions(const string&, const string&,
+                          const vector[Collision] &)
+    cppbool hasRobot(const string&)
+    Robot& robot(const string&)
+    void addContact(const Contact&)
+    void removeContact(const Contact&)
+    const ContactSet & contacts()
+    cppbool hasContact(const Contact &)
+
 cdef extern from "<mc_control/mc_python_controller.h>" namespace "mc_control":
   cdef cppclass PythonRWCallback:
     cppbool success

binding/python/mc_control/fsm/c_fsm.pxd

@@ -33,41 +33,8 @@ cdef extern from "mc_control_fsm_wrapper.hpp":
   run_cb make_run_cb[T,U](T, U)
 
 cdef extern from "<mc_control/fsm/Controller.h>" namespace "mc_control::fsm":
-  cdef cppclass Contact:
-    Contact(const string&, const string&, const string&, const string&, double)
-    Contact(const string&, const string&, const string&, const string&, double, const c_eigen.Vector6d)
-    Contact()
-    string r1
-    string r2
-    string r1Surface
-    string r2Surface
-    double friction
-    c_eigen.Vector6d dof
-
-  # Actually std::set<Contact, std::less<Contact>, Eigen::aligned_allocator<Contact>> but Cython only know std::set<T>
-  cdef cppclass ContactSet:
-    cppclass iterator:
-      Contact& operator*()
-      iterator operator++()
-      iterator operator--()
-      bint operator==(iterator)
-      bint operator!=(iterator)
-    iterator begin()
-    iterator end()
-
   cdef cppclass Controller(c_mc_control.MCController):
-    void addCollisions(const string&, const string&,
-                       const vector[c_mc_rbdyn.Collision] &)
-    void removeCollisions(const string&, const string&)
-    void removeCollisions(const string&, const string&,
-                          const vector[c_mc_rbdyn.Collision] &)
-    cppbool hasRobot(const string&)
-    c_mc_rbdyn.Robot& robot(const string&)
     shared_ptr[c_mc_tasks.PostureTask] getPostureTask(const string&)
-    void addContact(const Contact&)
-    void removeContact(const Contact&)
-    const ContactSet & contacts()
-    cppbool hasContact(const Contact &)
     c_mc_solver.ContactConstraint & contactConstraint()
 
 cdef extern from "<mc_control/fsm/State.h>" namespace "mc_control::fsm":

binding/python/mc_control/fsm/fsm.pxd

@@ -6,11 +6,6 @@ cimport c_fsm
 
 from mc_control.mc_control cimport MCController
 
-cdef class Contact(object):
-  cdef c_fsm.Contact impl
-
-cdef Contact ContactFromC(const c_fsm.Contact&)
-
 cdef class Controller(MCController):
   cdef c_fsm.Controller * impl
 

binding/python/mc_control/fsm/fsm.pyx

@@ -14,6 +14,7 @@ cimport mc_rbdyn.mc_rbdyn as mc_rbdyn
 cimport mc_solver.mc_solver as mc_solver
 
 from mc_control.mc_control cimport MCController
+cimport mc_control.mc_control as mc_control
 
 from cython.operator cimport dereference as deref
 
@@ -22,115 +23,11 @@ from libcpp.set cimport set as cppset
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 
-cdef class Contact(object):
-  def __ctor__(self, r1, r2, r1Surface, r2Surface, friction = mc_rbdyn.Contact.defaultFriction, eigen.Vector6d dof = None):
-    if isinstance(r1, unicode):
-      r1 = r1.encode(u'ascii')
-    if isinstance(r1Surface, unicode):
-      r1Surface = r1Surface.encode(u'ascii')
-    if isinstance(r2, unicode):
-      r2 = r2.encode(u'ascii')
-    if isinstance(r2Surface, unicode):
-      r2Surface = r2Surface.encode(u'ascii')
-    if dof is None:
-      self.impl = c_fsm.Contact(r1, r2, r1Surface, r2Surface, friction)
-    else:
-      self.impl = c_fsm.Contact(r1, r2, r1Surface, r2Surface, friction, dof.impl)
-  def __cinit__(self, *args):
-    if len(args) > 0:
-      self.__ctor__(*args)
-  property r1:
-    def __get__(self):
-      return self.impl.r1
-    def __set__(self, r1):
-      if isinstance(r1, unicode):
-        r1 = r1.encode(u'ascii')
-      self.impl.r1 = r1
-  property r1Surface:
-    def __get__(self):
-      return self.impl.r1Surface
-    def __set__(self, r1Surface):
-      if isinstance(r1Surface, unicode):
-        r1Surface = r1Surface.encode(u'ascii')
-      self.impl.r1Surface = r1Surface
-  property r2:
-    def __get__(self):
-      return self.impl.r2
-    def __set__(self, r2):
-      if isinstance(r2, unicode):
-        r2 = r2.encode(u'ascii')
-      self.impl.r2 = r2
-  property r2Surface:
-    def __get__(self):
-      return self.impl.r2Surface
-    def __set__(self, r2Surface):
-      if isinstance(r2Surface, unicode):
-        r2Surface = r2Surface.encode(u'ascii')
-      self.impl.r2Surface = r2Surface
-  property friction:
-    def __get__(self):
-      return self.impl.friction
-    def __set__(self, friction):
-      self.impl.friction = friction
-  property dof:
-    def __get__(self):
-      return eigen.Vector6dFromC(self.impl.dof)
-    def __set__(self, dof):
-      if isinstance(dof, eigen.Vector6d):
-        self.impl.dof = (<eigen.Vector6d>dof).impl
-      else:
-        self.dof = eigen.Vector6d(dof)
-
-cdef Contact ContactFromC(const c_fsm.Contact & c):
-  cdef Contact ret = Contact()
-  ret.impl = c
-  return ret
+Contact = mc_control.Contact
 
 cdef class Controller(MCController):
   def __cinit__(self):
     self.impl = self.base = NULL
-  def addCollisions(self, r1, r2, collisions):
-    assert(all([isinstance(col, mc_rbdyn.Collision) for col in collisions]))
-    cdef vector[c_mc_rbdyn.Collision] cols
-    if isinstance(r1, unicode):
-      r1 = r1.encode(u'ascii')
-    if isinstance(r2, unicode):
-      r2 = r2.encode(u'ascii')
-    for col in collisions:
-      cols.push_back((<mc_rbdyn.Collision>col).impl)
-    self.impl.addCollisions(r1, r2, cols)
-  def removeCollisions(self, r1, r2, collisions = None):
-    cdef vector[c_mc_rbdyn.Collision] cols
-    if isinstance(r1, unicode):
-      r1 = r1.encode(u'ascii')
-    if isinstance(r2, unicode):
-      r2 = r2.encode(u'ascii')
-    if collisions is None:
-      self.impl.removeCollisions(r1, r2)
-    else:
-      for col in collisions:
-        cols.push_back((<mc_rbdyn.Collision>col).impl)
-      self.impl.removeCollisions(r1, r2, cols)
-  def hasRobot(self, name):
-    if isinstance(name, unicode):
-      name = name.encode(u'ascii')
-    return self.impl.hasRobot(name)
-  def robot(self, name = None):
-    if isinstance(name, unicode):
-      name = name.encode(u'ascii')
-    if name is None:
-      return MCController.robot(self)
-    else:
-      return mc_rbdyn.RobotFromC(self.impl.robot(name))
-  def addContact(self, Contact c):
-    self.impl.addContact(c.impl)
-  def removeContact(self, Contact c):
-    self.impl.removeContact(c.impl)
-  def contacts(self):
-    cdef c_fsm.ContactSet cs = self.impl.contacts()
-    return [ContactFromC(c) for c in cs]
-  def hasContact(self, Contact c):
-    self.impl.hasContact(c.impl)
   def contactConstraint(self):
     return mc_solver.ContactConstraintFromPtr(&(self.impl.contactConstraint()))
 

binding/python/mc_control/mc_control.pxd

@@ -11,6 +11,11 @@ cdef class ControllerResetData(object):
 
 cdef ControllerResetData ControllerResetDataFromPtr(c_mc_control.ControllerResetData *)
 
+cdef class Contact(object):
+  cdef c_mc_control.Contact impl
+
+cdef Contact ContactFromC(const c_mc_control.Contact&)
+
 cdef public api class MCController(object)[object MCControllerObject, type MCControllerType]:
   cdef c_mc_control.MCController * base
 

binding/python/mc_control/mc_control.pyx

@@ -50,6 +50,71 @@ cdef ControllerResetData ControllerResetDataFromPtr(c_mc_control.ControllerReset
     ret.impl = p
     return ret
 
+cdef class Contact(object):
+  def __ctor__(self, r1, r2, r1Surface, r2Surface, friction = mc_rbdyn.Contact.defaultFriction, eigen.Vector6d dof = None):
+    if isinstance(r1, unicode):
+      r1 = r1.encode(u'ascii')
+    if isinstance(r1Surface, unicode):
+      r1Surface = r1Surface.encode(u'ascii')
+    if isinstance(r2, unicode):
+      r2 = r2.encode(u'ascii')
+    if isinstance(r2Surface, unicode):
+      r2Surface = r2Surface.encode(u'ascii')
+    if dof is None:
+      self.impl = c_mc_control.Contact(r1, r2, r1Surface, r2Surface, friction)
+    else:
+      self.impl = c_mc_control.Contact(r1, r2, r1Surface, r2Surface, friction, dof.impl)
+  def __cinit__(self, *args):
+    if len(args) > 0:
+      self.__ctor__(*args)
+  property r1:
+    def __get__(self):
+      return self.impl.r1
+    def __set__(self, r1):
+      if isinstance(r1, unicode):
+        r1 = r1.encode(u'ascii')
+      self.impl.r1 = r1
+  property r1Surface:
+    def __get__(self):
+      return self.impl.r1Surface
+    def __set__(self, r1Surface):
+      if isinstance(r1Surface, unicode):
+        r1Surface = r1Surface.encode(u'ascii')
+      self.impl.r1Surface = r1Surface
+  property r2:
+    def __get__(self):
+      return self.impl.r2
+    def __set__(self, r2):
+      if isinstance(r2, unicode):
+        r2 = r2.encode(u'ascii')
+      self.impl.r2 = r2
+  property r2Surface:
+    def __get__(self):
+      return self.impl.r2Surface
+    def __set__(self, r2Surface):
+      if isinstance(r2Surface, unicode):
+        r2Surface = r2Surface.encode(u'ascii')
+      self.impl.r2Surface = r2Surface
+  property friction:
+    def __get__(self):
+      return self.impl.friction
+    def __set__(self, friction):
+      self.impl.friction = friction
+  property dof:
+    def __get__(self):
+      return eigen.Vector6dFromC(self.impl.dof)
+    def __set__(self, dof):
+      if isinstance(dof, eigen.Vector6d):
+        self.impl.dof = (<eigen.Vector6d>dof).impl
+      else:
+        self.dof = eigen.Vector6d(dof)
+
+cdef Contact ContactFromC(const c_mc_control.Contact & c):
+  cdef Contact ret = Contact()
+  ret.impl = c
+  return ret
+
+
 cdef class MCController(object):
   def __cinit__(self):
     pass
@@ -113,6 +178,56 @@ cdef class MCController(object):
       ret.append(mc_observers.ObserverPipelineFromRef(deref(it)))
       preinc(it)
     return ret
+  def addCollisions(self, r1, r2, collisions):
+    assert(all([isinstance(col, mc_rbdyn.Collision) for col in collisions]))
+    cdef vector[c_mc_rbdyn.Collision] cols
+    if isinstance(r1, unicode):
+      r1 = r1.encode(u'ascii')
+    if isinstance(r2, unicode):
+      r2 = r2.encode(u'ascii')
+    for col in collisions:
+      cols.push_back((<mc_rbdyn.Collision>col).impl)
+    self.base.addCollisions(r1, r2, cols)
+  def removeCollisions(self, r1, r2, collisions = None):
+    cdef vector[c_mc_rbdyn.Collision] cols
+    if isinstance(r1, unicode):
+      r1 = r1.encode(u'ascii')
+    if isinstance(r2, unicode):
+      r2 = r2.encode(u'ascii')
+    if collisions is None:
+      self.base.removeCollisions(r1, r2)
+    else:
+      for col in collisions:
+        cols.push_back((<mc_rbdyn.Collision>col).impl)
+      self.base.removeCollisions(r1, r2, cols)
+  def hasRobot(self, name):
+    if isinstance(name, unicode):
+      name = name.encode(u'ascii')
+    return self.base.hasRobot(name)
+  def robot(self, name = None):
+    if isinstance(name, unicode):
+      name = name.encode(u'ascii')
+    if name is None:
+      return MCController.robot(self)
+    else:
+      return mc_rbdyn.RobotFromC(self.base.robot(name))
+  def addContact(self, c, *args):
+    if isinstance(c, Contact):
+      assert len(args) == 0, "addContact takes either an mc_control.Contact object or arguments for its construction"
+      self.base.addContact((<Contact>c).impl)
+    else:
+      self.addContact(Contact(c, *args))
+  def removeContact(self, c, *args):
+    if isinstance(c, Contact):
+      assert len(args) == 0, "removeContact takes either an mc_control.Contact object or arguments for its construction"
+      self.base.removeContact((<Contact>c).impl)
+    else:
+      self.removeContact(Contact(c, *args))
+  def contacts(self):
+    cdef c_mc_control.ContactSet cs = self.base.contacts()
+    return [ContactFromC(c) for c in cs]
+  def hasContact(self, Contact c):
+    self.base.hasContact(c.impl)
 
 cdef MCController MCControllerFromPtr(c_mc_control.MCController * p):
     cdef MCController ret = MCController()