text:
#include "ans.h"
#include "gaussian.h"

#include <fstream>
#include <vector>
#include <Eigen/Dense>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>

namespace py = pybind11;
using namespace pybind11::literals;

using VectorXd = Eigen::VectorXd;
using MatrixXd = Eigen::MatrixXd;
typedef Eigen::Array<symbol_t, Eigen::Dynamic, 1> ArrayXu;
typedef Eigen::Array<symbol_t, Eigen::Dynamic, Eigen::Dynamic> ArrayXXu;

class PyANS {
public:
    PyANS(int ans_mass_bits, int ans_init_bits, int ans_init_seed, int num_streams) {
        for (int i = 0; i < num_streams; ++i) {
            m_streams.emplace_back(ans_mass_bits, ans_init_bits, ans_init_seed + i);
        }
    }

    PyANS(py::list py_list) {
        // Construct straight from saved Python format
        from_py(py_list);
    }

    py::list to_py() {
        // Save all streams to a Python list
        assert_some_streams();
        py::list py_list;
        for (auto& st : m_streams) {
            // Store the stream state as a dictionary
            py_list.append(py::dict(
                "stream"_a=py::bytes((char*) &st.stream[0], st.stream.size() * sizeof(uint64_t)),
                "tip_lo"_a=(uint64_t) st.tip,
                "tip_hi"_a=(uint64_t) (st.tip >> 64),
                "mass_bits"_a=st.mass_bits
            ));
        }
        return py_list;
    }

    void from_py(py::list py_list) {
        // Load all streams from the saved Python format.
        // Note: this clears the current streams, and possibly changes the number of streams.
        m_streams.clear();
        for (auto dict : py_list) {
            // Create a stream with the appropriate setting of mass_bits
            m_streams.emplace_back(dict["mass_bits"].cast<int>(), 0, 0);
            auto& st = m_streams.back();
            // Load the tip data
            st.tip = (((uint128_t) dict["tip_hi"].cast<uint64_t>()) << 64)
                     | ((uint128_t) dict["tip_lo"].cast<uint64_t>());
            // Load the stream vector
            py::bytes py_stream = dict["stream"].cast<py::bytes>();
            char *buffer;
            ssize_t length;
            if (PYBIND11_BYTES_AS_STRING_AND_SIZE(py_stream.ptr(), &buffer, &length)) {
                throw std::runtime_error("Unable to extract bytes contents!");
            }
            if ((length % sizeof(uint64_t)) != 0) {
                throw std::runtime_error("Invalid stored stream length. Must be a sequence of 64-bit numbers.");
            }
            st.stream.resize(length / sizeof(uint64_t));
            memcpy(&st.stream[0], buffer, length);
        }
    }

    size_t stream_length() {
        size_t total = 0;
        for (const auto& ans : m_streams) {
            total += ans.length();
        }
        return total;
    }

    void encode_gaussian_diag(Eigen::Ref<ArrayXu> x, Eigen::Ref<VectorXd> means, Eigen::Ref<VectorXd> stds, const Discretization& disc, bool pad) {
        assert_some_streams();

        int dim = x.size();
        if (means.size() != dim || stds.size() != dim) {
            throw std::runtime_error("mismatched dimension");
        }
        if ((stds.array() < 0.).any()) {
            throw std::runtime_error("negative standard deviation");
        }
        parallelize(dim, [&](ANSBitstream& ans, int start, int end) {
            // encode backwards
            for (int i = end - 1; i >= start; --i) {
                DiscretizedGaussian g(means(i), stds(i), disc, ans.mass_bits, pad);
                symbol_t sym = x(i);
                ans.encode(g.pmf(sym), g.cdf(sym));
            }
        });
    }

    ArrayXu decode_gaussian_diag(Eigen::Ref<VectorXd> means, Eigen::Ref<VectorXd> stds, const Discretization& disc, bool pad) {
        assert_some_streams();

        int dim = means.size();
        if (stds.size() != dim) {
            throw std::runtime_error("mismatched dimension");
        }
        if ((stds.array() < 0.).any()) {
            throw std::runtime_error("negative standard deviation");
        }

        ArrayXu out(dim);
        parallelize(dim, [&](ANSBitstream& ans, int start, int end) {
            // decode forward
            for (int i = start; i < end; ++i) {
                DiscretizedGaussian g(means(i), stds(i), disc, ans.mass_bits, pad);
                auto p = ans.peek();
                symbol_t sym = g.inverse_cdf(p);
                out(i) = sym;
                ans.decode(p, g.pmf(sym), g.cdf(sym));
            }
        });
        return out;
    }

    void encode_gaussian_single(Eigen::Ref<ArrayXu> x, py::EigenDRef<MatrixXd> mean_coefs,
                                Eigen::Ref<VectorXd> biases, Eigen::Ref<VectorXd> stds,
                                bool left_to_right, const Discretization& disc, bool pad) {
        assert_some_streams();

        // single datapoint (vector from a multivariate Gaussian): always use stream 0
        generic_encode_gaussian(m_streams[0], x, mean_coefs, biases, stds, left_to_right, disc, pad);
    }

    ArrayXu decode_gaussian_single(py::EigenDRef<MatrixXd> mean_coefs,
                                   Eigen::Ref<VectorXd> biases, Eigen::Ref<VectorXd> stds,
                                   bool left_to_right, const Discretization& disc, bool pad) {
        assert_some_streams();

        int dim = stds.size();
        ArrayXu x(dim);
        // single datapoint (vector from a multivariate Gaussian): always use stream 0
        generic_decode_gaussian(m_streams[0], x, mean_coefs, biases, stds, left_to_right, disc, pad);
        return x;
    }

