Merge pull request #154 from florent-lamiraux/devel

[ConvexShapeContact] Add a check before creating an explicit constraint

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/explicit/convex-shape-contact.cc

@@ -36,12 +36,51 @@
 namespace hpp {
   namespace constraints {
     namespace explicit_ {
+      // Check that object contact surface
+      //   - belong to the same object (joint),
+      //   - are on a freeflyer joint.
+      // Throw in case these assertions are not true
+      static void checkContactSurfaces(const JointAndShapes_t& objectSurfaces)
+      {
+        JointPtr_t joint(0x0);
+        for(auto js : objectSurfaces){
+          if (!joint) {
+            joint = js.first;
+            // Check that joint is a freeflyer
+            if ((*joint->configurationSpace()!=*pinocchio::LiegroupSpace::SE3())
+                &&
+                (*joint->configurationSpace()!=*pinocchio::LiegroupSpace::R3xSO3
+                 ())){
+              std::ostringstream os;
+              os << "You are trying to build an explicit contact constraint but"
+                " the joint that holds at least on object contact surface is "
+                " not a freeflyer joint: " << joint->name();
+              throw std::logic_error(os.str().c_str());
+              if (joint->parentJoint()){
+                os << "You are trying to build an explicit contact constraint "
+                  "but the joint that holds at least on object contact "
+                  "surface: " << joint->name() <<
+                  " is attached to another joint. This is not supported";
+                throw std::logic_error(os.str().c_str());
+              }
+            }
+          } else if (js.first != joint){
+            std::ostringstream os;
+            os << "You are trying to build an explicit contact constraint "
+              "but several joints hold object contact surface: "
+               << joint->name() << " and " << js.first->name();
+            throw std::logic_error(os.str().c_str());
+          }
+        }
+      }
+
       ConvexShapeContactPtr_t ConvexShapeContact::create
       (const std::string& name, DevicePtr_t robot,
        const JointAndShapes_t& floorSurfaces,
        const JointAndShapes_t& objectSurfaces,
        const value_type& margin)
       {
+        checkContactSurfaces(objectSurfaces);
         ConvexShapeContact* ptr(new ConvexShapeContact
                                 (name, robot, floorSurfaces, objectSurfaces,
                                  margin));

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;