From f53c7e9bdda3f8b8f98fcc2f3788447efd54dbb5 Mon Sep 17 00:00:00 2001 From: SimonRohou Date: Thu, 17 Feb 2022 21:55:56 +0100 Subject: [PATCH] [tube] polynomial representation of tubes --- .../core/domains/tube/codac_py_TubeVector.cpp | 4 +- python/src/robotics/codac_py_TPlane.cpp | 2 +- src/core/CMakeLists.txt | 1 + src/core/domains/tube/codac_Tube.h | 6 +- .../tube/codac_TubePolynomialSynthesis.cpp | 80 +++++++++---------- .../tube/codac_TubePolynomialSynthesis.h | 34 ++++++-- src/core/domains/tube/codac_TubeSynthesis.h | 19 +++++ src/core/domains/tube/codac_TubeVector.cpp | 4 +- src/core/domains/tube/codac_TubeVector.h | 9 ++- src/robotics/loops/codac_TPlane.cpp | 12 +-- src/robotics/loops/codac_TPlane.h | 9 +++ 11 files changed, 111 insertions(+), 69 deletions(-) create mode 100644 src/core/domains/tube/codac_TubeSynthesis.h diff --git a/python/src/core/domains/tube/codac_py_TubeVector.cpp b/python/src/core/domains/tube/codac_py_TubeVector.cpp index 174f0c318..32f37c941 100644 --- a/python/src/core/domains/tube/codac_py_TubeVector.cpp +++ b/python/src/core/domains/tube/codac_py_TubeVector.cpp @@ -424,8 +424,8 @@ void export_TubeVector(py::module& m) // Tree synthesis structure .def("enable_synthesis", &TubeVector::enable_synthesis, - TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE, - "enable"_a=SynthesisMode::BINARY_TREE) + TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE_DOUBLE, + "enable"_a=SynthesisMode::BINARY_TREE, "eps"_a=1.e-3) .def("integral", (const IntervalVector (TubeVector::*)(double) const)&TubeVector::integral, TUBEVECTOR_CONSTINTERVALVECTOR_INTEGRAL_DOUBLE, diff --git a/python/src/robotics/codac_py_TPlane.cpp b/python/src/robotics/codac_py_TPlane.cpp index 8ab94b09d..9a26133ef 100644 --- a/python/src/robotics/codac_py_TPlane.cpp +++ b/python/src/robotics/codac_py_TPlane.cpp @@ -44,7 +44,7 @@ void export_TPlane(py::module& m) p_.enable_synthesis(SynthesisMode::BINARY_TREE); v_.enable_synthesis(SynthesisMode::BINARY_TREE); tplane.compute_detections(precision, p_, v_); - //p_.enable_synthesis(SynthesisMode::POLYNOMIAL); + //p_.enable_synthesis(SynthesisMode::POLYNOMIAL, 5.e-7); tplane.compute_proofs(p_, v_); }, TPLANE_VOID_COMPUTE_DETECTIONS_FLOAT_TUBEVECTOR_TUBEVECTOR, diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 73e698ec4..a9b8b9875 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -69,6 +69,7 @@ ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_Tube_operators.cpp ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.h ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeSynthesis.h ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.h ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.cpp ${CMAKE_CURRENT_SOURCE_DIR}/domains/slice/codac_Slice.h diff --git a/src/core/domains/tube/codac_Tube.h b/src/core/domains/tube/codac_Tube.h index cc6684fcf..07dec63d6 100644 --- a/src/core/domains/tube/codac_Tube.h +++ b/src/core/domains/tube/codac_Tube.h @@ -22,6 +22,7 @@ #include "codac_tube_arithmetic.h" #include "codac_TubeTreeSynthesis.h" #include "codac_TubePolynomialSynthesis.h" +#include "codac_TubeSynthesis.h" #include "codac_Polygon.h" #include "codac_BoolInterval.h" @@ -37,8 +38,6 @@ namespace codac class TubeTreeSynthesis; class TubePolynomialSynthesis; - enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL }; - /** * \class Tube * \brief One dimensional tube \f$[x](\cdot)\f$, defined as an interval of scalar trajectories @@ -1014,7 +1013,8 @@ namespace codac * * \note The synthesis tree speeds up computations such as integrals or evaluations * - * \param enable boolean + * \param mode mode of synthesis + * \param eps precision of the polynomial approximation, if selected */ void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const; diff --git a/src/core/domains/tube/codac_TubePolynomialSynthesis.cpp b/src/core/domains/tube/codac_TubePolynomialSynthesis.cpp index f56327704..6c15478a5 100644 --- a/src/core/domains/tube/codac_TubePolynomialSynthesis.cpp +++ b/src/core/domains/tube/codac_TubePolynomialSynthesis.cpp @@ -39,10 +39,12 @@ namespace codac m_tube_ref(x), m_tdomain(tdomain), m_upper_bound(upper_bound), - m_p(polynomial_traj(upper_bound)), + m_p(polynomial_factoredform(upper_bound)), + m_codomain(m_p.f(m_tdomain)), m_fig(fig) { - if(fabs(m_p.offset) > eps) + if(fabs(m_p.offset) > eps + && x.time_to_index(tdomain.ub()) - x.time_to_index(tdomain.lb()) > 3) { LargestFirst bisector; pair t = @@ -57,7 +59,7 @@ namespace codac ostringstream ostr; ostr << m_upper_bound << m_tdomain; m_fig.add_trajectory(traj, ostr.str(), m_upper_bound ? "blue" : "red"); - m_fig.draw_box(IntervalVector({m_tdomain, m_p.codomain}), m_upper_bound ? "blue" : "red"); + m_fig.draw_box(IntervalVector({m_tdomain, m_codomain}), m_upper_bound ? "blue" : "red"); // todo: manage delete } } @@ -80,24 +82,13 @@ namespace codac return m_left->operator()(t) | m_right->operator()(t); else if(t.is_superset(m_tdomain)) - return m_p.codomain; + return m_codomain; else - return f(m_p, t & m_tdomain); + return m_p.f(t & m_tdomain); } - Interval TubePolynomialTreeSynthesis::f(const PolynomialSubtraj& p, const Interval& t) const - { - if(p.factor_form) - return p.c*sqr(t-p.a)+p.b+p.offset; - - Interval x = p.offset; - for(int i = 0 ; i < POLYNOMIAL_ORDER + 1 ; i++) - x += p.coeff[i]*pow(t,i); - return x; - } - - void TubePolynomialTreeSynthesis::polyfit(const vector &t, const vector &v, array &coeff) const + Polynomial TubePolynomialTreeSynthesis::polyfit(const vector &t, const vector &v) const { // Create a matrix placeholder of size (n x k), // n = nb of datapoints, @@ -113,11 +104,11 @@ namespace codac T(i, j) = std::pow(t.at(i), j); // Linear least square fit + Polynomial p; result = T.householderQr().solve(V); for(int k = 0 ; k < POLYNOMIAL_ORDER + 1 ; k++) - coeff[k] = result[k]; - - + p.coeff[k] = result[k]; + return p; } void TubePolynomialTreeSynthesis::get_bounds(const Interval& tdomain, bool upper_bound, vector& t, vector& v) const @@ -139,50 +130,51 @@ namespace codac assert(t.size() > 3); } - Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialSubtraj& p) const + Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialFactoredForm& p) const { // todo: analytical traj? Trajectory traj; - double dt = m_tdomain.diam()/1000.; + double dt = m_tdomain.diam()/10000.; for(double t = m_tdomain.lb() ; t < m_tdomain.ub() + dt ; t += dt) { double t_ = std::min(t, m_tdomain.ub()); - traj.set(f(p,t_).mid(), t_); + traj.set(p.f(t_).mid(), t_); } return traj; } - PolynomialSubtraj TubePolynomialTreeSynthesis::polynomial_traj(bool upper_bound) const + PolynomialFactoredForm TubePolynomialTreeSynthesis::polynomial_factoredform(bool upper_bound) const { - PolynomialSubtraj p; + PolynomialFactoredForm p_factored; vector t,v; get_bounds(m_tdomain, upper_bound, t, v); - polyfit(t, v, p.coeff); + Polynomial p = polyfit(t, v); - double offset = 0.; - for(size_t i = 0 ; i < t.size() ; i++) + if(p.coeff[2] != 0.) // avoid div by zero { - Interval diff = v[i]-f(p, t[i]); - if(upper_bound) - offset = std::max(offset, diff.ub()); - else - offset = std::min(offset, diff.lb()); + p_factored.a = -p.coeff[1]/(2*p.coeff[2]); + p_factored.b = p.f(p_factored.a).mid(); + + double x = m_tdomain.lb(); + if(x == p_factored.a) + x = m_tdomain.ub(); // selecting another x: avoiding div by zero + + p_factored.c = (p.f(x).mid()-p_factored.b)/std::pow(x-p_factored.a,2); } - if(p.coeff[2] != 0.) + else + assert(false && "TubePolynomialTreeSynthesis: unable to factorize"); + + for(size_t i = 0 ; i < t.size() ; i++) { - p.a = -p.coeff[1]/(2*p.coeff[2]); - p.b = f(p, p.a).mid(); - if(m_tdomain.mid() != p.a) - { - p.c = (f(p, m_tdomain.mid()).mid()-p.b)/std::pow(m_tdomain.mid()-p.a,2); - p.factor_form = true; - } + Interval diff = v[i]-p.f(t[i]); + if(upper_bound) + p_factored.offset = (i == 0) ? diff.ub() : std::max(p_factored.offset, diff.ub()); + else + p_factored.offset = (i == 0) ? diff.lb() : std::min(p_factored.offset, diff.lb()); } - p.offset = offset; - p.codomain = f(p,m_tdomain); - return p; + return p_factored; } } \ No newline at end of file diff --git a/src/core/domains/tube/codac_TubePolynomialSynthesis.h b/src/core/domains/tube/codac_TubePolynomialSynthesis.h index e886a4d3c..8a569a438 100644 --- a/src/core/domains/tube/codac_TubePolynomialSynthesis.h +++ b/src/core/domains/tube/codac_TubePolynomialSynthesis.h @@ -23,13 +23,31 @@ namespace codac { - struct PolynomialSubtraj + struct Polynomial { - Interval codomain; + // Form: ax² + bx + c + offset = 0 std::array coeff; double offset = 0.; + + Interval f(const Interval& t) const + { + Interval x = offset; + for(int i = 0 ; i < POLYNOMIAL_ORDER+1 ; i++) + x += coeff[i]*pow(t,i); + return x; + } + }; + + struct PolynomialFactoredForm + { + // Form: c(x-a)² + b + offset = 0 double a, b, c; - bool factor_form = false; + double offset = 0.; + + Interval f(const Interval& t) const + { + return c*sqr(t-a) + b + offset; + } }; class VIBesFigTube; @@ -45,11 +63,10 @@ namespace codac protected: - Interval f(const PolynomialSubtraj& p, const Interval& t) const; - void polyfit(const std::vector &t, const std::vector &v, std::array &coeff) const; + Polynomial polyfit(const std::vector &t, const std::vector &v) const; void get_bounds(const Interval& tdomain, bool upper_bound, std::vector& t, std::vector& v) const; - Trajectory traj_from_polynom(const PolynomialSubtraj& p) const; - PolynomialSubtraj polynomial_traj(bool upper_bound) const; + Trajectory traj_from_polynom(const PolynomialFactoredForm& p) const; + PolynomialFactoredForm polynomial_factoredform(bool upper_bound) const; protected: @@ -57,7 +74,8 @@ namespace codac const Tube& m_tube_ref; const Interval m_tdomain; bool m_upper_bound; - PolynomialSubtraj m_p; + PolynomialFactoredForm m_p; + Interval m_codomain; TubePolynomialTreeSynthesis *m_left = nullptr, *m_right = nullptr; VIBesFigTube& m_fig; }; diff --git a/src/core/domains/tube/codac_TubeSynthesis.h b/src/core/domains/tube/codac_TubeSynthesis.h new file mode 100644 index 000000000..1b4ed5a55 --- /dev/null +++ b/src/core/domains/tube/codac_TubeSynthesis.h @@ -0,0 +1,19 @@ +/** + * TubeSynthesis class + * ---------------------------------------------------------------------------- + * \date 2022 + * \author Simon Rohou + * \copyright Copyright 2022 Codac Team + * \license This program is distributed under the terms of + * the GNU Lesser General Public License (LGPL). + */ + +#ifndef __CODAC_TUBESYNTHESIS_H__ +#define __CODAC_TUBESYNTHESIS_H__ + +namespace codac +{ + enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL }; +} + +#endif \ No newline at end of file diff --git a/src/core/domains/tube/codac_TubeVector.cpp b/src/core/domains/tube/codac_TubeVector.cpp index 6c7bfc4dc..4fd72390c 100644 --- a/src/core/domains/tube/codac_TubeVector.cpp +++ b/src/core/domains/tube/codac_TubeVector.cpp @@ -877,10 +877,10 @@ namespace codac // Tree synthesis structure - void TubeVector::enable_synthesis(SynthesisMode mode) const + void TubeVector::enable_synthesis(SynthesisMode mode, double eps) const { for(int i = 0 ; i < size() ; i++) - (*this)[i].enable_synthesis(mode); + (*this)[i].enable_synthesis(mode, eps); } // Integration diff --git a/src/core/domains/tube/codac_TubeVector.h b/src/core/domains/tube/codac_TubeVector.h index 7931f0411..c1ebb5f35 100644 --- a/src/core/domains/tube/codac_TubeVector.h +++ b/src/core/domains/tube/codac_TubeVector.h @@ -21,13 +21,13 @@ #include "codac_tube_arithmetic.h" #include "codac_serialize_tubes.h" #include "codac_BoolInterval.h" +#include "codac_TubeSynthesis.h" namespace codac { class TFnc; class Tube; class Trajectory; - enum class SynthesisMode; /** * \class TubeVector @@ -1022,10 +1022,11 @@ namespace codac * \brief Enables the computation of a synthesis tree * * \note The synthesis tree speeds up computations such as integrals or evaluations - * - * \param enable boolean + * + * \param mode mode of synthesis + * \param eps precision of the polynomial approximation, if selected */ - void enable_synthesis(SynthesisMode mode) const; + void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const; /// @} /// \name Integration diff --git a/src/robotics/loops/codac_TPlane.cpp b/src/robotics/loops/codac_TPlane.cpp index 477bddeb9..c2a63ef45 100644 --- a/src/robotics/loops/codac_TPlane.cpp +++ b/src/robotics/loops/codac_TPlane.cpp @@ -147,16 +147,18 @@ namespace codac void TPlane::compute_proofs(const TubeVector& p) { auto f = std::bind(&f_p, p, _1); - - for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++) - if(m_v_detected_loops[i].zero_proven(f)) - m_v_proven_loops.push_back(m_v_detected_loops[i]); + compute_proofs(f); } void TPlane::compute_proofs(const TubeVector& p, const TubeVector& v) { - clock_t t_start = clock(); auto f = std::bind(&f_pv, p, v, _1); + compute_proofs(f); + } + + void TPlane::compute_proofs(const function& f) + { + clock_t t_start = clock(); m_v_proven_loops.clear(); for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++) diff --git a/src/robotics/loops/codac_TPlane.h b/src/robotics/loops/codac_TPlane.h index 7b6ba7415..0af6c35ea 100644 --- a/src/robotics/loops/codac_TPlane.h +++ b/src/robotics/loops/codac_TPlane.h @@ -171,6 +171,15 @@ namespace codac protected: + /** + * \brief Tries to prove the existence of loops in each detection set + * + * \note The tplane must have been computed beforehand. + * + * \param f the inclusion function \f$[\mathbf{f}]:\mathbb{IR}^2\to\mathbb{IR}^2\f$ + */ + void compute_proofs(const std::function& f); + /** * \brief Recursive computation of the tplane, from the tube of positions \f$[\mathbf{p}](\cdot)\f$ * and the tube of velocities \f$[\mathbf{v}](\cdot)\f$.