    void encode_gaussian_batched(py::EigenDRef<ArrayXXu> xs, py::EigenDRef<MatrixXd> mean_coefs,
                                 py::EigenDRef<MatrixXd> biases, Eigen::Ref<VectorXd> stds,
                                 bool left_to_right, const Discretization& disc, bool pad) {
        assert_some_streams();

        if (biases.rows() != xs.rows()) {
            throw std::runtime_error("inconsistent batch size for data and biases");
        }
        parallelize(xs.rows(), [&](ANSBitstream& ans, int start, int end) {
            for (int i = end - 1; i >= start; --i) {
                generic_encode_gaussian(ans, xs.row(i), mean_coefs, biases.row(i).transpose(), stds, left_to_right, disc, pad);
            }
        });
    }

    ArrayXXu decode_gaussian_batched(py::EigenDRef<MatrixXd> mean_coefs, /* (d,d) */
                                     py::EigenDRef<MatrixXd> biases, /* (n,d) */
                                     Eigen::Ref<VectorXd> stds, /* (d,1) */
                                     bool left_to_right, const Discretization& disc, bool pad) {
        assert_some_streams();

        int count = biases.rows();
        int dim = stds.size();
        ArrayXXu xs(count, dim);
        parallelize(count, [&](ANSBitstream& ans, int start, int end) {
            for (int i = start; i < end; ++i) {
                generic_decode_gaussian(ans, xs.row(i), mean_coefs, biases.row(i).transpose(), stds, left_to_right, disc, pad);
            }
        });
        return xs;
    }

private:
    std::vector<ANSBitstream> m_streams;

    void assert_some_streams() {
        if (m_streams.empty()) {
            throw std::runtime_error("No ANS streams present");
        }
    }

    // Distribute ANS streams over threads
    void parallelize(int total, const std::function<void(ANSBitstream&, int, int)>& f) {
        #pragma omp parallel for schedule(runtime)
        for (int i_stream = 0; i_stream < m_streams.size(); ++i_stream) {
            // Each stream takes care of a block of data
            int extra = total % m_streams.size();
            int start = (total / m_streams.size()) * i_stream + std::min(i_stream, extra);
            int end = start + (total / m_streams.size()) + (int) (i_stream < extra);
            f(m_streams[i_stream], start, end);
        }
    }

    // Calculates the mean of one component of a multivariate Gaussian arranged in a linear autoregressive fashion
    template<typename A, typename B, typename C>
    double calc_mean(const Eigen::DenseBase<A>& x, const Eigen::DenseBase<B>& mean_coefs, const Eigen::DenseBase<C>& biases,
                     int pos, int dim, bool left_to_right) {
        if (left_to_right) {
            // left-to-right sampling: mean_coefs is lower triangular
            if (pos == 0) { return biases(pos); }
            // return mean_coefs.row(pos).head(pos).dot(x.head(pos) - biases.head(pos)) + biases(pos); // note don't use this for determinism?
            double out = biases(pos);
            for (int i = 0; i < pos; ++i) {
                out += mean_coefs(pos,i) * (x(i) - biases(i));
            }
            return out;
        } else {
            // right-to-left sampling: mean_coefs is upper triangular
            if (pos == dim - 1) { return biases(pos); }
            // return mean_coefs.row(pos).tail(dim - pos - 1).dot(x.tail(dim - pos - 1) - biases.tail(dim - pos - 1)) + biases(pos);
            double out = biases(pos);
            for (int i = pos - 1; i < dim; ++i) {
                out += mean_coefs(pos,i) * (x(i) - biases(i));
            }
            return out;
        }
    }

    // Encoding for a multivariate Gaussian specified as a linear AR model
    template<typename A, typename B, typename C, typename D>
    void generic_encode_gaussian(ANSBitstream& ans,
                                 const Eigen::DenseBase<A>& x,
                                 const Eigen::DenseBase<B>& mean_coefs,
                                 const Eigen::DenseBase<C>& biases,
                                 const Eigen::MatrixBase<D>& stds,
                                 bool left_to_right, const Discretization& disc, bool pad) {
        int dim = x.size();
        if (mean_coefs.rows() != dim || mean_coefs.cols() != dim) {
            throw std::runtime_error("mismatched size for mean_coefs");
        }
        if (biases.size() != dim) {
            throw std::runtime_error("mismatched size for biases");
        }
        if (stds.size() != dim) {
            throw std::runtime_error("mismatched size for stds");
        }
        // TODO stricter checks here for rows & cols of everything
        if ((stds.array() < 0.).any()) {
            throw std::runtime_error("negative standard deviation");
        }

        VectorXd x_real(dim);
        for (int i = 0; i < dim; ++i) { x_real(i) = disc.symbol_to_real(x(i)); }

        for (int i = 0; i < dim; ++i) {
            int pos = left_to_right ? (dim - i - 1) : i; // encode backwards
            DiscretizedGaussian g(calc_mean(x_real, mean_coefs, biases, pos, dim, left_to_right), stds(pos), disc, ans.mass_bits, pad);
            symbol_t sym = x(pos);
            ans.encode(g.pmf(sym), g.cdf(sym));
        }
    }

