Add contacts and inverse-dynamics-formulation classes with relative tests.

CMakeLists.txt

@@ -101,6 +101,13 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
     include/pininvdyn/tasks/task-actuation-equality.hpp
     include/pininvdyn/tasks/task-actuation-bounds.hpp
     include/pininvdyn/tasks/task-joint-bounds.hpp
+    include/pininvdyn/tasks/task-joint-posture.hpp
+  )
+
+SET(${PROJECT_NAME}_CONTACTS_HEADERS
+    include/pininvdyn/contacts/fwd.hpp
+    include/pininvdyn/contacts/contact-base.hpp
+    include/pininvdyn/contacts/contact-6d.hpp
   )
 
 SET(${PROJECT_NAME}_TRAJECTORIES_HEADERS
@@ -122,8 +129,10 @@ SET(HEADERS
     include/pininvdyn/config.hpp
     include/pininvdyn/robot-wrapper.hpp
     include/pininvdyn/inverse-dynamics-formulation-base.hpp
+    include/pininvdyn/inverse-dynamics-formulation-acc-force.hpp
     ${${PROJECT_NAME}_MATH_HEADERS}
     ${${PROJECT_NAME}_TASKS_HEADERS}
+    ${${PROJECT_NAME}_CONTACTS_HEADERS}
     ${${PROJECT_NAME}_TRAJECTORIES_HEADERS}
     ${${PROJECT_NAME}_SOLVERS_HEADERS}
  )

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

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn 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.
+// PinInvDyn 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
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_contact_6d_hpp__
+#define __invdyn_contact_6d_hpp__
+
+#include <pininvdyn/contacts/contact-base.hpp>
+#include <pininvdyn/tasks/task-se3-equality.hpp>
+
+namespace pininvdyn
+{
+  namespace contacts
+  {
+    class Contact6d:
+        public ContactBase
+    {
+    public:
+      typedef pininvdyn::RobotWrapper RobotWrapper;
+      typedef pininvdyn::math::ConstRefMatrix ConstRefMatrix;
+      typedef pininvdyn::math::ConstRefVector ConstRefVector;
+      typedef pininvdyn::math::Matrix3x Matrix3x;
+      typedef pininvdyn::math::Vector3 Vector3;
+      typedef pininvdyn::math::Vector Vector;
+      typedef pininvdyn::tasks::TaskMotion TaskMotion;
+      typedef pininvdyn::tasks::TaskSE3Equality TaskSE3Equality;
+      typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
+      typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
+      typedef se3::SE3 SE3;
+
+      Contact6d(const std::string & name,
+                RobotWrapper & robot,
+                const std::string & frameName,
+                ConstRefMatrix contactPoints,
+                ConstRefVector contactNormal,
+                const double frictionCoefficient,
+                const double minNormalForce,
+                const double regularizationTaskWeight);
+
+      /// Return the number of motion constraints
+      virtual unsigned int n_motion() const;
+
+      /// Return the number of force variables
+      virtual unsigned int n_force() const;
+
+      virtual const ConstraintBase & computeMotionTask(const double t,
+                                                       ConstRefVector q,
+                                                       ConstRefVector v,
+                                                       Data & data);
+
+      virtual const ConstraintInequality & computeForceTask(const double t,
+                                                            ConstRefVector q,
+                                                            ConstRefVector v,
+                                                            Data & data);
+
+      virtual const Matrix & getForceGeneratorMatrix();
+
+      virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
+                                                                        ConstRefVector q,
+                                                                        ConstRefVector v,
+                                                                        Data & data);
+
+      const TaskMotion & getMotionTask() const;
+      const ConstraintBase & getMotionConstraint() const;
+      const ConstraintInequality & getForceConstraint() const;
+      const ConstraintEquality & getForceRegularizationTask() const;
+      double getForceRegularizationWeight() const;
+
+      const Vector & Kp() const;
+      const Vector & Kd() const;
+      void Kp(ConstRefVector Kp);
+      void Kd(ConstRefVector Kp);
+
+      bool setContactPoints(ConstRefMatrix contactPoints);
+      bool setContactNormal(ConstRefVector contactNormal);
+
+      bool setFrictionCoefficient(const double frictionCoefficient);
+      bool setMinNormalForce(const double minNormalForce);
+      bool setRegularizationTaskWeight(const double w);
+      void setReference(const SE3 & ref);
+
+    protected:
+
+      void updateForceInequalityConstraints();
+      void updateForceRegularizationTask();
+      void updateForceGeneratorMatrix();
+
+      TaskSE3Equality m_motionTask;
+      ConstraintInequality m_forceInequality;
+      ConstraintEquality m_forceRegTask;
+      Matrix3x m_contactPoints;
+      Vector3 m_contactNormal;
+      double m_mu;
+      double m_fMin;
+      double m_regularizationTaskWeight;
+      Matrix m_forceGenMat;
+    };
+  }
+}
+
+#endif // ifndef __invdyn_contact_6d_hpp__

