From bb3744ce300a1dc7ed54e76e6f8158dc6c05239e Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Fri, 19 Apr 2019 16:30:40 +0200
Subject: [PATCH] [python] add method computeEndEffectorDistanceCost
---
python/bezier_com_traj.cpp | 15 +++++++++++++++
1 file changed, 15 insertions(+)
diff --git a/python/bezier_com_traj.cpp b/python/bezier_com_traj.cpp
index 5f757c5..3cd2cac 100644
--- a/python/bezier_com_traj.cpp
+++ b/python/bezier_com_traj.cpp
@@ -373,6 +373,7 @@ 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
@@ -403,6 +404,19 @@ MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, con
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());
@@ -535,6 +549,7 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
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);
--
GitLab