    // Decoding for a multivariate Gaussian specified as a linear AR model
    template<typename A, typename B, typename C, typename D>
    void generic_decode_gaussian(ANSBitstream& ans,
                                 Eigen::DenseBase<A> const & out_x,
                                 const Eigen::DenseBase<B>& mean_coefs,
                                 const Eigen::DenseBase<C>& biases,
                                 const Eigen::MatrixBase<D>& stds,
                                 bool left_to_right, const Discretization& disc, bool pad) {
        int dim = stds.size();
        if (mean_coefs.rows() != dim || mean_coefs.cols() != dim) {
            throw std::runtime_error("mismatched size for mean_coefs");
        }
        if (biases.size() != dim) {
            throw std::runtime_error("mismatched size for biases");
        }
        if ((stds.array() < 0.).any()) {
            throw std::runtime_error("negative standard deviation");
        }

        VectorXd x_real = VectorXd::Zero(dim);
        for (int i = 0; i < dim; ++i) {
            int pos = left_to_right ? i : (dim - i - 1); // decode forwards
            DiscretizedGaussian g(calc_mean(x_real, mean_coefs, biases, pos, dim, left_to_right), stds(pos), disc, ans.mass_bits, pad);
            auto p = ans.peek();
            symbol_t sym = g.inverse_cdf(p);
            const_cast<Eigen::DenseBase<A>&>(out_x)(pos) = sym; // https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
            x_real(pos) = disc.symbol_to_real(sym);
            ans.decode(p, g.pmf(sym), g.cdf(sym));
        }
    }
};

PYBIND11_MODULE(fast_ans, m) {
    extern void run_tests();
    m.def("run_tests", &run_tests);

    py::class_<Discretization>(m, "Discretization")
        .def(py::init<int, int, int>(), "lo"_a, "hi"_a, "bits"_a)
        .def("real_to_symbol", py::vectorize(&Discretization::real_to_symbol))
        .def("symbol_to_real", py::vectorize(&Discretization::symbol_to_real))
    ;

    py::class_<PyANS>(m, "ANS")
        .def(py::init<int, int, int, int>(), "ans_mass_bits"_a, "ans_init_bits"_a, "ans_init_seed"_a, "num_streams"_a)
        .def(py::init<py::list>())
        .def("stream_length", &PyANS::stream_length)
        .def("to_py", &PyANS::to_py)
        .def("from_py", &PyANS::from_py)
        .def("encode_gaussian_diag", &PyANS::encode_gaussian_diag, "x"_a, "means"_a, "stds"_a, "disc"_a, "pad"_a)
        .def("decode_gaussian_diag", &PyANS::decode_gaussian_diag, "means"_a, "stds"_a, "disc"_a, "pad"_a)
        .def("encode_gaussian", &PyANS::encode_gaussian_single, "x"_a, "mean_coefs"_a, "biases"_a, "stds"_a, "left_to_right"_a, "disc"_a, "pad"_a)
        .def("decode_gaussian", &PyANS::decode_gaussian_single, "mean_coefs"_a, "biases"_a, "stds"_a, "left_to_right"_a, "disc"_a, "pad"_a)
        .def("encode_gaussian_batched", &PyANS::encode_gaussian_batched, "xs"_a, "mean_coefs"_a, "biases"_a, "stds"_a, "left_to_right"_a, "disc"_a, "pad"_a)
        .def("decode_gaussian_batched", &PyANS::decode_gaussian_batched, "mean_coefs"_a, "biases"_a, "stds"_a, "left_to_right"_a, "disc"_a, "pad"_a)
    ;
}

max_stars_repo_name: andiac/localbitsback

max_issues_repo_name: andiac/localbitsback

max_forks_repo_name: andiac/localbitsback


text:
//
// Copyright (c) 2018 CNRS
// Authors: Pierre Fernbach
//
// This file is part of bezier_COM_traj
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#define BOOST_TEST_MODULE transition
#include <boost/test/included/unit_test.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include "test_helper.hh"

#define NOMINAL_COM_HEIGHT 0.795

std::vector<double> computeDiscretizedTime(const VectorX& phaseTimings,const int pointsPerPhase ){
    std::vector<double> timeArray;
    double t = 0;
    double t_total = 0;
    for(long int i = 0 ; i < phaseTimings.size() ; ++i)
        t_total += phaseTimings[i];

    for(long int i = 0 ; i < phaseTimings.size() ; ++i){
        double step = (double) phaseTimings[i] / pointsPerPhase;
        for(int j = 0 ; j < pointsPerPhase ; ++j){
            t += step;
            timeArray.push_back(t);
        }
    }
    timeArray.pop_back();
    timeArray.push_back(t_total); // avoid numerical errors
    return timeArray;
}

bool check_constraints(const bezier_com_traj::ContactData& contactPhase, Vector3 c,Vector3 dc, Vector3 ddc){

    BOOST_CHECK(verifyKinematicConstraints(std::make_pair(contactPhase.Kin_,contactPhase.kin_),c));
    BOOST_CHECK(verifyStabilityConstraintsDLP(*contactPhase.contactPhase_,c,dc,ddc));
    BOOST_CHECK(verifyStabilityConstraintsPP(*contactPhase.contactPhase_,c,dc,ddc));
    return true;
}

