Add implementation of TaskContactForceEquality (#141)

* [ForceTask] Add bindings and set task for contactPoint (3D), use Contact6D, put dt as an input in the constructor & add feedforward, add leak_rate parameter, change constructor & methods to take the contact
Authored-by: Noëlie Ramuzat <noelie.ramuzat@laas.fr>

bindings/python/tasks/task-contact-force-equality.cpp

+//
+// Copyright (c) 2021 LAAS-CNRS, University of Trento
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/bindings/python/tasks/task-contact-force-equality.hpp"
+#include "tsid/bindings/python/tasks/expose-tasks.hpp"
+
+namespace tsid
+{
+  namespace python
+  {
+    void exposeTaskContactForceEquality()
+    {
+      TaskContactForceEqualityPythonVisitor<tsid::tasks::TaskContactForceEquality>::expose("TaskContactForceEquality");
+    }
+  }
+}

include/tsid/bindings/python/tasks/task-contact-force-equality.hpp

+//
+// Copyright (c) 2021 LAAS-CNRS, University of Trento
+//
+// This file is part of tsid
+// tsid is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// tsid is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __tsid_python_task_contact_force_equality_hpp__
+#define __tsid_python_task_contact_force_equality_hpp__
+
+#include "tsid/bindings/python/fwd.hpp"
+
+#include "tsid/tasks/task-contact-force-equality.hpp"
+#include "tsid/robots/robot-wrapper.hpp"
+#include "tsid/trajectories/trajectory-base.hpp"
+#include "tsid/math/constraint-equality.hpp"
+#include "tsid/math/constraint-base.hpp"
+namespace tsid
+{
+  namespace python
+  {    
+    namespace bp = boost::python;
+
+    template<typename TaskContactForceEquality>
+    struct TaskContactForceEqualityPythonVisitor
+    : public boost::python::def_visitor< TaskContactForceEqualityPythonVisitor<TaskContactForceEquality> >
+    {
+      
+      template<class PyClass>     
+      
+
+      void visit(PyClass& cl) const
+      {
+        cl
+        .def(bp::init<std::string, robots::RobotWrapper &, double, contacts::ContactBase &> ((bp::arg("name"), bp::arg("robot"), bp::arg("dt"), bp::arg("contact")), "Default Constructor"))
+        .add_property("dim", &TaskContactForceEquality::dim, "return dimension size")
+        .def("setReference", &TaskContactForceEqualityPythonVisitor::setReference, bp::arg("ref"))
+        .def("setExternalForce", &TaskContactForceEqualityPythonVisitor::setExternalForce, bp::arg("f_ext"))
+        .def("compute", &TaskContactForceEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
+        .def("getConstraint",  &TaskContactForceEqualityPythonVisitor::getConstraint)
+        .add_property("name", &TaskContactForceEqualityPythonVisitor::name)
+        .add_property("Kp", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Kd", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("Ki", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
+        .add_property("getLeakRate", bp::make_function(&TaskContactForceEqualityPythonVisitor::getLeakRate, bp::return_value_policy<bp::copy_const_reference>()))
+        .def("setKp", &TaskContactForceEqualityPythonVisitor::setKp, bp::arg("Kp"))
+        .def("setKd", &TaskContactForceEqualityPythonVisitor::setKd, bp::arg("Kd"))
+        .def("setKi", &TaskContactForceEqualityPythonVisitor::setKi, bp::arg("Ki"))
+        .def("setLeakRate", &TaskContactForceEqualityPythonVisitor::setLeakRate, bp::arg("leak"))
+        ;
+      }
+      static std::string name(TaskContactForceEquality & self){
+        std::string name = self.name();
+        return name;
+      }
+      static math::ConstraintEquality compute(TaskContactForceEquality & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
+        self.compute(t, q, v, data);
+        math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
+        return cons;
+      }
+      static math::ConstraintEquality getConstraint(const TaskContactForceEquality & self){
+        math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
+        return cons;
+      }
+      static void setReference(TaskContactForceEquality & self, trajectories::TrajectorySample & ref){
+        self.setReference(ref);
+      }
+      static void setExternalForce(TaskContactForceEquality & self, trajectories::TrajectorySample & f_ext){
+        self.setExternalForce(f_ext);
+      }
+      static const Eigen::VectorXd & Kp (TaskContactForceEquality & self){
+        return self.Kp();
+      }
+      static const Eigen::VectorXd & Kd (TaskContactForceEquality & self){
+        return self.Kd();
+      }
+      static const Eigen::VectorXd & Ki (TaskContactForceEquality & self){
+        return self.Ki();
+      }
+      static const double & getLeakRate (TaskContactForceEquality & self){
+        return self.getLeakRate();
+      }
+      static void setKp (TaskContactForceEquality & self, const::Eigen::VectorXd Kp){
+        return self.Kp(Kp);
+      }
+      static void setKd (TaskContactForceEquality & self, const::Eigen::VectorXd Kd){
+        return self.Kd(Kd);
+      }
+      static void setKi (TaskContactForceEquality & self, const::Eigen::VectorXd Ki){
+        return self.Ki(Ki);
+      }
+      static void setLeakRate (TaskContactForceEquality & self, const double leak){
+        return self.setLeakRate(leak);
+      }
+      static void expose(const std::string & class_name)
+      {
+        std::string doc = "TaskContactForceEqualityPythonVisitor info.";
+        bp::class_<TaskContactForceEquality>(class_name.c_str(),
+                          doc.c_str(),
+                          bp::no_init)
+        .def(TaskContactForceEqualityPythonVisitor<TaskContactForceEquality>());
+      }
+    };
+  }
+}
+
+
+#endif // ifndef __tsid_python_task_contact_force_equality_hpp__

include/tsid/tasks/task-contact-force-equality.hpp

@@ -15,15 +15,89 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#ifndef __invdyn_tasks_fwd_hpp__
-#define __invdyn_tasks_fwd_hpp__
+#ifndef __invdyn_task_contact_force_equality_hpp__
+#define __invdyn_task_contact_force_equality_hpp__
+
+#include "tsid/math/fwd.hpp"
+#include "tsid/tasks/task-contact-force.hpp"
+#include "tsid/trajectories/trajectory-base.hpp"
+#include "tsid/math/constraint-equality.hpp"
+#include "tsid/contacts/contact-base.hpp"
 
 namespace tsid
 {
   namespace tasks
   {
-    
+
+    class TaskContactForceEquality : public TaskContactForce
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      typedef math::Index Index;
+      typedef trajectories::TrajectorySample TrajectorySample;
+      typedef math::Vector Vector;
+      typedef math::Vector6 Vector6;
+      typedef math::Vector3 Vector3;
+      typedef math::ConstraintEquality ConstraintEquality;
+      typedef pinocchio::SE3 SE3;
+
+      TaskContactForceEquality(const std::string & name,
+                      		     RobotWrapper & robot,
+                               const double dt,
+                      		     contacts::ContactBase & contact);
+
+      int dim() const;
+
+      virtual const std::string& getAssociatedContactName();
+      virtual const contacts::ContactBase& getAssociatedContact();
+      void setAssociatedContact(contacts::ContactBase & contact);
+
+      // Task expressed as a PID between the reference force and the external one
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     Data & data);
+
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     Data & data,
+                                     const std::vector<std::shared_ptr<ContactLevel> >  *contacts);
+
+      const ConstraintBase & getConstraint() const;
+
+      void setReference(TrajectorySample & ref);
+      const TrajectorySample & getReference() const;
+
+      void setExternalForce(TrajectorySample & f_ext);
+      const TrajectorySample & getExternalForce() const;
+
+      const Vector & Kp() const;
+      const Vector & Kd() const;
+      const Vector & Ki() const;
+      const double & getLeakRate() const;
+      void Kp(ConstRefVector Kp);
+      void Kd(ConstRefVector Kp);
+      void Ki(ConstRefVector Ki);
+      void setLeakRate(double leak);
+
+    protected:
+      // contact associated to the force task
+      contacts::ContactBase * m_contact;
+      std::string m_contact_name; // the associated contact name or an empty string
+      ConstraintEquality m_constraint;
+      TrajectorySample m_ref;      // reference Force 6D to follow 
+      TrajectorySample m_fext;     // external Force 6D in the same frame than the ref
+      Vector m_forceIntegralError; // Integral error of the PID
+	    Vector m_Kp;
+      Vector m_Kd;
+      Vector m_Ki;
+      double m_dt;
+      double m_leak_rate;
+    };
+
   }
 }
 
