diff --git a/include/hpp/bezier-com-traj/solve_end_effector.hh b/include/hpp/bezier-com-traj/solve_end_effector.hh
index 0b75d4e6eb78714c623b109418617be00013d2ab..0d68a0fec99da51ad7ad431e35e6ae9764d3e4de 100644
--- a/include/hpp/bezier-com-traj/solve_end_effector.hh
+++ b/include/hpp/bezier-com-traj/solve_end_effector.hh
@@ -188,31 +188,37 @@ void computeConstraintsMatrix(const ProblemData& pData,const std::vector<waypoin
b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);*/
}
-
-template <typename Path>
-void computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,const Path& path, MatrixXX& H,VectorX& g){
+std::pair<MatrixXX,VectorX> computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,std::vector<point3_t> pts_path){
+ assert(numPoints == pts_path.size() && "Pts_path size must be equal to numPoints");
double step = 1./(numPoints-1);
std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- std::vector<waypoint_t> cks;
- for(size_t i = 0 ; i < numPoints ; ++i){
- cks.push_back(evaluateCurveWaypointAtTime(pData,pi,i*step));
- }
- H = MatrixXX::Zero(dimVar(pData),dimVar(pData));
- g = VectorX::Zero(dimVar(pData));
+ waypoint_t c_wp;
+ MatrixXX H = MatrixXX::Zero(dimVar(pData),dimVar(pData));
+ VectorX g = VectorX::Zero(dimVar(pData));
point3_t pk;
- size_t i = 0;
- for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit){
- pk=path(i*step);
+ for(size_t i = 0 ; i < numPoints ; ++i){
+ c_wp = evaluateCurveWaypointAtTime(pData,pi,i*step);
+ pk = pts_path[i];
// std::cout<<"pk = "<<pk.transpose()<<std::endl;
// std::cout<<"coef First : "<<ckcit->first<<std::endl;
// std::cout<<"coef second : "<<ckcit->second.transpose()<<std::endl;
- H += (ckcit->first.transpose() * ckcit->first);
- g += ((ckcit->second - pk).transpose() * ckcit->first).transpose();
- i++;
+ H += (c_wp.first.transpose() * c_wp.first);
+ g += ((c_wp.second - pk).transpose() * c_wp.first).transpose();
}
double norm=H.norm();
H /= norm;
g /= norm;
+ return std::make_pair(H,g);
+}
+
+
+template <typename Path>
+std::pair<MatrixXX,VectorX> computeDistanceCostFunction(int numPoints,const ProblemData& pData, double T,const Path& path){
+ double step = 1./(numPoints-1);
+ std::vector<point3_t> pts_path;
+ for(size_t i = 0 ; i < numPoints ; ++i)
+ pts_path.push_back(path((double)(i*step)));
+ return computeDistanceCostFunction(numPoints,pData,T,pts_path);
}
//TODO
@@ -309,66 +315,75 @@ void computeJerkCostFunctionDiscretized(int numPoints,const ProblemData& pData,d
-
-
-template <typename Path>
-ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, const double T, const double weightDistance, bool useVelCost){
-
-
- if(verbose)
- std::cout<<"solve end effector, T = "<<T<<std::endl;
- assert (weightDistance>=0. && weightDistance<=1. && "WeightDistance must be between 0 and 1");
- double weightSmooth = 1. - weightDistance;
- std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,T);
+std::pair<MatrixXX,VectorX> computeEndEffectorConstraints(const ProblemData& pData, const double T,std::vector<bezier_t::point_t> pi){
std::vector<waypoint_t> wps_jerk=computeJerkWaypoints(pData,T,pi);
std::vector<waypoint_t> wps_acc=computeAccelerationWaypoints(pData,T,pi);
std::vector<waypoint_t> wps_vel=computeVelocityWaypoints(pData,T,pi);
// stack the constraint for each waypoint :
- MatrixXX A;
- VectorX b;
- Vector3 jerk_bounds(10000,10000,10000);
+ Vector3 jerk_bounds(10000,10000,10000); //TODO : read it from somewhere (ProblemData ?)
Vector3 acc_bounds(10000,10000,10000);
Vector3 vel_bounds(10000,10000,10000);
- const int DIM_VAR = dimVar(pData);
+ MatrixXX A;
+ VectorX b;
computeConstraintsMatrix(pData,wps_acc,wps_vel,acc_bounds,vel_bounds,A,b,wps_jerk,jerk_bounds);
- // std::cout<<"End eff A = "<<std::endl<<A<<std::endl;
- // std::cout<<"End eff b = "<<std::endl<<b<<std::endl;
- // compute cost function (discrete integral under the curve defined by 'path')
- MatrixXX H_rrt,H;
- VectorX g_rrt,g;
- std::pair<MatrixXX,VectorX> Hg_smooth;
+ return std::make_pair(A,b);
+}
+
+template <typename Path>
+std::pair<MatrixXX,VectorX> computeEndEffectorCost(const ProblemData& pData,const Path& path, const double T,const double weightDistance, bool /*useVelCost*/,std::vector<bezier_t::point_t> pi){
+ assert (weightDistance>=0. && weightDistance<=1. && "WeightDistance must be between 0 and 1");
+ double weightSmooth = 1. - weightDistance;
+ const int DIM_VAR = dimVar(pData);
+ // compute distance cost function (discrete integral under the curve defined by 'path')
+ MatrixXX H;
+ VectorX g;
+ std::pair<MatrixXX,VectorX> Hg_smooth,Hg_rrt;
if(weightDistance>0)
- computeDistanceCostFunction<Path>(50,pData,T,path,H_rrt,g_rrt);
+ Hg_rrt = computeDistanceCostFunction<Path>(50,pData,T,path);
else{
- H_rrt=MatrixXX::Zero(DIM_VAR,DIM_VAR);
- g_rrt=VectorX::Zero(DIM_VAR);
+ Hg_rrt.first=MatrixXX::Zero(DIM_VAR,DIM_VAR);
+ Hg_rrt.second=VectorX::Zero(DIM_VAR);
}
Hg_smooth = computeVelocityCost(pData,T,pi);
-
/* std::cout<<"End eff H_rrt = "<<std::endl<<H_rrt<<std::endl;
std::cout<<"End eff g_rrt = "<<std::endl<<g_rrt<<std::endl;
std::cout<<"End eff H_acc = "<<std::endl<<H_acc<<std::endl;
std::cout<<"End eff g_acc = "<<std::endl<<g_acc<<std::endl;
*/
-
// add the costs :
H = MatrixXX::Zero(DIM_VAR,DIM_VAR);
g = VectorX::Zero(DIM_VAR);
- H = weightSmooth*(Hg_smooth.first) + weightDistance*H_rrt;
- g = weightSmooth*(Hg_smooth.second) + weightDistance*g_rrt;
+ H = weightSmooth*(Hg_smooth.first) + weightDistance*Hg_rrt.first;
+ g = weightSmooth*(Hg_smooth.second) + weightDistance*Hg_rrt.second;
// H = Hg_smooth.first;
// g = Hg_smooth.second;
+
+ return std::make_pair(H,g);
+}
+
+
+template <typename Path>
+ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, const double T, const double weightDistance, bool useVelCost){
+
+
+ if(verbose)
+ std::cout<<"solve end effector, T = "<<T<<std::endl;
+ std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,T);
+ std::pair<MatrixXX, VectorX> Ab = computeEndEffectorConstraints(pData,T,pi);
+ std::pair<MatrixXX, VectorX> Hg = computeEndEffectorCost(pData,path,T,weightDistance,useVelCost,pi);
if(verbose){
- std::cout<<"End eff H = "<<std::endl<<H<<std::endl;
- std::cout<<"End eff g = "<<std::endl<<g<<std::endl;
+ std::cout<<"End eff A = "<<std::endl<<Ab.first<<std::endl;
+ std::cout<<"End eff b = "<<std::endl<<Ab.second<<std::endl;
+ std::cout<<"End eff H = "<<std::endl<<Hg.first<<std::endl;
+ std::cout<<"End eff g = "<<std::endl<<Hg.second<<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;
+ std::cout<<"Dim H = "<<Hg.first.rows()<<" x "<<Hg.first.cols()<<std::endl;
+ std::cout<<"Dim g = "<<Hg.second.rows()<<std::endl;
+ std::cout<<"Dim A = "<<Ab.first.rows()<<" x "<<Ab.first.cols()<<std::endl;
+ std::cout<<"Dim b = "<<Ab.first.rows()<<std::endl;
}
@@ -377,8 +392,7 @@ ResultDataCOMTraj solveEndEffector(const ProblemData& pData,const Path& path, co
// init =pData.c0_;
if(verbose)
std::cout<<"Init = "<<std::endl<<init.transpose()<<std::endl;
- std::pair<MatrixXX, VectorX> Ab = std::make_pair(A,b);
- std::pair<MatrixXX, VectorX> Hg = std::make_pair(H,g);
+
ResultData resQp = solve(Ab,Hg, init);
ResultDataCOMTraj res;
diff --git a/python/bezier_com_traj.cpp b/python/bezier_com_traj.cpp
index f73f47b27cd84d8f0e5620444f36dfb540e880db..3cd2cac5347911dabe4017d7ca3c6214ef30136d 100644
--- a/python/bezier_com_traj.cpp
+++ b/python/bezier_com_traj.cpp
@@ -1,6 +1,6 @@
-#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 <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>
@@ -16,6 +16,7 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::bezier_t)
namespace bezier_com_traj
{
using namespace boost::python;
+typedef double real;
ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, const Vector3& com ,const Vector3& dCom ,const Vector3& l0, const bool useAngMomentum
, const double timeDuration, const double timeStep)
@@ -370,12 +371,62 @@ struct DummyPath{
};
+typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
+typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
+
+
+struct MatrixVector
+{
+ linear_points_t res;
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() {return res.first;}
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() {return res.second;}
+};
+
+
ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time){
ResultDataCOMTraj res =solveEndEffector<DummyPath>(pData,DummyPath(),time, 0);
return new ResultDataCOMTraj(res);
}
+MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData, const double time){
+ std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
+ MatrixVector* res = new MatrixVector();
+ res->res =computeEndEffectorConstraints(pData,time, pi);
+ return res;
+}
+
+MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, const double time){
+ std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
+ MatrixVector* res = new MatrixVector();
+ res->res = computeVelocityCost(pData,time,pi);
+ return res;
+}
+
+
+MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData,const double time,const int numPoints,point_list_t pts_l){
+ std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
+ // transform the matrice 3xN in a std::vector<point3_t> of size N :
+ std::vector<point3_t> pts_path;
+ for(size_t c = 0 ; c < pts_l.cols() ; ++c){
+ pts_path.push_back(pts_l.block<3,1>(0,c));
+ }
+ MatrixVector* res = new MatrixVector();
+ res->res =computeDistanceCostFunction(numPoints,pData,time,pts_path);
+ return res;
+}
+
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> computeEndEffectorConstantWaypoints(const ProblemData& pData, const double time){
+ std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res (3, pi.size());
+ int col = 0;
+ for(std::vector<bezier_t::point_t>::const_iterator cit = pi.begin(); cit != pi.end(); ++cit, ++col)
+ res.block<3,1>(0,col) = *cit;
+ return res;
+ }
+
+
/** END end effector **/
@@ -478,9 +529,17 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
.value("END_VEL", END_VEL)
.value("END_ACC", END_ACC)
.value("END_JERK",END_JERK)
+ .value("ONE_FREE_VAR",ONE_FREE_VAR)
+ .value("TWO_FREE_VAR",TWO_FREE_VAR)
+ .value("THREE_FREE_VAR",THREE_FREE_VAR)
+ .value("FOUR_FREE_VAR",FOUR_FREE_VAR)
+ .value("FIVE_FREE_VAR",FIVE_FREE_VAR)
.value("UNKNOWN", UNKNOWN)
.export_values();
+ class_<MatrixVector>("MatrixVector", no_init)
+ .def_readonly("A", &MatrixVector::A)
+ .def_readonly("b", &MatrixVector::b);
def("zeroStepCapturability", &zeroStepCapturability, return_value_policy<manage_new_object>());
@@ -488,6 +547,11 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
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>());
+ def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython, return_value_policy<manage_new_object>());
+ def("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython, return_value_policy<manage_new_object>());
+ def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython, return_value_policy<manage_new_object>());
+ def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);
+
}
diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp
index 9f6f6a557b26654e126961dc850a049a497bbdbc..6680cc5ca78b60c39e05b0ab56b639545dcdce3d 100644
--- a/src/computeCOMTraj.cpp
+++ b/src/computeCOMTraj.cpp
@@ -58,7 +58,7 @@ bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData,dou
Normalize(A,b);
// add 1 for the slack variable :
A.block(0,3,dimH,1) = VectorX::Ones(dimH);
- return std::make_pair<MatrixXX,VectorX>(A,b);
+ return std::make_pair(A,b);
}
std::pair<MatrixXX,VectorX> dynamicStabilityConstraints(const MatrixXX& mH,const VectorX& h,const Vector3& g,const waypoint_t& w){
@@ -70,7 +70,7 @@ std::pair<MatrixXX,VectorX> dynamicStabilityConstraints(const MatrixXX& mH,const
A.block(0,0,dimH,3) = mH*w.first;
b = h + mH*(g_ - w.second);
Normalize(A,b);
- return std::make_pair<MatrixXX,VectorX>(A,b);
+ return std::make_pair(A,b);
}
std::vector<int> stepIdPerPhase(const T_time& timeArray) // const int pointsPerPhase)