Commit 12f05867 authored by alexandrethiault's avatar alexandrethiault
Browse files

Operator == for DifferentiableFunction and child classes to replace ptr equality

parent 041a0969
Pipeline #15274 failed with stage
in 5 minutes and 47 seconds
......@@ -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
......
......@@ -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:
......
......@@ -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_;
......
......@@ -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
......@@ -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_;
......
......@@ -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_;
......
......@@ -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_;
......
......@@ -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
......@@ -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;
......
......@@ -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_;
......
......@@ -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);
......
......@@ -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 ();
......
......@@ -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;
......
......@@ -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_;
......
......@@ -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_;
......
......@@ -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_;
......
......@@ -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_;
......
......@@ -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;
......
......@@ -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<