[c++] Add CoP task with python bindings.

CMakeLists.txt

@@ -107,6 +107,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
   include/tsid/tasks/task-com-equality.hpp
   include/tsid/tasks/task-se3-equality.hpp
   include/tsid/tasks/task-contact-force-equality.hpp
+  include/tsid/tasks/task-cop-equality.hpp
   include/tsid/tasks/task-actuation-equality.hpp
   include/tsid/tasks/task-actuation-bounds.hpp
   include/tsid/tasks/task-joint-bounds.hpp
@@ -150,6 +151,7 @@ SET(${PROJECT_NAME}_ROBOTS_HEADERS
   )
 
 SET(${PROJECT_NAME}_FORMULATIONS_HEADERS
+  include/tsid/formulations/contact-level.hpp
   include/tsid/formulations/inverse-dynamics-formulation-base.hpp
   include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
   )
@@ -198,6 +200,7 @@ SET(${PROJECT_NAME}_TASKS_SOURCES
   src/tasks/task-com-equality.cpp
   src/tasks/task-contact-force-equality.cpp
   src/tasks/task-contact-force.cpp
+  src/tasks/task-cop-equality.cpp
   src/tasks/task-joint-bounds.cpp
   src/tasks/task-joint-posture.cpp
   src/tasks/task-joint-posVelAcc-bounds.cpp
@@ -232,6 +235,7 @@ SET(${PROJECT_NAME}_ROBOTS_SOURCES
   )
 
 SET(${PROJECT_NAME}_FORMULATIONS_SOURCES
+  src/formulations/contact-level.cpp
   src/formulations/inverse-dynamics-formulation-base.cpp
   src/formulations/inverse-dynamics-formulation-acc-force.cpp
   )

bindings/python/tasks/task-cop-equality.cpp

+//
+// Copyright (c) 2021 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-cop-equality.hpp"
+#include "tsid/bindings/python/tasks/expose-tasks.hpp"
+
+namespace tsid
+{
+  namespace python
+  {
+    void exposeTaskCopEquality()
+    {
+      TaskCOPEqualityPythonVisitor<tsid::tasks::TaskCopEquality>::expose("TaskCopEquality");
+    }
+  }
+}

include/tsid/bindings/python/formulations/formulation.hpp

@@ -31,6 +31,7 @@
 #include "tsid/tasks/task-joint-posture.hpp"
 #include "tsid/tasks/task-se3-equality.hpp"
 #include "tsid/tasks/task-com-equality.hpp"
+#include "tsid/tasks/task-cop-equality.hpp"
 #include "tsid/tasks/task-actuation-bounds.hpp"
 #include "tsid/tasks/task-joint-bounds.hpp"
 #include "tsid/tasks/task-angular-momentum-equality.hpp"
@@ -62,6 +63,7 @@ namespace tsid
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM, bp::args("task", "weight", "priorityLevel", "transition duration"))
+        .def("addForceTask", &InvDynPythonVisitor::addForceTask_COP, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
         .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
         .def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeights, bp::args("contact_name", "force_regularization_weight"))
@@ -102,6 +104,9 @@ namespace tsid
       static bool addMotionTask_AM(T & self, tasks::TaskAMEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
         return self.addMotionTask(task, weight, priorityLevel, transition_duration);
       }
+      static bool addForceTask_COP(T & self, tasks::TaskCopEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
+        return self.addForceTask(task, weight, priorityLevel, transition_duration);
+      }
       static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
         return self.addActuationTask(task, weight, priorityLevel, transition_duration);
       }

include/tsid/bindings/python/tasks/expose-tasks.hpp

@@ -19,6 +19,7 @@
 #define __tsid_python_expose_tasks_hpp__
 
 #include "tsid/bindings/python/tasks/task-com-equality.hpp"
+#include "tsid/bindings/python/tasks/task-cop-equality.hpp"
 #include "tsid/bindings/python/tasks/task-se3-equality.hpp"
 #include "tsid/bindings/python/tasks/task-joint-posture.hpp"
 #include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