include/pininvdyn/contacts/contact-base.hpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn 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.
+// PinInvDyn 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
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_contact_base_hpp__
+#define __invdyn_contact_base_hpp__
+
+#include <pininvdyn/robot-wrapper.hpp>
+#include <pininvdyn/math/utils.hpp>
+#include <pininvdyn/tasks/task-motion.hpp>
+#include <pininvdyn/math/constraint-base.hpp>
+#include <pininvdyn/math/constraint-equality.hpp>
+#include <pininvdyn/math/constraint-inequality.hpp>
+
+namespace pininvdyn
+{
+  namespace contacts
+  {
+    
+    ///
+    /// \brief Base template of a Contact.
+    ///
+    class ContactBase
+    {
+    public:
+      typedef pininvdyn::RobotWrapper RobotWrapper;
+      typedef pininvdyn::math::ConstraintBase ConstraintBase;
+      typedef pininvdyn::math::ConstraintInequality ConstraintInequality;
+      typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
+      typedef pininvdyn::math::ConstRefVector ConstRefVector;
+      typedef pininvdyn::math::Matrix Matrix;
+      typedef pininvdyn::tasks::TaskMotion TaskMotion;
+      typedef se3::Data Data;
+
+      ContactBase(const std::string & name,
+                  RobotWrapper & robot);
+
+      const std::string & name() const;
+
+      void name(const std::string & name);
+      
+      /// Return the number of motion constraints
+      virtual unsigned int n_motion() const = 0;
+
+      /// Return the number of force variables
+      virtual unsigned int n_force() const = 0;
+
+      virtual const ConstraintBase & computeMotionTask(const double t,
+                                                       ConstRefVector q,
+                                                       ConstRefVector v,
+                                                       Data & data) = 0;
+
+      virtual const ConstraintInequality & computeForceTask(const double t,
+                                                            ConstRefVector q,
+                                                            ConstRefVector v,
+                                                            Data & data) = 0;
+
+      virtual const Matrix & getForceGeneratorMatrix() = 0;
+
+      virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
+                                                                    ConstRefVector q,
+                                                                    ConstRefVector v,
+                                                                    Data & data) = 0;
+
+      virtual const TaskMotion & getMotionTask() const = 0;
+      virtual const ConstraintBase & getMotionConstraint() const = 0;
+      virtual const ConstraintInequality & getForceConstraint() const = 0;
+      virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
+      virtual double getForceRegularizationWeight() const = 0;
+      
+    protected:
+      std::string m_name;
+      /// \brief Reference on the robot model.
+      RobotWrapper & m_robot;
+    };
+    
+  }
+}
+
+#endif // ifndef __invdyn_contact_base_hpp__

include/pininvdyn/contacts/fwd.hpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// pinocchio 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.
+// pinocchio 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
+// pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_tasks_fwd_hpp__
+#define __invdyn_tasks_fwd_hpp__
+
+namespace pininvdyn
+{
+  namespace tasks
+  {
+    
+  }
+}
+
+#endif // ifndef __invdyn_tasks_fwd_hpp__