void discretized_check(const bezier_com_traj::ProblemData& pData, const VectorX& Ts,
                       const bezier_com_traj::ResultDataCOMTraj& res, const int pointsPerPhase,
                       const double t_total)
{
     // check if timing is respected
    std::vector<double> timings = computeDiscretizedTime(Ts,pointsPerPhase);
    BOOST_CHECK_EQUAL(timings.back(),t_total);

    Vector3 c,dc,ddc;
    bezier_com_traj::bezier_t ct = res.c_of_t_;
    bezier_com_traj::bezier_t dct = ct.compute_derivate(1);
    bezier_com_traj::bezier_t ddct = dct.compute_derivate(1);

    BOOST_CHECK_EQUAL(ct.min(),0);
    BOOST_CHECK_EQUAL(dct.min(),0);
    BOOST_CHECK_EQUAL(ddct.min(),0);
    BOOST_CHECK_EQUAL(ct.max(),t_total);
    BOOST_CHECK_EQUAL(dct.max(),t_total);
    BOOST_CHECK_EQUAL(ddct.max(),t_total);

    // check if each discretized point is feasible :

    std::vector<int> stepIdForPhase; // stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
    for(int i = 0 ; i < Ts.size() ; ++i)
        stepIdForPhase.push_back(pointsPerPhase*(i+1)-1);
    std::size_t id_phase = 0;
    bezier_com_traj::ContactData phase = pData.contacts_[id_phase];

    for(size_t id_step = 0 ; id_step < timings.size() ; ++id_step){
        c = ct(timings[id_step]);
        dc = dct(timings[id_step]);
        ddc = ddct(timings[id_step]);

        // check i (c,dc,ddc) verify the constraints of current phase
        check_constraints(phase,c,dc,ddc);

        // check if we switch phases
        for(std::size_t i = 0 ; i < (stepIdForPhase.size()-1) ; ++i){
            if(id_step == (std::size_t)stepIdForPhase[i]){
                id_phase=i+1;
                phase = pData.contacts_[id_phase];
                check_constraints(phase,c,dc,ddc);
            }
        }
    }
}

void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shouldFail=false,bool continuous = false, bool test_continuous = true ){
    BOOST_CHECK_EQUAL(pData.contacts_.size(),Ts.size());

    double t_total = 0;
    for(long int i = 0 ; i < Ts.size() ; ++i)
        t_total += Ts[i];

    int pointsPerPhase = 5;

    // check if transition is feasible (should be)
    bezier_com_traj::ResultDataCOMTraj res;
    if(continuous)
    {
        // testing all available solvers
        res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG);
        if(pData.representation_ ==  bezier_com_traj::FORCE)
        {
            /*bezier_com_traj::ResultDataCOMTraj res2 =
                    bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG_SPARSE);
            BOOST_CHECK(res.success_ == res2.success_);
            if(res.success_)
            {
                std::cout << " x " << res.cost_ << std::endl;
                std::cout << " x2 " << res2.cost_ << std::endl;
                discretized_check(pData,Ts,res2,pointsPerPhase,t_total);
                BOOST_CHECK(!res.success_ || (res.x.head<3>() - res2.x.head<3>()).norm() < EPSILON);
            }*/
#ifdef USE_GLPK_SOLVER
            //clock_t s0,e0;
            //s0 = clock();
            bezier_com_traj::ResultDataCOMTraj res3 =
                    bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_GLPK);
            //e0 = clock();
            //std::cout<<"Time to perform full lp : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000.<<" ms "<<std::endl;
            BOOST_CHECK(res.success_ == res3.success_);
            if(res3.success_)
            {
                discretized_check(pData,Ts,res3,pointsPerPhase,t_total);
            }
#endif
        }
    }
    else
        res = bezier_com_traj::computeCOMTrajFixedSize(pData,Ts,pointsPerPhase);

    if(shouldFail){
        BOOST_CHECK(!res.success_);
        if(test_continuous)
        {
            res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG);
            BOOST_CHECK(!res.success_);
            pData.representation_ ==  bezier_com_traj::FORCE;
            res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_QUADPROG);
            BOOST_CHECK(!res.success_);
#ifdef USE_GLPK_SOLVER
            res = bezier_com_traj::computeCOMTraj(pData,Ts,-1,solvers::SOLVER_GLPK);
            BOOST_CHECK(!res.success_);
#endif
        }
        return;
    }

    BOOST_CHECK(res.success_);

    if(res.success_)
        discretized_check(pData,Ts,res,pointsPerPhase,t_total);
    if(continuous && pData.representation_ == bezier_com_traj::DOUBLE_DESCRIPTION)
    {
        pData.representation_ = bezier_com_traj::FORCE;
        check_transition(pData,Ts,shouldFail,true);
    }
    else if(!continuous && test_continuous)
        check_transition(pData,Ts,shouldFail,true);
    else{
        for(size_t i = 0 ; i < pData.contacts_.size() ; ++i){
            delete pData.contacts_[i].contactPhase_;
        }
    }

}

BOOST_AUTO_TEST_SUITE( flat_ground )

// one step (left foot) : init pos of end effector : (0,0.1)(0,-0.1)
//                                         end pos : (0.3,0.1)(0,-0.1)
// three phases : both feet in contact, only right feet, both feet

