[transition test] add methode solveOneStep()

include/bezier-com-traj/solve.hh

@@ -24,6 +24,34 @@ namespace bezier_com_traj
      BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep = -1);
 
 
+     void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res);
+
+
+
+     /// Methods for transition test :
+
+     /**
+      * @brief solveIntersection Solve the QP problem, that find a point inside the constraints Ab that minimise the cost Hg
+      * @param Ab s.t. Ax <= b
+      * @param Hg min  x'Hx  + g'x
+      * @param init x_init
+      * @return ResultData
+      */
+     BEZIER_COM_TRAJ_DLLAPI ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg,  const Vector3& init);
+
+     /**
+     * @brief solveOnestep Tries to solve the one step problem :  Given two contact phases, an initial and final com position and velocity,
+     *  try to compute the CoM trajectory (as a Bezier curve) that connect them
+     * @param pData problem Data. Should contain only two contact phase.
+     * @param Ts timelength of each contact phase. Should only contain two value
+     * @param timeStep time step used by the discretization
+     * @return ResultData a struct containing the resulting trajectory, if success is true.
+     */
+    BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveOnestep(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep = -1);
+
+
+
+
 } // end namespace bezier_com_traj
 
 #endif

src/CMakeLists.txt

@@ -18,6 +18,7 @@ SET(${LIBRARY_NAME}_SOURCES
                 solve.cpp
                 common_solve_methods.cpp
                 eiquadprog-fast.cpp
+                solve_transition.cpp
 		#~ ${INCLUDE_DIR}/centroidal-dynamics-lib/solver_LP_abstract.hh
 		#~ ${INCLUDE_DIR}/centroidal-dynamics-lib/solver_LP_qpoases.hh
 		#~ ${INCLUDE_DIR}/centroidal-dynamics-lib/solver_LP_clp.hh

src/solve.cpp

@@ -272,4 +272,34 @@ ResultDataCOMTraj solve0step(const ProblemData& pData,  const std::vector<double
 }
 
 
+std::pair<MatrixX3, VectorX> computeConstraintsOneStep(const ProblemData& pData,const std::vector<double>& Ts,const double timeStep){
+
+}
+
+
+std::pair<MatrixX3, VectorX> computeCostFunctionOneStep(const ProblemData&pData){
+
+}
+
+ResultDataCOMTraj solveOnestep(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep){
+    assert(pData.contacts_.size() ==2);
+    assert(Ts.size() == pData.contacts_.size());
+    bool fail = true;
+    ResultDataCOMTraj res;
+    std::pair<MatrixX3, VectorX> Ab = computeConstraintsOneStep(pData,Ts,timeStep);
+    std::pair<MatrixX3, VectorX> Hg = computeCostFunctionOneStep(pData);
+    Vector3 midPoint = (pData.c0_ + pData.c1_)/2.;
+    // rewriting 0.5 || Dx -d ||^2 as x'Hx  + g'x
+    ResultData resQp = solve(Ab.first,Ab.second,Hg.first,Hg.second, midPoint);
+    if(resQp.success_)
+    {
+        res.success_ = true;
+        res.x = resQp.x;
+        computeRealCost(pData, res);
+        computeC_of_T (pData,Ts,res);
+    }
+    return res;
+}
+
+
 } // namespace bezier_com_traj

src/solve_transition.cpp

+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#include <bezier-com-traj/solve.hh>
+#include <bezier-com-traj/common_solve_methods.hh>
+
+namespace bezier_com_traj
+{
+
+
+ResultData solveIntersection(const std::pair<MatrixXX, VectorX>& Ab,const std::pair<MatrixXX, VectorX>& Hg,  const Vector3& init)
+{
+    //TODO ??
+    return solve(Ab.first,Ab.second,Hg.first,Hg.second, init);
+}
+
+
+} // namespace