Merge remote-tracking branch 'ahn/devel' into devel

include/hpp/constraints/convex-shape-contact.hh

@@ -156,6 +156,39 @@ namespace hpp {
 
       /// Display object in a stream
       std::ostream& print (std::ostream& o) const;
+
+      /// Return pair of joints on which the value of the function depends
+      ///
+      /// If the value of the function only depends on the relative pose between
+      /// two joints, the function returns the pair of joints.
+      /// If a joint is empty, it stands for the environment.
+      ///
+      /// Currently only supports the case when all the joints for floors are
+      /// the same and all the joints for objects involved are the same.
+      std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
+          (DeviceConstPtr_t robot) const {
+        if ((floorConvexShapes_.size() == 0) || (objectConvexShapes_.size() == 0)) {
+          return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
+        }
+        JointConstPtr_t floor0_joint = floorConvexShapes_[0].joint_;
+        JointConstPtr_t object0_joint = objectConvexShapes_[0].joint_;
+
+        // check that all the joints involved are the same
+        for (ConvexShapes_t::const_iterator it (floorConvexShapes_.begin());
+            it != floorConvexShapes_.end(); ++it)
+        {
+          assert (it->joint_ == floor0_joint);
+        }
+
+        for (ConvexShapes_t::const_iterator it (objectConvexShapes_.begin());
+            it != objectConvexShapes_.end(); ++it)
+        {
+          assert (it->joint_ == object0_joint);
+        }
+
+        return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint, object0_joint);
+      };
+
     protected:
       /// Constructor
       /// \param name name of the constraint,

include/hpp/constraints/explicit/implicit-function.hh

@@ -42,7 +42,7 @@ namespace hpp {
     ///      q,
     ///  \li q_in is the vector composed of other configuration variables of
     ///      q,
-    ///  f, g are differentiable functions with values in  a Lie group.
+    ///  \li f, g are differentiable functions with values in a Lie group.
     ///
     ///  This class is mainly used to create hpp::constraints::Explicit
     ///  instances.
@@ -112,6 +112,21 @@ namespace hpp {
         return true;
       }
 
+      /// Return pair of joints whose relative pose modifies the value if any
+      ///
+      /// Currently checks if the implicit function specifies a joint
+      /// where
+      ///  \li q_out is a vector corresponding to only 1 joint
+      ///  \li q_in is an empty vector (since f is constant and specifies
+      ///      the whole or part of the pose of the joint)
+      /// \param robotPtr the robot the constraints are applied on,
+      /// \return (null_ptr, jointPtr) where jointPtr is a shared pointer to
+      ///          the joint, or an empty pointer if the implicit
+      ///          function does not specify a locked joint.
+      /// \note this method is intended to detect locked joints.
+      std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
+          (DeviceConstPtr_t robotPtr) const;
+
     private:
       void computeJacobianBlocks ();
 

include/hpp/constraints/explicit/relative-transformation.hh

@@ -45,9 +45,9 @@ namespace hpp {
     /// Relative transformation as a mapping between configuration variables
     ///
     /// When the positions of two joints are constrained by a full
-    /// transformation constraint, if the second joint is hold by a freeflyer
+    /// transformation constraint, if the second joint is held by a freeflyer
     /// joint, the position of this latter joint can be
-    /// explicitely expressed with respect to the position of the first joint.
+    /// explicitly expressed with respect to the position of the first joint.
     ///
     /// This class provides this expression. The input configuration variables
     /// are the joint values of all joints except the above mentioned freeflyer
@@ -84,6 +84,16 @@ namespace hpp {
         return joint2_;
       }
 
+      /// Return pair of joints on which the value of the function depends
+      ///
+      /// If the value of the function only depends on the relative pose between
+      /// two joints, the function returns the pair of joints.
+      /// If a joint is empty, it stands for the environment.
+      std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
+          (DeviceConstPtr_t robot) const {
+        return std::pair<JointConstPtr_t, JointConstPtr_t>(joint1(), joint2());
+      };
+
     protected:
       typedef Eigen::BlockIndex BlockIndex;
       typedef Eigen::RowBlockIndices RowBlockIndices;

include/hpp/constraints/fwd.hh

@@ -106,6 +106,7 @@ namespace hpp {
     typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
     typedef pinocchio::Device Device;
     typedef pinocchio::DevicePtr_t DevicePtr_t;
+    typedef pinocchio::DeviceConstPtr_t DeviceConstPtr_t;
     typedef pinocchio::CenterOfMassComputation CenterOfMassComputation;
     typedef pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t;
     typedef shared_ptr <DifferentiableFunction>

include/hpp/constraints/generic-transformation.hh

@@ -300,6 +300,16 @@ namespace hpp {
 	return m_.F2inJ2;
       }
 
+      /// Return pair of joints on which the value of the function depends
+      ///
+      /// If the value of the function only depends on the relative pose between
+      /// two joints, the function returns the pair of joints.
+      /// If a joint is empty, it stands for the environment.
+      std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
+          (DeviceConstPtr_t robot) const {
+        return std::pair<JointConstPtr_t, JointConstPtr_t>(joint1(), joint2());
+      };
+
       virtual std::ostream& print (std::ostream& o) const;
 
       ///Constructor

src/explicit/implicit-function.cc

@@ -311,6 +311,35 @@ namespace hpp {
         }
       }
 
+      /// Return pair of joints on which the value of the function depends
+      ///
+      /// If the value of the function only depends on the relative pose between
+      /// two joints, the function returns the pair of joints.
+      /// If a joint is empty, it stands for the environment.
+      std::pair<JointConstPtr_t, JointConstPtr_t> ImplicitFunction::jointsInvolved
+          (DeviceConstPtr_t robotPtr) const {
+        if (inputToOutput_->inputSize() != 0)
+          return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
+        // get the joints involved in the output config
+        // quite inefficient for now
+        JointConstPtr_t jointPtr = nullptr;
+        for (segment_t row: outputConfIntervals_.rows()) {
+          for (size_type rank(row.first); rank < row.first + row.second; ++rank) {
+            JointConstPtr_t newJointPtr = robotPtr->getJointAtConfigRank(rank);
+            if (newJointPtr) {
+              // if there is more than 1 locked joint, return null
+              if ((jointPtr) && (jointPtr != newJointPtr)) {
+                hppDout(debug, "this constraint locks both joints "
+                  << jointPtr->index() << " and " << newJointPtr->index());
+                return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
+              }
+              jointPtr = newJointPtr;
+            }
+          }
+        }
+        return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, jointPtr);
+      };
+
       HPP_SERIALIZATION_IMPLEMENT(ImplicitFunction);
     } // namespace explicit_
   } // namespace constraints