bezier_com_traj::ContactData phase0_flat(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(0,0.1,0);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(0,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,-0.1,0));
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,0.1,0)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase1_flat(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(1,3),positions(1,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(0,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,-0.1,0));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase2_flat(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(0.3,0.1,0);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(0,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,-0.1,0));
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.3,0.1,0)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ProblemData gen_problem_data_flat(){
    bezier_com_traj::ProblemData pData;
    pData.c0_ = Vector3(0,0,NOMINAL_COM_HEIGHT);
    pData.c1_ = Vector3(0.15,0,NOMINAL_COM_HEIGHT);
    pData.dc0_ = Vector3::Zero();
    pData.dc1_ = Vector3::Zero();
    pData.ddc0_ = Vector3::Zero();
    pData.ddc1_ = Vector3::Zero();

    pData.contacts_.push_back(phase0_flat());
    pData.contacts_.push_back(phase1_flat());
    pData.contacts_.push_back(phase2_flat());

    return pData;
}

BOOST_AUTO_TEST_CASE(quasi_static){

// compute kinematic constraints for the right foot :
    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,-0.1,0));
    ConstraintsPair kin0 = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0,0.1,0)));
    ConstraintsPair kin2 = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.3,0.1,0)));

    bezier_com_traj::ContactData cDataMid = phase1_flat();
    ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);

    std::pair<Matrix3,Vector3> Hg = computeCost();
    Vector3 init = Vector3::Zero();

    ConstraintsPair Ab_first = stackConstraints(kin0,stab);
    bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
    BOOST_CHECK(res_first.success_);

    ConstraintsPair Ab_second = stackConstraints(kin2,stab);
    bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
    BOOST_CHECK(res_second.success_);
    delete cDataMid.contactPhase_;
}

BOOST_AUTO_TEST_CASE(transition){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_noc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_no_terminal_constraints){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.contacts_.pop_back();
    pData.contacts_.pop_back();
    pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::INIT_ACC;
    VectorX Ts(1);
    Ts<<0.2;
    check_transition(pData,Ts,false,false,true);
}

BOOST_AUTO_TEST_CASE(transition_noDc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_ddc0){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
    pData.constraints_.flag_ |= bezier_com_traj::END_ACC;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_noAcc){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.constraintAcceleration_ = false;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_noDc1_noAcc){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
    pData.constraints_.constraintAcceleration_ = false;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}
BOOST_AUTO_TEST_CASE(transition_ddc0_noAcc){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
    pData.constraints_.constraintAcceleration_ = false;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_noAcc){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
    pData.constraints_.constraintAcceleration_ = false;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_Acc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.maxAcceleration_=1.;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,false,false,false); // fail with continuous formulation
}

BOOST_AUTO_TEST_CASE(transition_noDc1_Acc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
    pData.constraints_.maxAcceleration_=1.;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,false,false,false); // fail with continuous formulation
}
BOOST_AUTO_TEST_CASE(transition_ddc0_Acc2){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
    pData.constraints_.maxAcceleration_=2.;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,false,false,false); // fail with continuous formulation
}

BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc2){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
    pData.constraints_.maxAcceleration_=2.;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts);
}

BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc05){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
    pData.constraints_.maxAcceleration_=0.5;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,false,false,false); // fail with continuous formulation
}

BOOST_AUTO_TEST_CASE(transition_Acc05){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.maxAcceleration_=0.5;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,false,false,false); // fail with continuous formulation
}

// constraints that should fails :

BOOST_AUTO_TEST_CASE(transition_Acc02){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.maxAcceleration_=0.2;
    pData.constraints_.constraintAcceleration_=true;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,true);
}

BOOST_AUTO_TEST_CASE(transition_noDc1_Acc05){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
    pData.constraints_.maxAcceleration_=0.5;
    pData.constraints_.constraintAcceleration_=true;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,true);
}

BOOST_AUTO_TEST_CASE(transition_ddc0_Acc1){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC;
    pData.constraints_.maxAcceleration_=1.;
    pData.constraints_.constraintAcceleration_=true;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,true);
}

