Merge pull request #1 from pFernbach/topic/end_effector

End effector API

include/hpp/bezier-com-traj/solve_end_effector.hh

@@ -15,7 +15,7 @@ namespace bezier_com_traj
 typedef std::pair<double,point3_t> coefs_t;
 const int DIM_POINT=3;
 //const int NUM_DISCRETIZATION = 11;
-const bool verbose = true;
+const bool verbose = false;
 
 
 /**
@@ -364,13 +364,13 @@ ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, co
     if(verbose){
       std::cout<<"End eff H = "<<std::endl<<H<<std::endl;
       std::cout<<"End eff g = "<<std::endl<<g<<std::endl;
+      std::cout<<"Dim Var = "<<dimVar(pData)<<std::endl;
+      std::cout<<"Dim H   = "<<H.rows()<<" x "<<H.cols()<<std::endl;
+      std::cout<<"Dim g   = "<<g.rows()<<std::endl;
+      std::cout<<"Dim A   = "<<A.rows()<<" x "<<A.cols()<<std::endl;
+      std::cout<<"Dim b   = "<<b.rows()<<std::endl;
     }
-    // call the solver
-    std::cout<<"Dim Var = "<<dimVar(pData)<<std::endl;
-    std::cout<<"Dim H   = "<<H.rows()<<" x "<<H.cols()<<std::endl;
-    std::cout<<"Dim g   = "<<g.rows()<<std::endl;
-    std::cout<<"Dim A   = "<<A.rows()<<" x "<<A.cols()<<std::endl;
-    std::cout<<"Dim b   = "<<b.rows()<<std::endl;
+
 
     VectorX init = VectorX(dimVar(pData));
    // init = (pData.c0_ + pData.c1_)/2.;

python/bezier_com_traj.cpp

 #include "hpp/bezier-com-traj/solve.hh"
 #include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
+#include "hpp/bezier-com-traj/solve_end_effector.hh"
 #include <eigenpy/memory.hpp>
 #include <eigenpy/eigenpy.hpp>
 
@@ -358,6 +359,25 @@ ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(const ProblemData& pData, c
 
 
 /** END computeCOMTraj **/
+/** BEGIN end effector **/
+
+struct DummyPath{
+
+  point3_t operator()(double /*u*/)const{
+    return point3_t::Zero();
+  }
+
+};
+
+
+ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time){
+
+   ResultDataCOMTraj  res =solveEndEffector<DummyPath>(pData,DummyPath(),time, 0);
+   return new ResultDataCOMTraj(res);
+}
+
+/** END end effector **/
+
 
 BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
 {
@@ -453,9 +473,11 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
             .value("INIT_POS", INIT_POS)
             .value("INIT_VEL", INIT_VEL)
             .value("INIT_ACC", INIT_ACC)
-            .value("END_POS", END_POS)
+            .value("INIT_JERK",INIT_JERK)
             .value("END_POS", END_POS)
             .value("END_VEL", END_VEL)
+            .value("END_ACC", END_ACC)
+            .value("END_JERK",END_JERK)
             .value("UNKNOWN", UNKNOWN)
             .export_values();
 
@@ -465,6 +487,7 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
     def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints, return_value_policy<manage_new_object>());
     def("computeCOMTraj", &computeCOMTrajPointer, return_value_policy<manage_new_object>());
     def("computeCOMTraj", &computeCOMTrajPointerChooseSolver, return_value_policy<manage_new_object>());
+    def("computeEndEffector", &computeEndEffector, return_value_policy<manage_new_object>());
 
 }