Merge remote-tracking branch 'athiault/diff-func-equality' into devel

include/hpp/constraints/active-set-differentiable-function.hh

@@ -83,6 +83,19 @@ namespace hpp {
             jacobian.middleCols (_int->first, _int->second).setZero ();
         }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          const ActiveSetDifferentiableFunction& castother = dynamic_cast<const ActiveSetDifferentiableFunction&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (function_ != castother.function_)
+            return false;
+          if (intervals_ != castother.intervals_)
+            return false;
+          
+          return true;
+        }
+
         DifferentiableFunctionPtr_t function_;
         segments_t intervals_;
     }; // class ActiveSetDifferentiableFunction

include/hpp/constraints/affine-function.hh

@@ -56,6 +56,14 @@ namespace hpp {
           J.setIdentity();
         }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          dynamic_cast<const Identity&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+
+          return true;
+        }
+
       private:
         Identity() {}
         HPP_SERIALIZABLE();
@@ -100,6 +108,19 @@ namespace hpp {
           init();
         }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          const AffineFunction& castother = dynamic_cast<const AffineFunction&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (J_ != castother.J_)
+            return false;
+          if (b_ != castother.b_)
+            return false;
+          
+          return true;
+        }
+
       private:
         /// User implementation of function evaluation
         void impl_compute (LiegroupElementRef y, vectorIn_t x) const
@@ -171,6 +192,17 @@ namespace hpp {
 
         void impl_jacobian (matrixOut_t J, vectorIn_t) const { J.setZero(); }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          const ConstantFunction& castother = dynamic_cast<const ConstantFunction&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (c_.vector() == castother.c_.vector())
+            return false;
+          
+          return true;
+        }
+
         const LiegroupElement c_;
 
     private:

include/hpp/constraints/com-between-feet.hh

@@ -94,6 +94,34 @@ namespace hpp {
 
         virtual void impl_jacobian (matrixOut_t jacobian,
             ConfigurationIn_t arg) const;
+
+        bool isEqual(const DifferentiableFunction& other) const {
+          const ComBetweenFeet& castother = dynamic_cast<const ComBetweenFeet&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (robot_ != castother.robot_)
+            return false;
+          if (com_->centerOfMassComputation() != castother.com_->centerOfMassComputation())
+            return false;
+          if (left_->joint() != castother.left_->joint())
+            return false;
+          if (right_->joint() != castother.right_->joint())
+            return false;
+          if (left_->local() != castother.left_->local())
+            return false;
+          if (right_->local() != castother.right_->local())
+            return false;
+          if (jointRef_ != castother.jointRef_)
+            return false;
+          if (pointRef_ != castother.pointRef_)
+            return false;
+          if (mask_ != castother.mask_)
+            return false;
+          
+          return true;
+        }
+
       private:
         DevicePtr_t robot_;
         mutable Traits<PointCom>::Ptr_t com_;

include/hpp/constraints/configuration-constraint.hh

@@ -72,12 +72,27 @@ namespace hpp {
             ConfigurationIn_t arg) const;
 
         std::ostream& print (std::ostream& o) const;
+
+        bool isEqual(const DifferentiableFunction& other) const {
+          const ConfigurationConstraint& castother = dynamic_cast<const ConfigurationConstraint&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (robot_ != castother.robot_)
+            return false;
+          if (goal_.vector() != castother.goal_.vector())
+            return false;
+          if (weights_ != castother.weights_)
+            return false;
+          
+          return true;
+        }
       private:
         typedef Eigen::Array <bool, Eigen::Dynamic, 1> EigenBoolVector_t;
         DevicePtr_t robot_;
         LiegroupElement goal_;
         vector_t weights_;
-    }; // class ComBetweenFeet
+    }; // class ConfigurationConstraint
   } // namespace constraints
 } // namespace hpp
 #endif // HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH

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

@@ -154,6 +154,21 @@ namespace hpp {
                           const JointAndShapes_t& floorSurfaces,
                           const JointAndShapes_t& objectSurfaces);
 
+      bool isEqual(const DifferentiableFunction& other) const {
+        const ConvexShapeContact& castother = dynamic_cast<const ConvexShapeContact&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (robot_ != castother.robot_)
+          return false;
+        if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
+          return false;
+        for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
+          if (floorConvexShapes_[i] != castother.floorConvexShapes_[i])
+            return false;
+        
+        return true;
+      }
 
       private:
         /// Add a ConvexShape as an object.