@@ -31,6 +32,7 @@ namespace tsid
   namespace python
   {
     void exposeTaskComEquality();
+    void exposeTaskCopEquality();
     void exposeTaskSE3Equality();
     void exposeTaskJointPosture();
     void exposeTaskActuationBounds();
@@ -40,6 +42,7 @@ namespace tsid
     inline void exposeTasks()
     {
       exposeTaskComEquality();
+      exposeTaskCopEquality();
       exposeTaskSE3Equality();
       exposeTaskJointPosture();
       exposeTaskActuationBounds();

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

+//
+// Copyright (c) 2021 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_cop_hpp__
+#define __tsid_python_task_cop_hpp__
+
+#include <pinocchio/fwd.hpp>
+#include <boost/python.hpp>
+#include <string>
+#include <eigenpy/eigenpy.hpp>
+#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+#include "tsid/tasks/task-cop-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 TaskCOP>
+    struct TaskCOPEqualityPythonVisitor
+    : public boost::python::def_visitor< TaskCOPEqualityPythonVisitor<TaskCOP> >
+    {
+      
+      template<class PyClass>     
+      
+
+      void visit(PyClass& cl) const
+      {
+        cl
+        .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("compute", &TaskCOPEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
+        .def("getConstraint",  &TaskCOPEqualityPythonVisitor::getConstraint)
+        .add_property("name", &TaskCOPEqualityPythonVisitor::name)
+        ;
+      }
+      static std::string name(TaskCOP & self){
+        std::string name = self.name();
+        return name;
+      }
+      static math::ConstraintEquality compute(TaskCOP & 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 TaskCOP & self){
+        math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
+        return cons;
+      }
+      static void setReference(TaskCOP & self, const Eigen::Vector3d & ref){
+        self.setReference(ref);
+      }
+      static void expose(const std::string & class_name)
+      {
+        std::string doc = "TaskCOPEqualityPythonVisitor info.";
+        bp::class_<TaskCOP>(class_name.c_str(),
+                          doc.c_str(),
+                          bp::no_init)
+        .def(TaskCOPEqualityPythonVisitor<TaskCOP>());
+
+        // bp::register_ptr_to_python< boost::shared_ptr<math::ConstraintBase> >();
+      }
+    };
+  }
+}
+
+
+#endif // ifndef __tsid_python_task_cop_hpp__

include/tsid/contacts/contact-6d.hpp

@@ -39,7 +39,6 @@ namespace tsid
       typedef math::Vector6 Vector6;
       typedef math::Vector3 Vector3;
       typedef math::Vector Vector;
-      typedef tasks::TaskMotion TaskMotion;
       typedef tasks::TaskSE3Equality TaskSE3Equality;
       typedef math::ConstraintInequality ConstraintInequality;
       typedef math::ConstraintEquality ConstraintEquality;
@@ -87,7 +86,7 @@ namespace tsid
                                                                         ConstRefVector v,
                                                                         const Data & data);
 
-      const TaskMotion & getMotionTask() const;
+      const TaskSE3Equality & getMotionTask() const;
       const ConstraintBase & getMotionConstraint() const;
       const ConstraintInequality & getForceConstraint() const;
       const ConstraintEquality & getForceRegularizationTask() const;
@@ -95,6 +94,7 @@ namespace tsid
       double getNormalForce(ConstRefVector f) const;
       double getMinNormalForce() const;
       double getMaxNormalForce() const;
+      const Matrix3x & getContactPoints() const;
 
       const Vector & Kp() const;
       const Vector & Kd() const;

include/tsid/contacts/contact-base.hpp

@@ -20,7 +20,7 @@
 
 #include "tsid/math/fwd.hpp"
 #include "tsid/robots/fwd.hpp"
-#include "tsid/tasks/task-motion.hpp"
+#include "tsid/tasks/task-se3-equality.hpp"
 
 
 namespace tsid
@@ -41,7 +41,8 @@ namespace tsid
       typedef math::ConstraintEquality ConstraintEquality;
       typedef math::ConstRefVector ConstRefVector;
       typedef math::Matrix Matrix;
-      typedef tasks::TaskMotion TaskMotion;
+      typedef math::Matrix3x Matrix3x;
+      typedef tasks::TaskSE3Equality TaskSE3Equality;
       typedef pinocchio::Data Data;
       typedef robots::RobotWrapper RobotWrapper;
 
@@ -75,7 +76,7 @@ namespace tsid
                                                                         ConstRefVector v,
                                                                         const Data & data) = 0;
 
-      virtual const TaskMotion & getMotionTask() const = 0;
+      virtual const TaskSE3Equality & getMotionTask() const = 0;
       virtual const ConstraintBase & getMotionConstraint() const = 0;
       virtual const ConstraintInequality & getForceConstraint() const = 0;
       virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
@@ -85,6 +86,7 @@ namespace tsid
       virtual bool setMinNormalForce(const double minNormalForce) = 0;
       virtual bool setMaxNormalForce(const double maxNormalForce) = 0;
       virtual double getNormalForce(ConstRefVector f) const = 0;
+      virtual const Matrix3x & getContactPoints() const = 0;
 
     protected:
       std::string m_name;

include/tsid/contacts/contact-point.hpp

@@ -38,7 +38,6 @@ namespace tsid
       typedef math::Vector6 Vector6;
       typedef math::Vector3 Vector3;
       typedef math::Vector Vector;
-      typedef tasks::TaskMotion TaskMotion;
       typedef tasks::TaskSE3Equality TaskSE3Equality;
       typedef math::ConstraintInequality ConstraintInequality;
       typedef math::ConstraintEquality ConstraintEquality;
