[c++] Add method to set contact normal in TaskCop

include/tsid/bindings/python/tasks/task-cop-equality.hpp

@@ -49,6 +49,7 @@ namespace tsid
         .def(bp::init<std::string, robots::RobotWrapper &> ((bp::arg("name"), bp::arg("robot")), "Default Constructor"))
         .add_property("dim", &TaskCOP::dim, "return dimension size")
         .def("setReference", &TaskCOPEqualityPythonVisitor::setReference, bp::arg("ref"))
+        .def("setContactNormal", &TaskCOPEqualityPythonVisitor::setContactNormal, bp::arg("normal"))
         .def("compute", &TaskCOPEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
         .def("getConstraint",  &TaskCOPEqualityPythonVisitor::getConstraint)
         .add_property("name", &TaskCOPEqualityPythonVisitor::name)
@@ -70,6 +71,9 @@ namespace tsid
       static void setReference(TaskCOP & self, const Eigen::Vector3d & ref){
         self.setReference(ref);
       }
+      static void setContactNormal(TaskCOP & self, const Eigen::Vector3d & n){
+        self.setContactNormal(n);
+      }
       static void expose(const std::string & class_name)
       {
         std::string doc = "TaskCOPEqualityPythonVisitor info.";
@@ -77,8 +81,6 @@ namespace tsid
                           doc.c_str(),
                           bp::no_init)
         .def(TaskCOPEqualityPythonVisitor<TaskCOP>());
-
-        // bp::register_ptr_to_python< boost::shared_ptr<math::ConstraintBase> >();
       }
     };
   }

include/tsid/tasks/task-cop-equality.hpp

@@ -66,6 +66,9 @@ namespace tsid
       void setReference(const Vector3 & ref);
       const Vector3 & getReference() const;
 
+      void setContactNormal(const Vector3 & n);
+      const Vector3 & getContactNormal() const;
+
     protected:
       const std::vector<std::shared_ptr<ContactLevel> > *m_contacts;
       std::string m_contact_name; // an empty string

src/tasks/task-com-equality.cpp

@@ -58,7 +58,7 @@ namespace tsid
 
     int TaskComEquality::dim() const
     {
-      return m_mask.sum();
+      return int(m_mask.sum());
     }
 
     const Vector3 & TaskComEquality::Kp(){ return m_Kp; }

src/tasks/task-cop-equality.cpp

@@ -110,5 +110,15 @@ const Vector3 & TaskCopEquality::getReference() const
   return m_ref;
 }
 
+void TaskCopEquality::setContactNormal(const Vector3 & n)
+{
+  m_normal = n;
+}
+
+const Vector3 & TaskCopEquality::getContactNormal() const
+{
+  return m_normal;
+}
+
 }
 }