@@ -255,7 +270,12 @@ namespace hpp {
                                     const JointAndShapes_t& floorSurfaces,
                                     const JointAndShapes_t& objectSurfaces);
 
-
+      bool isEqual(const DifferentiableFunction& other) const {
+        const ConvexShapeContactComplement& castother = dynamic_cast<const ConvexShapeContactComplement&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        return (sibling_ != castother.sibling_);
+      }
     private:
       void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument) const;
 
@@ -314,6 +334,19 @@ namespace hpp {
         const;
       virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg)
         const;
+
+      bool isEqual(const DifferentiableFunction& other) const {
+        const ConvexShapeContactHold& castother = dynamic_cast<const ConvexShapeContactHold&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (constraint_ != castother.constraint_)
+          return false;
+        if (complement_ != castother.complement_)
+          return false;
+        
+        return true;
+      }
     private:
       ConvexShapeContactPtr_t constraint_;
       ConvexShapeContactComplementPtr_t complement_;

include/hpp/constraints/convex-shape.hh

@@ -156,6 +156,21 @@ namespace hpp {
         /// Transform of the shape in the joint frame
         inline const Transform3f& positionInJoint () const { return MinJoint_; }
 
+        bool operator==(ConvexShape const & other) const {
+          if (Pts_ != other.Pts_) return false;
+          if (shapeDimension_ != other.shapeDimension_) return false;
+          if (C_ != other.C_) return false;
+          if (N_ != other.N_) return false;
+          if (Ns_ != other.Ns_) return false;
+          if (Us_ != other.Us_) return false;
+          if (Ls_ != other.Ls_) return false;
+          if (MinJoint_ != other.MinJoint_) return false;
+          if (joint_ != other.joint_) return false;
+        }
+        bool operator!=(ConvexShape const & other) const {
+          return !(this->operator==(other));
+        }
+
         /// The points in the joint frame. It is constant.
         std::vector <vector3_t> Pts_;
         size_t shapeDimension_;

include/hpp/constraints/differentiable-function-set.hh

@@ -126,6 +126,23 @@ namespace hpp {
             row += f.outputDerivativeSize();
           }
         }
+
+        bool isEqual(const DifferentiableFunction& other) const {
+          const DifferentiableFunctionSet& castother = dynamic_cast<const DifferentiableFunctionSet&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (functions_ != castother.functions_)
+            return false;
+          if (result_.size() != castother.result_.size())
+            return false;
+          for (std::size_t i=0; i<result_.size(); i++)
+            if (result_[i] != castother.result_[i])
+              return false;
+          
+          return true; 
+        }
+
       private:
         Functions_t functions_;
         mutable std::vector <LiegroupElement> result_;

include/hpp/constraints/differentiable-function.hh

@@ -19,6 +19,7 @@
 #ifndef HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
 # define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
 
+#include <hpp/util/debug.hh>
 # include <hpp/constraints/fwd.hh>
 # include <hpp/constraints/config.hh>
 # include <hpp/pinocchio/liegroup-element.hh>
