diff --git a/include/hpp/bezier-com-traj/utils.hh b/include/hpp/bezier-com-traj/utils.hh
index 01239a15210843fbae7bc344b46cebe610c63409..aaf39f490a5566276192d714d6c5486b7468da67 100644
--- a/include/hpp/bezier-com-traj/utils.hh
+++ b/include/hpp/bezier-com-traj/utils.hh
@@ -33,6 +33,16 @@ struct waypoint_t {
   waypoint_t(MatrixXX A, VectorX b) : first(A), second(b) {}
 
   static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
+
+  size_t size() const{return second.size();}
+
+  bool isApprox(const waypoint_t& other, const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const{
+    return first.isApprox(other.first,prec) && second.isApprox(other.second,prec);
+  }
+
+  bool operator==(const waypoint_t& other) const{ return isApprox(other); }
+
+  bool operator!=(const waypoint_t& other) const{ return !(*this == other); }
 };
 
 /**