include/pininvdyn/inverse-dynamics-formulation-acc-force.hpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn 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.
+// PinInvDyn 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
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
+#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
+
+#include <pininvdyn/inverse-dynamics-formulation-base.hpp>
+#include <vector>
+
+namespace pininvdyn
+{
+
+  class TaskLevel
+  {
+  public:
+    pininvdyn::tasks::TaskBase & task;
+    pininvdyn::math::ConstraintBase * constraint;
+    double weight;
+    unsigned int priority;
+
+    TaskLevel(pininvdyn::tasks::TaskBase & task,
+              double weight,
+              unsigned int priority);
+  };
+
+  class ContactLevel
+  {
+  public:
+    pininvdyn::contacts::ContactBase & contact;
+    pininvdyn::math::ConstraintBase * motionConstraint;
+    pininvdyn::math::ConstraintInequality * forceConstraint;
+    pininvdyn::math::ConstraintEquality * forceRegTask;
+    unsigned int index; /// index of 1st element of associated force variable in the force vector
+
+    ContactLevel(pininvdyn::contacts::ContactBase & contact);
+  };
+
+  class InverseDynamicsFormulationAccForce:
+      public InverseDynamicsFormulationBase
+  {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    typedef se3::Data Data;
+    typedef pininvdyn::math::Matrix Matrix;
+    typedef pininvdyn::math::ConstRefVector ConstRefVector;
+    typedef pininvdyn::tasks::TaskBase TaskBase;
+    typedef pininvdyn::tasks::TaskMotion TaskMotion;
+    typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
+    typedef pininvdyn::tasks::TaskActuation TaskActuation;
+
+
+    InverseDynamicsFormulationAccForce(const std::string & name,
+                                       RobotWrapper & robot,
+                                       bool verbose=false);
+
+    const Data & data() const;
+
+    unsigned int nVar() const;
+    unsigned int nEq() const;
+    unsigned int nIn() const;
+
+    bool addMotionTask(TaskMotion & task,
+                       double weight,
+                       unsigned int priorityLevel,
+                       double transition_duration=0.0);
+
+    bool addForceTask(TaskContactForce & task,
+                      double weight,
+                      unsigned int priorityLevel,
+                      double transition_duration=0.0);
+
+    bool addTorqueTask(TaskActuation & task,
+                       double weight,
+                       unsigned int priorityLevel,
+                       double transition_duration=0.0);
+
+    bool addRigidContact(ContactBase & contact);
+
+    bool removeTask(const std::string & taskName,
+                    double transition_duration=0.0);
+
+    bool removeRigidContact(const std::string & contactName,
+                            double transition_duration=0.0);
+
+    const HqpData & computeProblemData(double time,
+                                       ConstRefVector q,
+                                       ConstRefVector v);
+
+  public:
+
+    void addTask(TaskLevel* task,
+                 double weight,
+                 unsigned int priorityLevel);
+
+    void resizeHqpData();
+
+    Data m_data;
+    HqpData m_hqpData;
+    std::vector<TaskLevel*>     m_taskMotions;
+    std::vector<TaskLevel*>     m_taskContactForces;
+    std::vector<TaskLevel*>     m_taskActuations;
+    std::vector<ContactLevel*>   m_contacts;
+    double m_t;         /// time
+    unsigned int m_k;   /// number of contact-force variables
+    unsigned int m_v;   /// number of acceleration variables
+    unsigned int m_eq;  /// number of equality constraints
+    unsigned int m_in;  /// number of inequality constraints
+    Matrix m_Jc;        /// contact force Jacobian
+    pininvdyn::math::ConstraintEquality m_baseDynamics;
+  };
+
+}
+
+#endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__

include/pininvdyn/inverse-dynamics-formulation-base.hpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn 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.
+// PinInvDyn 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
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
+#define __invdyn_inverse_dynamics_formulation_base_hpp__
+
+#include <pininvdyn/math/utils.hpp>
+#include <pininvdyn/robot-wrapper.hpp>
+#include <pininvdyn/tasks/task-actuation.hpp>
+#include <pininvdyn/tasks/task-motion.hpp>
+#include <pininvdyn/tasks/task-contact-force.hpp>
+#include <pininvdyn/contacts/contact-base.hpp>
+#include <pininvdyn/solvers/solver-HQP-base.hpp>
+
+#include <string>
+
+namespace pininvdyn
+{
+
+  ///
+  /// \brief Wrapper for a robot based on pinocchio
+  ///
+  class InverseDynamicsFormulationBase
+  {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    typedef se3::Data Data;
+    typedef pininvdyn::math::ConstRefVector ConstRefVector;
+    typedef pininvdyn::tasks::TaskMotion TaskMotion;
+    typedef pininvdyn::tasks::TaskContactForce TaskContactForce;
+    typedef pininvdyn::tasks::TaskActuation TaskActuation;
+    typedef pininvdyn::contacts::ContactBase ContactBase;
+    typedef pininvdyn::solvers::HqpData HqpData;
+
+
+    InverseDynamicsFormulationBase(const std::string & name,
+                                   RobotWrapper & robot,
+                                   bool verbose=false);
+
+    virtual const Data & data() const = 0;
+
+    virtual unsigned int nVar() const = 0;
+    virtual unsigned int nEq() const = 0;
+    virtual unsigned int nIn() const = 0;
+
+    virtual bool addMotionTask(TaskMotion & task,
+                               double weight,
+                               unsigned int priorityLevel,
+                               double transition_duration=0.0) = 0;
+
+    virtual bool addForceTask(TaskContactForce & task,
+                              double weight,
+                              unsigned int priorityLevel,
+                              double transition_duration=0.0) = 0;
+
+    virtual bool addTorqueTask(TaskActuation & task,
+                               double weight,
+                               unsigned int priorityLevel,
+                               double transition_duration=0.0) = 0;
+
+    virtual bool addRigidContact(ContactBase & contact) = 0;
+
+    virtual bool removeTask(const std::string & taskName,
+                            double transition_duration=0.0) = 0;
+
+    virtual bool removeRigidContact(const std::string & contactName,
+                                    double transition_duration=0.0) = 0;
+
+    virtual const HqpData & computeProblemData(double time,
+                                               ConstRefVector q,
+                                               ConstRefVector v) = 0;
+
+  protected:
+    std::string m_name;
+    RobotWrapper m_robot;
+    bool m_verbose;
+  };
+
+}
+
+#endif // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__