@@ -53,6 +54,7 @@ namespace hpp {
     {
     public:
       virtual ~DifferentiableFunction () {}
+
       /// Evaluate the function at a given parameter.
       ///
       /// \note parameters should be of the correct size.
@@ -173,6 +175,19 @@ namespace hpp {
           DevicePtr_t robot = DevicePtr_t (),
           value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
 
+      inline bool operator== (DifferentiableFunction const & other) const
+      {
+        try {
+          return isEqual(other) && other.isEqual(*this);
+        } catch (const std::bad_cast& exc) {
+          return false;
+        }
+      }
+      inline bool operator!= (DifferentiableFunction const & b) const
+      {
+        return !(*this == b);
+      }
+      //virtual bool isEqual(DifferentiableFunctionPtr_t const &, bool) const {return true;}
     protected:
       /// \brief Concrete class constructor should call this constructor.
       ///
@@ -203,6 +218,20 @@ namespace hpp {
       virtual void impl_jacobian (matrixOut_t jacobian,
 				  vectorIn_t arg) const = 0;
 
+      virtual bool isEqual(const DifferentiableFunction& other) const 
+      {
+        if (name_ != other.name_)
+          return false;
+        if (inputSize_ != other.inputSize_)
+          return false;
+        if (inputDerivativeSize_ != other.inputDerivativeSize_)
+          return false;
+        if (*outputSpace_ != *(other.outputSpace_))
+          return false;
+
+        return true;
+      }
+
       /// Dimension of input vector.
       size_type inputSize_;
       /// Dimension of input derivative
@@ -240,5 +269,24 @@ namespace hpp {
   } // namespace constraints
 } // namespace hpp
 
+/*
+// This will override boost::shared_ptr 's equality operator
+// between 2 DifferentiableFunctionPtr_t
+namespace boost {
+  using namespace hpp::constraints;
+  typedef DifferentiableFunction T;
+  typedef shared_ptr<T> TPtr;
+
+  template<> inline bool operator==<T, T> (TPtr const & a, TPtr const & b) BOOST_SP_NOEXCEPT
+  {
+    return *a == *b;
+  }
+
+  template<> inline bool operator!=<T, T> (TPtr const & a, TPtr const & b) BOOST_SP_NOEXCEPT
+  {
+    return !(a == b);
+  }
+}
+*/
 
 #endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH

include/hpp/constraints/distance-between-bodies.hh

@@ -92,6 +92,23 @@ namespace hpp {
 				 ConfigurationIn_t argument) const;
       virtual void impl_jacobian (matrixOut_t jacobian,
 				  ConfigurationIn_t arg) const;
+
+      bool isEqual(const DifferentiableFunction& other) const {
+        const DistanceBetweenBodies& castother = dynamic_cast<const DistanceBetweenBodies&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (robot_ != castother.robot_)
+          return false;
+        if (joint1_ != castother.joint1_)
+          return false;
+        if (joint2_ != castother.joint2_)
+          return false;
+        if (data_ != castother.data_)
+          return false;
+        
+        return true;
+      }
     private:
       typedef ::pinocchio::GeometryData GeometryData;
 

include/hpp/constraints/distance-between-points-in-bodies.hh

@@ -95,6 +95,25 @@ namespace hpp {
 				 ConfigurationIn_t argument) const;
       virtual void impl_jacobian (matrixOut_t jacobian,
 				  ConfigurationIn_t arg) const;
+
+      bool isEqual(const DifferentiableFunction& other) const {
+        const DistanceBetweenPointsInBodies& castother = dynamic_cast<const DistanceBetweenPointsInBodies&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (robot_ != castother.robot_)
+          return false;
+        if (joint1_ != castother.joint1_)
+          return false;
+        if (joint2_ != castother.joint2_)
+          return false;
+        if (point1_ != castother.point1_)
+          return false;
+        if (point2_ != castother.point2_)
+          return false;
+        
+        return true;
+      }
     private:
       DevicePtr_t robot_;
       JointPtr_t joint1_;

include/hpp/constraints/explicit.hh

@@ -248,6 +248,8 @@ namespace hpp {
 
       /// Copy constructor
       Explicit (const Explicit& other);
+      
+      bool isEqual (const Implicit& other, bool swapAndTest) const;
 
       // Store weak pointer to itself
       void init (const ExplicitWkPtr_t& weak);

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

@@ -79,6 +79,27 @@ namespace hpp {
       /// Compute Jacobian of g (q_out) - f (q_in) with respect to q.
       void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;
 
+      bool isEqual(const DifferentiableFunction& other) const {
+        const ImplicitFunction& castother = dynamic_cast<const ImplicitFunction&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (robot_ != castother.robot_)
+          return false;
+        if (inputToOutput_ != castother.inputToOutput_)
+          return false;
+        if (inputConfIntervals_.rows() != castother.inputConfIntervals_.rows())
+          return false;
+        if (outputConfIntervals_.rows() != castother.outputConfIntervals_.rows())
+          return false;
+        if (inputDerivIntervals_.rows() != castother.inputDerivIntervals_.rows())
+          return false;
+        if (outputDerivIntervals_.rows() != castother.outputDerivIntervals_.rows())
+          return false;
+
+        return true;
+      }
+
     private:
       void computeJacobianBlocks ();
 

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

@@ -112,6 +112,37 @@ namespace hpp {
 
       void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;
 
+      bool isEqual(const DifferentiableFunction& other) const {
+        const RelativeTransformation& castother = dynamic_cast<const RelativeTransformation&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (robot_ != castother.robot_)
+          return false;
+        if (parentJoint_ != castother.parentJoint_)
+          return false;
+        if (joint1_ != castother.joint1_)
+          return false;
+        if (joint2_ != castother.joint2_)
+          return false;
+        if (frame1_ != castother.frame1_)
+          return false;
+        if (frame2_ != castother.frame2_)
+          return false;
+        if (inConf_.rows() != castother.inConf_.rows())
+          return false;
+        if (inVel_.cols() != castother.inVel_.cols())
+          return false;
+        if (outConf_.rows() != castother.outConf_.rows())
+          return false;
+        if (outVel_.rows() != castother.outVel_.rows())
+          return false;
+        if (F1inJ1_invF2inJ2_ != castother.F1inJ1_invF2inJ2_)
+          return false;
+        
+        return true;
+      }
+
     private:
       void forwardKinematics (vectorIn_t arg) const;
 

include/hpp/constraints/function/difference.hh

@@ -58,6 +58,25 @@ namespace hpp {
           J.middleCols (rsd_.first, rsd_.second) *= -1;
         }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          const Difference& castother = dynamic_cast<const Difference&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (inner_ != castother.inner_)
+            return false;
+          if (lsa_ != castother.lsa_)
+            return false;
+          if (lsd_ != castother.lsd_)
+            return false;
+          if (rsa_ != castother.rsa_)
+            return false;
+          if (rsd_ != castother.rsd_)
+            return false;
+          
+          return true;
+        }
+
         std::ostream& print (std::ostream& os) const;
 
         DifferentiableFunctionPtr_t inner_;

include/hpp/constraints/function/of-parameter-subset.hh

@@ -79,6 +79,21 @@ namespace hpp {
                            arg.segment (sa_.first, sa_.second));
         }
 
+        bool isEqual(const DifferentiableFunction& other) const {
+          const OfParameterSubset& castother = dynamic_cast<const OfParameterSubset&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (g_ != castother.g_)
+            return false;
+          if (sa_ != castother.sa_)
+            return false;
+          if (sd_ != castother.sd_)
+            return false;
+          
+          return true;
+        }
+
         std::ostream& print (std::ostream& os) const;
 
         DifferentiableFunctionPtr_t g_;

include/hpp/constraints/generic-transformation.hh

@@ -309,6 +309,19 @@ namespace hpp {
 				 ConfigurationIn_t argument) const;
       virtual void impl_jacobian (matrixOut_t jacobian,
 				  ConfigurationIn_t arg) const;
+
+      bool isEqual(const DifferentiableFunction& other) const {
+        const GenericTransformation& castother = dynamic_cast<const GenericTransformation&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        if (robot_ != castother.robot_)
+          return false;
+        if (mask_ != castother.mask_)
+          return false;
+        
+        return true;
+      }
+
     private:
       void computeActiveParams ();
       DevicePtr_t robot_;

include/hpp/constraints/manipulability.hh

@@ -55,6 +55,25 @@ namespace hpp {
 
       void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;
 
+      bool isEqual(const DifferentiableFunction& other) const {
+        const Manipulability& castother = dynamic_cast<const Manipulability&>(other);
+        if (!DifferentiableFunction::isEqual(other))
+          return false;
+        
+        if (function_ != castother.function_)
+          return false;
+        if (robot_ != castother.robot_)
+          return false;
+        if (cols_.cols() != castother.cols_.cols())
+          return false;
+        if (J_ != castother.J_)
+          return false;
+        if (J_JT_ != castother.J_JT_)
+          return false;
+        
+        return true;
+      }
+
     private:
       DifferentiableFunctionPtr_t function_;
       DevicePtr_t robot_;

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

@@ -72,7 +72,27 @@ namespace hpp {
         MatrixOfExpressions<>& phi () {
           return phi_;
         }
-
+      protected:
+        bool isEqual(const DifferentiableFunction& other) const {
+          const QPStaticStability& castother = dynamic_cast<const QPStaticStability&>(other);
+          if (!DifferentiableFunction::isEqual(other))
+            return false;
+          
+          if (name_ != castother.name_)
+            return false;
+          if (robot_ != castother.robot_)
+            return false;
+          if (nbContacts_ != castother.nbContacts_)
+            return false;
+          if (com_ != castother.com_)
+            return false;
+          if (Zeros != castother.Zeros)
+            return false;
+          if (nWSR != castother.nWSR)
+            return false;
+          
+          return true;
+        }
       private:
         static const Eigen::Matrix <value_type, 6, 1> MinusGravity;
 

include/hpp/constraints/relative-com.hh

@@ -97,6 +97,25 @@ namespace hpp {
 	const;
       virtual void impl_jacobian (matrixOut_t jacobian,
 				  ConfigurationIn_t arg) const;
+
+      bool isEqual(const DifferentiableFunction& other) const {
+        const RelativeCom& castother = dynamic_cast<const RelativeCom&>(other);
+        if (!DifferentiableFunction::isEqual(