BOOST_AUTO_TEST_CASE(transition_ddc0_ddc1_Acc02){
    bezier_com_traj::ProblemData pData = gen_problem_data_flat();
    pData.constraints_.flag_ |= bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC ;
    pData.constraints_.maxAcceleration_=0.2;
    pData.constraints_.constraintAcceleration_=true;
    VectorX Ts(3);
    Ts<<0.6,0.6,0.6;
    check_transition(pData,Ts,true);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( platform )

// platform : first step :
// (0.35,0.1,0) ; (0.35,-0.1,0) -> (0.775, 0.23, -0.02);(0.35,-0.1,0)  (normal : 0.0, -0.423, 0.906)

// second step :
// (0.775, 0.23, -0.02);(0.35,-0.1,0) -> (0.775, 0.23, -0.02);(1.15,-0.1,0)
// unfeasible in quasi-static

//third step :
//(0.775, 0.23, -0.02);(1.15,-0.1,0) -> (1.15,0.1,0);(1.15,-0.1,0)

bezier_com_traj::ContactData phase0_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(0.35,0.1,0);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(0.35,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,-0.1,0));
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,0.1,0)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase1_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(1,3),positions(1,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(0.35,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,-0.1,0));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase2_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0.0, -0.423, 0.906).normalized();
    positions.block<1,3>(0,0)=Vector3(0.775, 0.23, -0.02);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(0.35,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(0.35,-0.1,0));
    Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0.0, -0.423, 0.906));
    Matrix3 rot = quat.normalized().toRotationMatrix();
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(rot,Vector3(0.775, 0.23, -0.02)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase3_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(1,3),positions(1,3);
    normals.block<1,3>(0,0)=Vector3(0.0, -0.423, 0.906).normalized();
    positions.block<1,3>(0,0)=Vector3(0.775, 0.23, -0.02);

    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0.0, -0.423, 0.906));
    Matrix3 rot = quat.normalized().toRotationMatrix();
    ConstraintsPair kin = generateKinematicsConstraints(rot,Vector3(0.775, 0.23, -0.02));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase4_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0.0, -0.423, 0.906).normalized();
    positions.block<1,3>(0,0)=Vector3(0.775, 0.23, -0.02);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(1.15,-0.1,0);
    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,-0.1,0));
    Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0.0, -0.423, 0.906));
    Matrix3 rot = quat.normalized().toRotationMatrix();
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(rot,Vector3(0.775, 0.23, -0.02)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase5_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(1,3),positions(1,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(1.15,-0.1,0);

    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin = generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,-0.1,0));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ContactData phase6_platform(){
    bezier_com_traj::ContactData cData;
    MatrixX3 normals(2,3),positions(2,3);
    normals.block<1,3>(0,0)=Vector3(0,0,1);
    positions.block<1,3>(0,0)=Vector3(1.15,-0.1,0);
    normals.block<1,3>(1,0)=Vector3(0,0,1);
    positions.block<1,3>(1,0)=Vector3(1.15,0.1,0);

    std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals,positions,LX,LY);
    cData.contactPhase_  = new centroidal_dynamics::Equilibrium(ComputeContactCone(contacts.first,contacts.second));

    ConstraintsPair kin1 = generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,-0.1,0));
    ConstraintsPair kin = stackConstraints(kin1,generateKinematicsConstraints(Matrix3::Identity(),Vector3(1.15,0.1,0)));
    cData.Kin_ = kin.first;
    cData.kin_ = kin.second;

    return cData;
}

bezier_com_traj::ProblemData gen_problem_data_Step0(){
    bezier_com_traj::ProblemData pData;
    pData.c0_ = Vector3(0.35,0,NOMINAL_COM_HEIGHT);
    pData.c1_ = Vector3(0.56,0.04,NOMINAL_COM_HEIGHT);
    pData.dc0_ = Vector3::Zero();
    pData.dc1_ = Vector3::Zero();
    pData.ddc0_ = Vector3::Zero();
    pData.ddc1_ = Vector3::Zero();

    pData.contacts_.push_back(phase0_platform());
    pData.contacts_.push_back(phase1_platform());
    pData.contacts_.push_back(phase2_platform());

    return pData;
}

bezier_com_traj::ProblemData gen_problem_data_Step1(){
    bezier_com_traj::ProblemData pData;
    pData.c0_ = Vector3(0.56,0.04,0.765);
    pData.c1_ = Vector3(0.98,0.02,0.77);
    pData.dc0_ = Vector3::Zero();
    pData.dc1_ = Vector3::Zero();
    pData.ddc0_ = Vector3::Zero();
    pData.ddc1_ = Vector3::Zero();

    pData.contacts_.push_back(phase2_platform());
    pData.contacts_.push_back(phase3_platform());
    pData.contacts_.push_back(phase4_platform());

    return pData;
}

bezier_com_traj::ProblemData gen_problem_data_Step2(){
    bezier_com_traj::ProblemData pData;
    pData.c0_ = Vector3(0.98,0.04,NOMINAL_COM_HEIGHT);
    pData.c1_ = Vector3(1.15,0,NOMINAL_COM_HEIGHT);
    pData.dc0_ = Vector3::Zero();
    pData.dc1_ = Vector3::Zero();
    pData.ddc0_ = Vector3::Zero();
    pData.ddc1_ = Vector3::Zero();

    pData.contacts_.push_back(phase4_platform());
    pData.contacts_.push_back(phase5_platform());
    pData.contacts_.push_back(phase6_platform());

    return pData;
}

BOOST_AUTO_TEST_CASE(quasi_static_0){
    // should be successfull in quasiStatic
// compute kinematic constraints for the right foot :
    bezier_com_traj::ContactData cDataFirst = phase0_platform();
    bezier_com_traj::ContactData cDataMid = phase1_platform();
    bezier_com_traj::ContactData cDataSecond = phase2_platform();

    ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_,cDataFirst.kin_);
    ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_,cDataSecond.kin_);
    ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
    std::pair<Matrix3,Vector3> Hg = computeCost();
    Vector3 init = Vector3::Zero();

    ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
    bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
    BOOST_CHECK(res_first.success_);

    ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
    bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
    BOOST_CHECK(res_second.success_);

    delete cDataFirst.contactPhase_;
    delete cDataMid.contactPhase_;
    delete cDataSecond.contactPhase_;
}

BOOST_AUTO_TEST_CASE(quasi_static_1){
    // should NOT be successfull in quasiStatic
// compute kinematic constraints for the right foot :
    bezier_com_traj::ContactData cDataFirst = phase2_platform();
    bezier_com_traj::ContactData cDataMid = phase3_platform();
    bezier_com_traj::ContactData cDataSecond = phase4_platform();

    ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_,cDataFirst.kin_);
    ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_,cDataSecond.kin_);
    ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
    std::pair<Matrix3,Vector3> Hg = computeCost();
    Vector3 init = Vector3::Zero();

    ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
    bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
    BOOST_CHECK(! res_first.success_);

    ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
    bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
    BOOST_CHECK(! res_second.success_);

    delete cDataFirst.contactPhase_;
    delete cDataMid.contactPhase_;
    delete cDataSecond.contactPhase_;
}