@@ -75,18 +74,19 @@ namespace tsid
                                                                         ConstRefVector v,
                                                                         const Data & data);
 
-      const TaskMotion & getMotionTask() const;
+      const TaskSE3Equality & getMotionTask() const;
       const ConstraintBase & getMotionConstraint() const;
       const ConstraintInequality & getForceConstraint() const;
       const ConstraintEquality & getForceRegularizationTask() const;
       double getMotionTaskWeight() const;
+      const Matrix3x & getContactPoints() const;
 
       double getNormalForce(ConstRefVector f) const;
       double getMinNormalForce() const;
       double getMaxNormalForce() const;
 
-      const Vector & Kp() const;
-      const Vector & Kd() const;
+      const Vector & Kp();  // cannot be const because it set a member variable inside
+      const Vector & Kd();  // cannot be const because it set a member variable inside
       void Kp(ConstRefVector Kp);
       void Kd(ConstRefVector Kp);
 
@@ -123,6 +123,8 @@ namespace tsid
       Vector3 m_contactNormal;
       Vector3 m_fRef;
       Vector3 m_weightForceRegTask;
+      Matrix3x m_contactPoints;
+      Vector m_Kp3, m_Kd3;  // gain vectors to be returned by reference
       double m_mu;
       double m_fMin;
       double m_fMax;

include/tsid/formulations/contact-level.hpp

+//
+// Copyright (c) 2021 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_contact_level_hpp__
+#define __tsid_contact_level_hpp__
+
+#include "tsid/math/fwd.hpp"
+#include "tsid/contacts/contact-base.hpp"
+
+namespace tsid
+{
+
+  /** Data structure collecting information regarding a single contact.
+   * In particular, this structure contains the index of the force corresponding
+   * to this contact in the force vector used as decision variable in the QP.
+   * Moreover it contains all the default constraints associated to a contact for representing
+   * the motion constraints (contact points do not move), the friction cone constraints
+   * and the force regularization cost.
+   */
+  struct ContactLevel
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+    contacts::ContactBase & contact;
+    std::shared_ptr<math::ConstraintBase> motionConstraint;
+    std::shared_ptr<math::ConstraintInequality> forceConstraint;
+    std::shared_ptr<math::ConstraintEquality> forceRegTask;
+    unsigned int index; /// index of 1st element of associated force variable in the force vector
+
+    ContactLevel(contacts::ContactBase & contact);
+  };
+
+}
+
+#endif // ifndef __tsid_contact_level_hpp__

include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp

 //
-// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
+// Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
 //
 // This file is part of tsid
 // tsid is free software: you can redistribute it
@@ -19,38 +19,13 @@
 #define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
 
 #include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
+#include "tsid/formulations/contact-level.hpp"
 #include "tsid/math/constraint-equality.hpp"
 
 #include <vector>
 
 namespace tsid
 {
-  class TaskLevel
-  {
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    tasks::TaskBase & task;
-    std::shared_ptr<math::ConstraintBase> constraint;
-    unsigned int priority;
-
-    TaskLevel(tasks::TaskBase & task,
-              unsigned int priority);
-  };
-
-  class ContactLevel
-  {
-  public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    contacts::ContactBase & contact;
-    std::shared_ptr<math::ConstraintBase> motionConstraint;
-    std::shared_ptr<math::ConstraintInequality> forceConstraint;
-    std::shared_ptr<math::ConstraintEquality> forceRegTask;
-    unsigned int index; /// index of 1st element of associated force variable in the force vector
-
-    ContactLevel(contacts::ContactBase & contact);
-  };
 
   class ContactTransitionInfo
   {
@@ -138,7 +113,8 @@ namespace tsid
 
   public:
 
-    void addTask(std::shared_ptr<TaskLevel> task,
+    template<class TaskLevelPointer>
+    void addTask(TaskLevelPointer task,
                  double weight,
                  unsigned int priorityLevel);
 
@@ -151,7 +127,7 @@ namespace tsid
     Data m_data;
     HQPData m_hqpData;
     std::vector<std::shared_ptr<TaskLevel> >        m_taskMotions;
-    std::vector<std::shared_ptr<TaskLevel> >     m_taskContactForces;
+    std::vector<std::shared_ptr<TaskLevelForce> >   m_taskContactForces;
     std::vector<std::shared_ptr<TaskLevel> >        m_taskActuations;
     std::vector<std::shared_ptr<ContactLevel> >     m_contacts;
     double m_t;         /// time

include/tsid/formulations/inverse-dynamics-formulation-base.hpp

@@ -32,6 +32,30 @@
 namespace tsid
 {
 
+  struct TaskLevel
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+    tasks::TaskBase & task;
+    std::shared_ptr<math::ConstraintBase> constraint;
+    unsigned int priority;
+
+    TaskLevel(tasks::TaskBase & task,
+              unsigned int priority);
+  };
+
+  struct TaskLevelForce
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+    tasks::TaskContactForce & task;
+    std::shared_ptr<math::ConstraintBase> constraint;
+    unsigned int priority;
+
+    TaskLevelForce(tasks::TaskContactForce & task,
+                    unsigned int priority);
+  };
+
   ///
   /// \brief Wrapper for a robot based on pinocchio
   ///

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

@@ -19,6 +19,9 @@
 #define __invdyn_task_contact_force_hpp__
 
 #include <tsid/tasks/task-base.hpp>
+#include <tsid/formulations/contact-level.hpp>
+#include <memory>
+
 
 namespace tsid
 {
@@ -31,6 +34,23 @@ namespace tsid
       
       TaskContactForce(const std::string & name,
                        RobotWrapper & robot);
+
+      /**
+       * Contact force tasks have an additional compute method that takes as extra input
+       * argument the list of active contacts. This can be needed for force tasks that
+       * involve all contacts, such as the CoP task.
+       */
+      virtual const ConstraintBase & compute(const double t,
+                                             ConstRefVector q,
+                                             ConstRefVector v,
+                                             Data & data,
+                                             const std::vector<std::shared_ptr<ContactLevel> >  *contacts) = 0;
+
+      /**
+       * Return the name of the contact associated to this task if this task is associated to a specific contact.
+       * If this task is associated to multiple contact forces (all of them), returns an empty string.
+       */
+      virtual const std::string& getAssociatedContactName() = 0;
     };
   }
 }

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

