[QPStaticStability] Refactor and document.

include/hpp/constraints/qp-static-stability.hh

@@ -42,6 +42,17 @@ namespace hpp {
 
     /// \addtogroup constraints
     /// \{
+    ///
+
+    /// Quasi static equilibrium constraint defined as a QP program
+    ///
+    /// This class defines a function whose value is equal to zero
+    /// if and only if there exists a set of non-negative forces applied
+    /// at some given contact points along some given normals that
+    //  counter-balance the gravity force and momentum of a kinematics chain.
+    ///
+    /// \sa https://hal.archives-ouvertes.fr/tel-01516897 Chapter 2, Sections
+    ///     3.1 and 3.2.
     class HPP_CONSTRAINTS_DLLAPI QPStaticStability : public DifferentiableFunction {
       public:
         static const Eigen::Matrix <value_type, 6, 1> Gravity;
@@ -51,7 +62,9 @@ namespace hpp {
         typedef ConvexShapeContact::ForceData ForceData;
 
         /// Constructor
+        /// \param name name of the constraint
         /// \param robot the robot the constraints is applied to,
+        /// \param contacts set of contact points
         /// \param com COM of the robot
         QPStaticStability (const std::string& name, const DevicePtr_t& robot,
             const Contacts_t& contacts,
@@ -89,7 +102,7 @@ namespace hpp {
           const QPStaticStability& castother = dynamic_cast<const QPStaticStability&>(other);
           if (!DifferentiableFunction::isEqual(other))
             return false;
-          
+
           if (name() != castother.name())
             return false;
           if (robot_ != castother.robot_)
@@ -102,7 +115,7 @@ namespace hpp {
             return false;
           if (nWSR != castother.nWSR)
             return false;
-          
+
           return true;
         }
       private:

include/hpp/constraints/static-stability.hh

@@ -50,16 +50,13 @@ namespace hpp {
         static const Eigen::Matrix <value_type, 6, 1> Gravity;
 
         struct Contact_t {
-          JointPtr_t joint1, joint2;
-          vector3_t point1, point2;
-          vector3_t normal1, normal2;
+          JointPtr_t joint;
+          vector3_t point;
+          vector3_t normal;
           bool operator==(Contact_t const& other) const {
-            if (joint1 != other.joint1) return false;
-            if (joint2 != other.joint2) return false;
-            if (point1 != other.point1) return false;
-            if (point2 != other.point2) return false;
-            if (normal1 != other.normal1) return false;
-            if (normal2 != other.normal2) return false;
+            if (joint != other.joint) return false;
+            if (point != other.point) return false;
+            if (normal != other.normal) return false;
             return true;
           }
         };

src/qp-static-stability.cc

@@ -77,9 +77,9 @@ namespace hpp {
       Traits<PointCom>::Ptr_t OG = PointCom::create(com);
       for (std::size_t i = 0; i < contacts.size(); ++i) {
         Traits<PointInJoint>::Ptr_t OP2 = PointInJoint::create
-          (contacts[i].joint2,contacts[i].point2,robot->numberDof());
+          (contacts[i].joint,contacts[i].point,robot->numberDof());
         Traits<VectorInJoint>::Ptr_t n2 = VectorInJoint::create
-          (contacts[i].joint2,contacts[i].normal2,robot->numberDof()); 
+          (contacts[i].joint,contacts[i].normal,robot->numberDof());
 
         phi_ (0,i) = n2;
         phi_ (1,i) = (OG - OP2) ^ n2;
@@ -110,7 +110,7 @@ namespace hpp {
       std::size_t col = 0;
       for (std::size_t i = 0; i < contacts.size (); ++i) {
         Traits<VectorInJoint>::Ptr_t n = VectorInJoint::create
-          (contacts[i].joint,contacts[i].normal,robot->numberDof()); 
+          (contacts[i].joint,contacts[i].normal,robot->numberDof());
         for (std::size_t j = 0; j < contacts[i].points.size (); ++j) {
           Traits<PointInJoint>::Ptr_t OP = PointInJoint::create
             (contacts[i].joint,contacts[i].points[j],robot->numberDof());

src/static-stability.cc

@@ -63,9 +63,9 @@ namespace hpp {
       Traits<PointCom>::Ptr_t OG = PointCom::create (com);
       for (std::size_t i = 0; i < contacts.size(); ++i) {
         Traits<PointInJoint>::Ptr_t OP2 =
-          PointInJoint::create (contacts[i].joint2,contacts[i].point2,robot->numberDof());
+          PointInJoint::create (contacts[i].joint,contacts[i].point,robot->numberDof());
         Traits<VectorInJoint>::Ptr_t n2 =
-          VectorInJoint::create (contacts[i].joint2,contacts[i].normal2,robot->numberDof()); 
+          VectorInJoint::create (contacts[i].joint,contacts[i].normal,robot->numberDof());
 
         phi_ (0,i) = n2;
         phi_ (1,i) = (OG - OP2) ^ n2;