BOOST_AUTO_TEST_CASE(quasi_static_2){
    // should be successfull in quasiStatic
// compute kinematic constraints for the right foot :
    bezier_com_traj::ContactData cDataFirst = phase4_platform();
    bezier_com_traj::ContactData cDataMid = phase5_platform();
    bezier_com_traj::ContactData cDataSecond = phase6_platform();

    ConstraintsPair kin_first = std::make_pair(cDataFirst.Kin_,cDataFirst.kin_);
    ConstraintsPair kin_second = std::make_pair(cDataSecond.Kin_,cDataSecond.kin_);
    ConstraintsPair stab = generateStabilityConstraints(*cDataMid.contactPhase_);
    std::pair<Matrix3,Vector3> Hg = computeCost();
    Vector3 init = Vector3::Zero();

    ConstraintsPair Ab_first = stackConstraints(kin_first,stab);
    bezier_com_traj::ResultData res_first = bezier_com_traj::solve(Ab_first,Hg,init);
    BOOST_CHECK(res_first.success_);

    ConstraintsPair Ab_second = stackConstraints(kin_second,stab);
    bezier_com_traj::ResultData res_second = bezier_com_traj::solve(Ab_second,Hg,init);
    BOOST_CHECK(res_second.success_);

    delete cDataFirst.contactPhase_;
    delete cDataMid.contactPhase_;
    delete cDataSecond.contactPhase_;
}

BOOST_AUTO_TEST_CASE(transition_0){
    bezier_com_traj::ProblemData pData = gen_problem_data_Step0();
    VectorX Ts(3);
    Ts<<0.8,0.6,0.8;
    check_transition(pData,Ts);

}

BOOST_AUTO_TEST_CASE(transition_1){
    bezier_com_traj::ProblemData pData = gen_problem_data_Step1();
    VectorX Ts(3);
    Ts<<0.4,0.2,0.4;

    check_transition(pData,Ts);

}

BOOST_AUTO_TEST_CASE(transition_2){
    bezier_com_traj::ProblemData pData = gen_problem_data_Step2();
    VectorX Ts(3);
    Ts<<0.8,0.6,0.8;

    check_transition(pData,Ts);

}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: jmirabel/hpp-bezier-com-traj

max_issues_repo_name: jmirabel/hpp-bezier-com-traj

max_forks_repo_name: jmirabel/hpp-bezier-com-traj


text:
#include "Config.h"

#include <boost/assign/list_of.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <iostream>
#include <fstream>
#include <iterator>

Nova::Config::Config(){
}

void
Nova::Config::Parse(const std::vector<std::string>& paths, int argc, char** argv){

    std::string config_file;
    // Declare a group of options that will be
    // allowed only on command line
    po::options_description generic("Generic options");
    generic.add_options()
        ("version,v", "print version string")
        ("help", "produce help message")
        ("config,c", po::value<std::string>(&config_file), "name of a file of a configuration.")
        ("scenepath,s", po::value<std::string>(&scenepath), "path to the scene to display.")
        ("fontname",po::value<std::string>(&fontname)->default_value(std::string("fonts/arial.ttf")),"path to the font to use for rendering")
        ;

    // Declare a group of options that will be
    // allowed both on command line and in
    // config file
    po::options_description config("Configuration");
    config.add_options()
        ("FPS,f", po::value<float>(&fps)->default_value(24.0f),"frame rate for playback.")
        ("Bind", po::value<std::vector<std::string> >(&bindings)->default_value(std::vector<std::string>(), ""), "Bindings to add to the Keybinding map." )
        ("Plugin", po::value<std::vector<std::string> >(&pluginList), "a list of plugins to load")
        ("PluginSearchPath", po::value<std::vector<std::string> >(&pluginpaths)->default_value(std::vector<std::string>(1,"./plugins"),"./plugins"), "path to search for plugins, in addition to the scene directory.")
        ("ShaderSearchPath", po::value<std::vector<std::string> >(&shaderpaths)->default_value(std::vector<std::string>(1,"./shaders"),"./shaders"), "path to search for shaders, in addition to the scene directory.")
        ("TextureSearchPath", po::value<std::vector<std::string> >(&texturepaths)->default_value(std::vector<std::string>(1,"./textures"),"./textures"), "path to search for textures, in addition to the scene directory.")
        ;

    // Hidden options, will be allowed both on command line and
    // in config file, but will not be shown to the user.
    po::options_description hidden("Hidden options");
    hidden.add_options()
        ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config).add(hidden);

    po::options_description config_file_options;
    config_file_options.add(config).add(hidden);

    po::options_description visible("Allowed options");
    visible.add(generic).add(config);

    po::positional_options_description p;
    p.add("scenepath", -1);

    try{
        po::variables_map vm;
        store(po::command_line_parser(argc, argv).
              options(cmdline_options).positional(p).run(), vm);
        notify(vm);

        if (vm.count("help")) {
            std::cout << visible << std::endl;;
            exit(0);
        }

        if (vm.count("version")) {
            std::cout << "Nova Program, No Version" << std::endl;;
            exit(0);
        }

        if( vm.count("config") ){
            std::ifstream ifs(config_file.c_str());
            if (!ifs)
            {
                std::cout << "can not open config file: " << config_file << std::endl;
                return;
            }
            else
                {
                store(parse_config_file(ifs, config_file_options), vm);
                notify(vm);
                }
        }
        else{
            for( auto iter = paths.rbegin(); iter != paths.rend(); iter++ ){
                std::ifstream ifs(iter->c_str());
                if (!ifs)
                    continue;
                else{
                    store(parse_config_file(ifs, config_file_options), vm);
                    notify(vm);
                    break;
                }
            }
        }
    }
    catch( std::exception& e ){
        std::cout << e.what() << std::endl;
        exit(1);
    }
}