+//
+// Copyright (c) 2021 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 __invdyn_task_cop_equality_hpp__
+#define __invdyn_task_cop_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/formulations/inverse-dynamics-formulation-base.hpp"
+
+namespace tsid
+{
+  namespace tasks
+  {
+
+    class TaskCopEquality : public TaskContactForce
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      typedef math::Index Index;
+      typedef trajectories::TrajectorySample TrajectorySample;
+      typedef math::Vector Vector;
+      typedef math::Vector3 Vector3;
+      typedef math::ConstraintEquality ConstraintEquality;
+      typedef pinocchio::SE3 SE3;
+
+      TaskCopEquality(const std::string & name,
+                      RobotWrapper & robot);
+
+      void setContactList(const std::vector<std::shared_ptr<ContactLevel> >  *contacts);
+
+      int dim() const;
+
+      virtual const std::string& getAssociatedContactName();
+
+      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(const Vector3 & ref);
+      const Vector3 & getReference() const;
+
+    protected:
+      const std::vector<std::shared_ptr<ContactLevel> > *m_contacts;
+      std::string m_contact_name; // an empty string
+      Vector3 m_normal;     // normal direction to the ground expressed in world frame
+      Vector3 m_ref;        // reference CoP in world frame
+      ConstraintEquality m_constraint;
+    };
+
+  }
+}
+
+#endif // ifndef __invdyn_task_com_equality_hpp__

src/contacts/contact-6d.cpp

@@ -171,6 +171,11 @@ bool Contact6d::setContactPoints(ConstRefMatrix contactPoints)
   return true;
 }
 
+const Matrix3x & Contact6d::getContactPoints() const
+{
+  return m_contactPoints;
+}
+
 bool Contact6d::setContactNormal(ConstRefVector contactNormal)
 {
   assert(contactNormal.size()==3);
@@ -259,7 +264,7 @@ computeForceRegularizationTask(const double ,
 double Contact6d::getMinNormalForce() const { return m_fMin; }
 double Contact6d::getMaxNormalForce() const { return m_fMax; }
 
-const TaskMotion & Contact6d::getMotionTask() const { return m_motionTask; }
+const TaskSE3Equality & Contact6d::getMotionTask() const { return m_motionTask; }
 
 const ConstraintBase & Contact6d::getMotionConstraint() const { return m_motionTask.getConstraint(); }
 

src/contacts/contact-point.cpp

@@ -45,6 +45,8 @@ ContactPoint::ContactPoint(const std::string & name,
   m_weightForceRegTask << 1, 1, 1e-3;
   m_forceGenMat.resize(3, 3);
   m_fRef = Vector3::Zero();
+  m_contactPoints.resize(3,1);
+  m_contactPoints.setZero();
   updateForceGeneratorMatrix();
   updateForceInequalityConstraints();
   updateForceRegularizationTask();
@@ -94,6 +96,11 @@ double ContactPoint::getNormalForce(ConstRefVector f) const
   return m_contactNormal.dot(f);
 }
 
+const Matrix3x & ContactPoint::getContactPoints() const
+{
+  return m_contactPoints;
+}
+
 void ContactPoint::setRegularizationTaskWeightVector(ConstRefVector & w)
 {