-#endif // ifndef __invdyn_tasks_fwd_hpp__
+#endif // ifndef __invdyn_task_contact_force_equality_hpp__

src/tasks/task-contact-force-equality.cpp

@@ -17,12 +17,135 @@
 
 #include <Eigen/Dense>
 #include <pinocchio/multibody/model.hpp>
+#include "tsid/tasks/task-contact-force-equality.hpp"
 
-namespace tsid
+namespace tsid {
+namespace tasks {
+
+using namespace tsid::math;
+using namespace std;
+
+TaskContactForceEquality::TaskContactForceEquality(const std::string & name, RobotWrapper & robot,
+                                                   const double dt, contacts::ContactBase & contact):
+  TaskContactForce(name, robot),
+  m_contact(&contact),
+  m_constraint(name, 6, 12),  
+  m_ref(6,6),
+  m_fext(6,6) {
+  m_forceIntegralError = Vector::Zero(6);
+  m_dt = dt;
+  m_leak_rate = 0.05;
+  m_contact_name = m_contact->name();
+}
+
+int TaskContactForceEquality::dim() const {
+  return 6;
+}
+
+const Vector & TaskContactForceEquality::Kp() const { return m_Kp; }
+const Vector & TaskContactForceEquality::Kd() const { return m_Kd; }
+const Vector & TaskContactForceEquality::Ki() const { return m_Ki; }
+const double & TaskContactForceEquality::getLeakRate() const { return m_leak_rate; }
+
+void TaskContactForceEquality::Kp(ConstRefVector Kp)
+{
+  assert(Kp.size()==6);
+  m_Kp = Kp;
+}
+
+void TaskContactForceEquality::Kd(ConstRefVector Kd)
 {
-  namespace tasks
-  {
-    
-    
+  assert(Kd.size()==6);
+  m_Kd = Kd;
+}
+
+void TaskContactForceEquality::Ki(ConstRefVector Ki)
+{
+  assert(Ki.size()==6);
+  m_Ki = Ki;
+}
+
+void TaskContactForceEquality::setLeakRate(double leak)
+{
+  m_leak_rate = leak;
+}
+
+const std::string& TaskContactForceEquality::getAssociatedContactName() {
+  return m_contact_name;
+}
+
+const contacts::ContactBase& TaskContactForceEquality::getAssociatedContact() {
+  return *m_contact;
+}
+
+void TaskContactForceEquality::setAssociatedContact(contacts::ContactBase & contact) {
+  m_contact = &contact;
+  m_contact_name = m_contact->name();
+}
+
+void TaskContactForceEquality::setReference(TrajectorySample & ref) {
+  m_ref = ref;
+}
+
+const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getReference() const {
+  return m_ref;
+}
+
+void TaskContactForceEquality::setExternalForce(TrajectorySample & f_ext) {
+  m_fext = f_ext;
+}
+
+const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getExternalForce() const {
+  return m_fext;
+}
+
+
+const ConstraintBase & TaskContactForceEquality::compute(const double t,
+                                                         ConstRefVector q,
+                                                         ConstRefVector v,
+                                                         Data & data,
+                                                         const std::vector<std::shared_ptr<ContactLevel> >  *contacts) {
+  bool contactFound = false;
+  if (m_contact_name != "") {
+    // look if the associated contact is in the list of contact
+    for(auto cl : *contacts) {
+      if (m_contact_name == cl->contact.name()) {
+        contactFound = true;
+        break;
+      }
+    }
+  } else {
+    std::cout << "[TaskContactForceEquality] ERROR: Contact name empty" << std::endl;
+    return m_constraint;
+  }
+  if (!contactFound) {
+    std::cout << "[TaskContactForceEquality] ERROR: Contact name not in the list of contact in the formulation pb" << std::endl;
+    return m_constraint;
   }
+  return compute(t, q, v, data);
+}
+
+const ConstraintBase & TaskContactForceEquality::compute(const double,
+                                                         ConstRefVector,
+                                                         ConstRefVector,
+                                                         Data & data) {
+
+  auto& M = m_constraint.matrix();
+  M = m_contact->getForceGeneratorMatrix(); // 6x12 for a 6d contact
+
+  Vector forceError = m_ref.getValue() - m_fext.getValue();
+  Vector f_ref = m_ref.getValue() + m_Kp.cwiseProduct(forceError) + m_Kd.cwiseProduct(m_ref.getDerivative() - m_fext.getDerivative()) 
+                 + m_Ki.cwiseProduct(m_forceIntegralError);
+  m_constraint.vector() = f_ref;
+
+  m_forceIntegralError += (forceError - m_leak_rate * m_forceIntegralError) * m_dt;
+
+  return m_constraint;
+}
+
+const ConstraintBase & TaskContactForceEquality::getConstraint() const {
+  return m_constraint;
+}
+
+}
 }