text:
// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include "vw/core/vw_versions.h"

#include "vw/core/global_data.h"
#include "vw/core/version.h"
#include "vw/io/logger.h"

#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

using namespace VW;

BOOST_AUTO_TEST_CASE(verify_vw_versions)
{
  using namespace VW::version_definitions;

  // check default vw version value
  auto null_logger = VW::io::create_null_logger();
  workspace dummy_vw(null_logger);
  BOOST_CHECK(dummy_vw.model_file_ver == EMPTY_VERSION_FILE);
  BOOST_CHECK(dummy_vw.model_file_ver < VERSION_FILE_WITH_CB_ADF_SAVE);

  BOOST_CHECK(VERSION_FILE_WITH_RANK_IN_HEADER < VERSION_FILE_WITH_INTERACTIONS);
  BOOST_CHECK(VERSION_FILE_WITH_CB_ADF_SAVE < VERSION_FILE_WITH_CCB_MULTI_SLOTS_SEEN_FLAG);
  BOOST_CHECK(VERSION_FILE_WITH_CCB_MULTI_SLOTS_SEEN_FLAG < VERSION_FILE_WITH_CB_TO_CBADF);
}

BOOST_AUTO_TEST_CASE(verify_vw_version_operators)
{
  BOOST_CHECK((VW::version_struct{0, 0, 0} == VW::version_struct{0, 0, 0}));
  BOOST_CHECK((VW::version_struct{1, 1, 1} == VW::version_struct{1, 1, 1}));
  BOOST_CHECK((VW::version_struct{1, 1, 0} != VW::version_struct{1, 1, 1}));
  BOOST_CHECK((VW::version_struct{0, 0, 0} < VW::version_struct{0, 0, 1}));
  BOOST_CHECK((VW::version_struct{0, 0, 1} > VW::version_struct{0, 0, 0}));
  BOOST_CHECK((VW::version_struct{1, 0, 1} > VW::version_struct{0, 5, 0}));
  BOOST_CHECK((VW::version_struct{1, 2, 3} <= VW::version_struct{1, 2, 3}));
  BOOST_CHECK((VW::version_struct{1, 2, 2} <= VW::version_struct{1, 2, 3}));
  BOOST_CHECK((VW::version_struct{1, 2, 3} >= VW::version_struct{1, 2, 3}));
  BOOST_CHECK((VW::version_struct{1, 2, 4} >= VW::version_struct{1, 2, 3}));
  BOOST_CHECK((VW::version_struct{5, 70, 80} < VW::version_struct{11, 1, 1}));
}

BOOST_AUTO_TEST_CASE(verify_vw_version_tostring)
{
  BOOST_CHECK_EQUAL((VW::version_struct{0, 0, 0}.to_string()), "0.0.0");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 1, 1}.to_string()), "1.1.1");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 1, 0}.to_string()), "1.1.0");
  BOOST_CHECK_EQUAL((VW::version_struct{0, 0, 0}.to_string()), "0.0.0");
  BOOST_CHECK_EQUAL((VW::version_struct{0, 0, 1}.to_string()), "0.0.1");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 0, 1}.to_string()), "1.0.1");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 2, 3}.to_string()), "1.2.3");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 2, 2}.to_string()), "1.2.2");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 2, 3}.to_string()), "1.2.3");
  BOOST_CHECK_EQUAL((VW::version_struct{1, 2, 4}.to_string()), "1.2.4");
  BOOST_CHECK_EQUAL((VW::version_struct{5, 70, 80}.to_string()), "5.70.80");
}

BOOST_AUTO_TEST_CASE(verify_vw_version_fromstring)
{
  BOOST_CHECK((VW::version_struct{0, 0, 0} == VW::version_struct{"0.0.0"}));
  BOOST_CHECK((VW::version_struct{1, 1, 1} == VW::version_struct{"1.1.1"}));
  BOOST_CHECK((VW::version_struct{1, 1, 0} == VW::version_struct{"1.1.0"}));
  BOOST_CHECK((VW::version_struct{0, 0, 0} == VW::version_struct{"0.0.0"}));
  BOOST_CHECK((VW::version_struct{0, 0, 1} == VW::version_struct{"0.0.1"}));
  BOOST_CHECK((VW::version_struct{1, 0, 1} == VW::version_struct{"1.0.1"}));
  BOOST_CHECK((VW::version_struct{1, 2, 3} == VW::version_struct{"1.2.3"}));
  BOOST_CHECK((VW::version_struct{1, 2, 2} == VW::version_struct{"1.2.2"}));
  BOOST_CHECK((VW::version_struct{1, 2, 3} == VW::version_struct{"1.2.3"}));
  BOOST_CHECK((VW::version_struct{1, 2, 4} == VW::version_struct{"1.2.4"}));
  BOOST_CHECK((VW::version_struct{5, 70, 80} == VW::version_struct{"5.70.80"}));
}

ext: cc