include/pininvdyn/math/constraint-base.hpp

@@ -39,37 +39,41 @@ namespace pininvdyn
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-      ConstraintBase(std::string name):
-        m_name(name){}
+      ConstraintBase(const std::string & name);
 
-      ConstraintBase(std::string name, const unsigned int rows, const unsigned int cols):
-        m_name(name)
-      {
-        m_A = Matrix::Zero(rows, cols);
-      }
+      ConstraintBase(const std::string & name,
+                     const unsigned int rows,
+                     const unsigned int cols);
 
-      ConstraintBase(std::string name, const Matrix & A):
-        m_name(name),
-        m_A(A)
-      {}
+      ConstraintBase(const std::string & name,
+                     ConstRefMatrix A);
 
+      virtual const std::string & name() const;
       virtual unsigned int rows() const = 0;
       virtual unsigned int cols() const = 0;
+      virtual void resize(const unsigned int r, const unsigned int c) = 0;
 
       virtual bool isEquality() const = 0;
       virtual bool isInequality() const = 0;
       virtual bool isBound() const = 0;
 
-      virtual const Matrix & matrix() const { return m_A; }
+      virtual const Matrix & matrix() const;
       virtual const Vector & vector() const = 0;
       virtual const Vector & lowerBound() const = 0;
       virtual const Vector & upperBound() const = 0;
 
-      virtual bool setMatrix(ConstRefMatrix A) { m_A = A; return true; }
+      virtual Matrix & matrix();
+      virtual Vector & vector() = 0;
+      virtual Vector & lowerBound() = 0;
+      virtual Vector & upperBound() = 0;
+
+      virtual bool setMatrix(ConstRefMatrix A);
       virtual bool setVector(ConstRefVector b) = 0;
       virtual bool setLowerBound(ConstRefVector lb) = 0;
       virtual bool setUpperBound(ConstRefVector ub) = 0;
 
+      virtual bool checkConstraint(ConstRefVector x, double tol=1e-6) const = 0;
+
     protected:
       std::string m_name;
       Matrix m_A;

include/pininvdyn/math/constraint-bound.hpp

@@ -2,16 +2,16 @@
 // Copyright (c) 2017 CNRS
 //
 // This file is part of PinInvDyn
-// pinocchio is free software: you can redistribute it
+// PinInvDyn 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.
-// pinocchio is distributed in the hope that it will be
+// PinInvDyn 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
-// pinocchio If not, see
+// PinInvDyn If not, see
 // <http://www.gnu.org/licenses/>.
 //
 
@@ -32,45 +32,36 @@ namespace pininvdyn
     {
     public:
 
-      ConstraintBound(std::string name):
-        ConstraintBase(name)
-      {}
-
-      ConstraintBound(std::string name, const unsigned int size):
-        ConstraintBase(name),
-        m_lb(Vector::Zero(size)),
-        m_ub(Vector::Zero(size))
-      {}
-
-      ConstraintBound(std::string name, const Vector & lb, const Vector & ub):
-        ConstraintBase(name),
-        m_lb(lb),
-        m_ub(ub)
-      {}
-
-      inline unsigned int rows() const
-      {
-        assert(m_lb.rows()==m_ub.rows());
-        return (unsigned int) m_lb.rows();
-      }
-
-      inline unsigned int cols() const
-      {
-        assert(m_lb.rows()==m_ub.rows());
-        return (unsigned int) m_lb.rows();
-      }
-
-      inline bool isEquality() const    { return false; }