diff --git a/multicontact-api/Makefile b/multicontact-api/Makefile index 55b5039d975346b3326033a7a971d6569e76c2e1..ba9f52e587bebada9accc783266de12c4c67b6de 100644 --- a/multicontact-api/Makefile +++ b/multicontact-api/Makefile @@ -5,6 +5,7 @@ ORG= loco-3d NAME= multicontact-api VERSION= 2.1.0 +PKGREVISION= 1 CATEGORIES= wip COMMENT= Multi-contact locomotion for multi-body systems @@ -14,7 +15,7 @@ MAINTAINER= gepetto-soft@laas.fr CMAKE_ARGS+= -DBUILD_PYTHON_INTERFACE=OFF include ../../devel/jrl-cmakemodules/Makefile.common -include ../../math/curves/depend.mk +include ../../math/ndcurves/depend.mk include ../../graphics/assimp/depend.mk include ../../graphics/urdfdom/depend.mk include ../../mapping/octomap/depend.mk diff --git a/multicontact-api/distinfo b/multicontact-api/distinfo index 51a16359e3aa403b0b8360e1139a5fb2f58047a7..3a8c5be8e507fdc446ad62acc05536114429652c 100644 --- a/multicontact-api/distinfo +++ b/multicontact-api/distinfo @@ -1,3 +1,5 @@ SHA1 (multicontact-api-2.1.0.tar.gz) = 7639cefdb28c5eb0f992b6bb604f97a4cf82dec6 RMD160 (multicontact-api-2.1.0.tar.gz) = 6ae2f6c5214a1da4c4680ed80c7b2c27de32b43d Size (multicontact-api-2.1.0.tar.gz) = 82404503 bytes +SHA1 (patch-21) = 4948a8981ed94c766bf440371f4bd8074042894d +SHA1 (patch-aa) = 0eaa23c5ef1e463afe91d8b6302f2dd1ef9caecc diff --git a/multicontact-api/patches/patch-21 b/multicontact-api/patches/patch-21 new file mode 100644 index 0000000000000000000000000000000000000000..3d4ad0d3075c2bb21da7819e5cb24a1130fb3183 --- /dev/null +++ b/multicontact-api/patches/patch-21 @@ -0,0 +1,633 @@ +From 017a69a2932c08c5515c4d5f46d9d6a25026c6f3 Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Tue, 16 Mar 2021 17:25:23 +0100 +Subject: [PATCH] =?UTF-8?q?curves=20=E2=86=92=20ndcurves?= +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +ref. https://github.com/loco-3d/ndcurves/pull/58 +--- + CMakeLists.txt | 4 +- + bindings/python/scenario/contact-phase.cpp | 16 ++-- + bindings/python/scenario/contact-sequence.cpp | 16 ++-- + examples/display_gepetto_gui.py | 2 +- + .../python/scenario/contact-phase.hpp | 32 ++++---- + .../multicontact-api/geometry/curve-map.hpp | 4 +- + .../scenario/contact-phase.hpp | 62 +++++++-------- + .../scenario/contact-sequence.hpp | 26 +++---- + unittest/examples.cpp | 16 ++-- + unittest/python/scenario.py | 2 +- + unittest/python/serialization_examples.py | 2 +- + unittest/scenario.cpp | 75 ++++++++++--------- + unittest/serialization_versionning.cpp | 17 ++--- + 13 files changed, 137 insertions(+), 137 deletions(-) + +diff --git a/CMakeLists.txt b/CMakeLists.txt +index d687991..1f8ff44 100644 +--- CMakeLists.txt ++++ CMakeLists.txt +@@ -36,7 +36,7 @@ SET(CMAKE_CXX_STANDARD 11) + + # Project dependencies + ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.0.0") +-ADD_PROJECT_DEPENDENCY(curves 0.5.1 REQUIRED PKG_CONFIG_REQUIRES "curves >= 0.5.1") ++ADD_PROJECT_DEPENDENCY(ndcurves 1.0.0 REQUIRED PKG_CONFIG_REQUIRES "ndcurves >= 0.5.1") + IF(NOT CURVES_WITH_PINOCCHIO_SUPPORT) + MESSAGE(FATAL_ERROR "you need to use a curves version compiled with pinocchio support") + ENDIF(NOT CURVES_WITH_PINOCCHIO_SUPPORT) +@@ -59,7 +59,7 @@ INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS}) + + ADD_LIBRARY(${PROJECT_NAME} INTERFACE) + TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>) +-TARGET_LINK_LIBRARIES(${PROJECT_NAME} INTERFACE pinocchio::pinocchio curves::curves) ++TARGET_LINK_LIBRARIES(${PROJECT_NAME} INTERFACE pinocchio::pinocchio ndcurves::ndcurves) + + IF(NOT INSTALL_PYTHON_INTERFACE_ONLY) + INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) +diff --git a/bindings/python/scenario/contact-phase.cpp b/bindings/python/scenario/contact-phase.cpp +index 84edd71..cb5a712 100644 +--- bindings/python/scenario/contact-phase.cpp ++++ bindings/python/scenario/contact-phase.cpp +@@ -5,14 +5,14 @@ + #include "multicontact-api/bindings/python/scenario/contact-phase.hpp" + + // required because of the serialization of the curves pointer : +-#include <curves/fwd.h> +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + namespace multicontact_api { + namespace python { +diff --git a/bindings/python/scenario/contact-sequence.cpp b/bindings/python/scenario/contact-sequence.cpp +index c046bb6..411636f 100644 +--- bindings/python/scenario/contact-sequence.cpp ++++ bindings/python/scenario/contact-sequence.cpp +@@ -5,14 +5,14 @@ + #include "multicontact-api/bindings/python/scenario/contact-sequence.hpp" + + // required because of the serialization of the curves pointer : +-#include <curves/fwd.h> +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + namespace multicontact_api { + namespace python { +diff --git a/include/multicontact-api/bindings/python/scenario/contact-phase.hpp b/include/multicontact-api/bindings/python/scenario/contact-phase.hpp +index bf559e8..3004c46 100644 +--- include/multicontact-api/bindings/python/scenario/contact-phase.hpp ++++ include/multicontact-api/bindings/python/scenario/contact-phase.hpp +@@ -11,7 +11,7 @@ + #include "multicontact-api/scenario/contact-phase.hpp" + #include "multicontact-api/bindings/python/serialization/archive.hpp" + #include "multicontact-api/bindings/python/utils/printable.hpp" +-#include <curves/python/python_definitions.h> ++#include <ndcurves/python/python_definitions.h> + #include <boost/python/suite/indexing/map_indexing_suite.hpp> + #include <boost/python/suite/indexing/vector_indexing_suite.hpp> + +@@ -31,10 +31,10 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + typedef typename ContactPhase::point3_t point3_t; + typedef typename ContactPhase::point6_t point6_t; + typedef typename ContactPhase::pointX_t pointX_t; +- typedef curves::t_pointX_t t_pointX_t; +- typedef curves::t_time_t t_time_t; +- typedef curves::pointX_list_t pointX_list_t; +- typedef curves::time_waypoints_t time_waypoints_t; ++ typedef ndcurves::t_pointX_t t_pointX_t; ++ typedef ndcurves::t_time_t t_time_t; ++ typedef ndcurves::pointX_list_t pointX_list_t; ++ typedef ndcurves::time_waypoints_t time_waypoints_t; + + // call macro for all ContactPhase methods that can be overloaded + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isConsistent_overloads, ContactPhase::isConsistent, 0, 1) +@@ -291,11 +291,11 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + const pointX_list_t& points_derivative, + const pointX_list_t& points_second_derivative, + const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); + t_pointX_t points_second_derivative_list = +- curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setCOMtrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, + time_points_list); + return; +@@ -305,11 +305,11 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + const pointX_list_t& points_derivative, + const pointX_list_t& points_second_derivative, + const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); + t_pointX_t points_second_derivative_list = +- curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setJointsTrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, + time_points_list); + return; +@@ -317,9 +317,9 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + + static void setAMtrajectoryFromPoints(ContactPhase& self, const pointX_list_t& points, + const pointX_list_t& points_derivative, const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setAMtrajectoryFromPoints(points_list, points_derivative_list, time_points_list); + return; + } +diff --git a/include/multicontact-api/geometry/curve-map.hpp b/include/multicontact-api/geometry/curve-map.hpp +index 9dcb7e3..f3db303 100644 +--- include/multicontact-api/geometry/curve-map.hpp ++++ include/multicontact-api/geometry/curve-map.hpp +@@ -4,7 +4,7 @@ + #ifndef __multicontact_api_geometry_curve_map_hpp__ + #define __multicontact_api_geometry_curve_map_hpp__ + +-#include <curves/curve_abc.h> ++#include <ndcurves/curve_abc.h> + #include <map> + #include <string> + +@@ -15,7 +15,7 @@ + #include <boost/serialization/string.hpp> + #include <boost/serialization/map.hpp> + #include <boost/serialization/base_object.hpp> +-#include <curves/serialization/registeration.hpp> ++#include <ndcurves/serialization/registeration.hpp> + + template <typename Curve> + struct CurveMap : public std::map<std::string, Curve> { +diff --git a/include/multicontact-api/scenario/contact-phase.hpp b/include/multicontact-api/scenario/contact-phase.hpp +index 3a1b4d1..da2bfea 100644 +--- include/multicontact-api/scenario/contact-phase.hpp ++++ include/multicontact-api/scenario/contact-phase.hpp +@@ -9,9 +9,9 @@ + #include "multicontact-api/serialization/eigen-matrix.hpp" + #include "multicontact-api/serialization/spatial.hpp" + +-#include <curves/fwd.h> +-#include <curves/piecewise_curve.h> +-#include <curves/serialization/curves.hpp> ++#include <ndcurves/fwd.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/serialization/curves.hpp> + #include <map> + #include <vector> + #include <set> +@@ -31,22 +31,22 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + typedef _Scalar Scalar; + + // Eigen types +- typedef curves::pointX_t pointX_t; +- typedef curves::point3_t point3_t; +- typedef curves::point6_t point6_t; +- typedef curves::t_point3_t t_point3_t; +- typedef curves::t_pointX_t t_pointX_t; +- typedef curves::transform_t transform_t; ++ typedef ndcurves::pointX_t pointX_t; ++ typedef ndcurves::point3_t point3_t; ++ typedef ndcurves::point6_t point6_t; ++ typedef ndcurves::t_point3_t t_point3_t; ++ typedef ndcurves::t_pointX_t t_pointX_t; ++ typedef ndcurves::transform_t transform_t; + + // Curves types +- typedef curves::curve_abc_t curve_t; +- // typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t; +- typedef curves::curve_SE3_t curve_SE3_t; +- typedef curves::curve_ptr_t curve_ptr; ++ typedef ndcurves::curve_abc_t curve_t; ++ // typedef ndcurves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t; ++ typedef ndcurves::curve_SE3_t curve_SE3_t; ++ typedef ndcurves::curve_ptr_t curve_ptr; + // typedef boost::shared_ptr<curve_3_t> curve_3_ptr; +- typedef curves::curve_SE3_ptr_t curve_SE3_ptr; +- typedef curves::piecewise3_t piecewise3_t; +- typedef curves::piecewise_t piecewise_t; ++ typedef ndcurves::curve_SE3_ptr_t curve_SE3_ptr; ++ typedef ndcurves::piecewise3_t piecewise3_t; ++ typedef ndcurves::piecewise_t piecewise_t; + typedef piecewise_t::t_time_t t_time_t; + + typedef std::vector<std::string> t_strings; +@@ -452,7 +452,7 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setCOMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_pointX_t& points_second_derivative, const t_time_t& time_points) { + /* +- piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); + if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + m_c = curve_ptr(new piecewise_t(c_t)); +@@ -460,10 +460,10 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + m_ddc = curve_ptr(c_t.compute_derivate_ptr(2)); + */ + m_c = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dc = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); +- m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); ++ m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points_second_derivative, time_points))); + if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3) + throw std::invalid_argument("Dimension of the points must be 3."); +@@ -489,16 +489,16 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setAMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_time_t& time_points) { + /* +- piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, time_points); + if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + m_L = curve_ptr(new piecewise_t(L_t)); + m_dL = curve_ptr(L_t.compute_derivate_ptr(1)); + */ + m_L = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dL = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); + if (m_L->dim() != 3 || m_dL->dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + + m_L_init = point3_t(points.front()); +@@ -521,17 +521,17 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setJointsTrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_pointX_t& points_second_derivative, const t_time_t& time_points) { + /* +- piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); + m_q = curve_ptr(new piecewise_t(q_t)); + m_dq = curve_ptr(q_t.compute_derivate_ptr(1)); + m_ddq = curve_ptr(q_t.compute_derivate_ptr(2)); + */ + m_q = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dq = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); +- m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); ++ m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points_second_derivative, time_points))); + m_q_init = points.front(); + m_q_final = points.back(); +@@ -660,13 +660,13 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + template <class Archive> + void serialize(Archive& ar, const unsigned int version) { + // ar& boost::serialization::make_nvp("placement", m_placement); +- unsigned int curve_version; // Curves API version related to the archive multicontact-api API version +- if(version <2){ ++ unsigned int curve_version; // Curves API version related to the archive multicontact-api API version ++ if (version < 2) { + curve_version = 0; +- }else{ ++ } else { + curve_version = 1; + } +- curves::serialization::register_types<Archive>(ar, curve_version); ++ ndcurves::serialization::register_types<Archive>(ar, curve_version); + ar& boost::serialization::make_nvp("c_init", m_c_init); + ar& boost::serialization::make_nvp("dc_init", m_dc_init); + ar& boost::serialization::make_nvp("ddc_init", m_ddc_init); +diff --git a/include/multicontact-api/scenario/contact-sequence.hpp b/include/multicontact-api/scenario/contact-sequence.hpp +index 21a5022..690aea0 100644 +--- include/multicontact-api/scenario/contact-sequence.hpp ++++ include/multicontact-api/scenario/contact-sequence.hpp +@@ -3,11 +3,11 @@ + + #include "multicontact-api/scenario/fwd.hpp" + #include "multicontact-api/scenario/contact-phase.hpp" +-#include <curves/fwd.h> +-#include <curves/curve_abc.h> +-#include <curves/piecewise_curve.h> +-#include <curves/polynomial.h> +-#include <curves/se3_curve.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/curve_abc.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/se3_curve.h> + #include "multicontact-api/serialization/archive.hpp" + + #include <vector> +@@ -25,16 +25,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp + typedef typename ContactPhase::SE3 SE3; + typedef std::vector<ContactPhase> ContactPhaseVector; + +- typedef curves::pointX_t pointX_t; +- typedef curves::transform_t transform_t; +- typedef curves::curve_abc_t curve_t; +- typedef curves::curve_SE3_t curve_SE3_t; ++ typedef ndcurves::pointX_t pointX_t; ++ typedef ndcurves::transform_t transform_t; ++ typedef ndcurves::curve_abc_t curve_t; ++ typedef ndcurves::curve_SE3_t curve_SE3_t; + typedef boost::shared_ptr<curve_t> curve_ptr; + typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr; +- typedef curves::piecewise_t piecewise_t; +- typedef curves::piecewise_SE3_t piecewise_SE3_t; +- typedef curves::SE3Curve_t SE3Curve_t; +- typedef curves::polynomial_t polynomial_t; ++ typedef ndcurves::piecewise_t piecewise_t; ++ typedef ndcurves::piecewise_SE3_t piecewise_SE3_t; ++ typedef ndcurves::SE3Curve_t SE3Curve_t; ++ typedef ndcurves::polynomial_t polynomial_t; + + ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {} + +diff --git a/unittest/examples.cpp b/unittest/examples.cpp +index 4db65e2..37698ca 100644 +--- unittest/examples.cpp ++++ unittest/examples.cpp +@@ -9,14 +9,14 @@ + + #include "multicontact-api/scenario/contact-sequence.hpp" + #include "multicontact-api/scenario/fwd.hpp" +-#include "curves/fwd.h" +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + /** + * This unit test try to deserialize the ContactSequences in the examples folder +diff --git a/unittest/python/scenario.py b/unittest/python/scenario.py +index 32162f1..4b46cd9 100644 +--- unittest/python/scenario.py ++++ unittest/python/scenario.py +@@ -5,7 +5,7 @@ + from random import uniform + + import numpy as np +-from curves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial ++from ndcurves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial + from numpy import array, array_equal, isclose, random + + import pinocchio as pin +diff --git a/unittest/python/serialization_examples.py b/unittest/python/serialization_examples.py +index d0158b2..956f4a4 100644 +--- unittest/python/serialization_examples.py ++++ unittest/python/serialization_examples.py +@@ -1,7 +1,7 @@ + import pathlib + import unittest + +-import curves # noqa: requiered to get C++ type exposition ++import ndcurves # noqa: requiered to get C++ type exposition + + import pinocchio as pin + from multicontact_api import ContactSequence +diff --git a/unittest/scenario.cpp b/unittest/scenario.cpp +index 6df4481..c5fc643 100644 +--- unittest/scenario.cpp ++++ unittest/scenario.cpp +@@ -12,25 +12,25 @@ + #include "multicontact-api/scenario/contact-phase.hpp" + #include "multicontact-api/scenario/contact-sequence.hpp" + +-#include <curves/fwd.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> + + typedef Eigen::Matrix<double, 1, 1> point1_t; +-using curves::point3_t; +-using curves::point6_t; ++using ndcurves::point3_t; ++using ndcurves::point6_t; + typedef Eigen::Matrix<double, 12, 1> point12_t; +-using curves::curve_ptr_t; +-using curves::curve_SE3_ptr_t; +-using curves::matrix3_t; +-using curves::piecewise_SE3_t; +-using curves::piecewise_t; +-using curves::pointX_t; +-using curves::quaternion_t; +-using curves::t_point3_t; +-using curves::t_pointX_t; ++using ndcurves::curve_ptr_t; ++using ndcurves::curve_SE3_ptr_t; ++using ndcurves::matrix3_t; ++using ndcurves::piecewise_SE3_t; ++using ndcurves::piecewise_t; ++using ndcurves::pointX_t; ++using ndcurves::quaternion_t; ++using ndcurves::t_point3_t; ++using ndcurves::t_pointX_t; + using namespace multicontact_api::scenario; + typedef ContactSequence::ContactPhaseVector ContactPhaseVector; + typedef ContactModel::Matrix3X Matrix3X; +@@ -76,9 +76,9 @@ curve_ptr_t buildPiecewisePolynomialC2() { + time_points.push_back(t2); + time_points.push_back(t3); + +- curves::piecewise_t ppc_C2 = curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ ndcurves::piecewise_t ppc_C2 = ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); +- curve_ptr_t res(new curves::piecewise_t(ppc_C2)); ++ curve_ptr_t res(new ndcurves::piecewise_t(ppc_C2)); + return res; + } + +@@ -99,7 +99,7 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) { + vec.push_back(b); + vec.push_back(c); + vec.push_back(d); +- curve_ptr_t res(new curves::polynomial_t(vec.begin(), vec.end(), min, max)); ++ curve_ptr_t res(new ndcurves::polynomial_t(vec.begin(), vec.end(), min, max)); + return res; + } + +@@ -123,7 +123,7 @@ curve_SE3_ptr_t buildPiecewiseSE3() { + quaternion_t q1(0., 1., 0, 0); + pointX_t p0 = point3_t(1., 1.5, -2.); + pointX_t p1 = point3_t(3., 0, 1.); +- curve_SE3_ptr_t cLinear(new curves::SE3Curve_t(p0, p1, q0, q1, min, mid)); ++ curve_SE3_ptr_t cLinear(new ndcurves::SE3Curve_t(p0, p1, q0, q1, min, mid)); + point3_t a(1, 2, 3); + point3_t b(2, 3, 4); + point3_t c(3, 4, 5); +@@ -133,12 +133,13 @@ curve_SE3_ptr_t buildPiecewiseSE3() { + params.push_back(b); + params.push_back(c); + params.push_back(d); +- boost::shared_ptr<curves::bezier_t> translation_bezier(new curves::bezier_t(params.begin(), params.end(), mid, max)); +- curve_SE3_ptr_t cBezier(new curves::SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix())); ++ boost::shared_ptr<ndcurves::bezier_t> translation_bezier( ++ new ndcurves::bezier_t(params.begin(), params.end(), mid, max)); ++ curve_SE3_ptr_t cBezier(new ndcurves::SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix())); + +- curves::piecewise_SE3_t piecewiseSE3(cLinear); ++ ndcurves::piecewise_SE3_t piecewiseSE3(cLinear); + piecewiseSE3.add_curve_ptr(cBezier); +- curve_SE3_ptr_t res = boost::make_shared<curves::piecewise_SE3_t>(piecewiseSE3); ++ curve_SE3_ptr_t res = boost::make_shared<ndcurves::piecewise_SE3_t>(piecewiseSE3); + return res; + } + +@@ -157,7 +158,7 @@ curve_SE3_ptr_t buildRandomSE3LinearTraj(const double min, const double max) { + pointX_t p0 = point3_t::Random(); + pointX_t p1 = point3_t::Random(); + +- curve_SE3_ptr_t res(new curves::SE3Curve_t(p0, p1, q0, q1, min, max)); ++ curve_SE3_ptr_t res(new ndcurves::SE3Curve_t(p0, p1, q0, q1, min, max)); + return res; + } + +@@ -599,7 +600,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + pointX_t f1 = point12_t::Random(); + const double min = 0.5; + const double max = 2.; +- curve_ptr_t force12d(new curves::polynomial_t(f0, f1, min, max)); ++ curve_ptr_t force12d(new ndcurves::polynomial_t(f0, f1, min, max)); + bool newTraj = cp2.addContactForceTrajectory("left_leg", force12d); + BOOST_CHECK(newTraj); + newTraj = cp2.addContactForceTrajectory("left_leg", force12d); +@@ -612,7 +613,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + BOOST_CHECK((*cp2.contactForces("left_leg"))(1.2) == (*force12d)(1.2)); + pointX_t f0_1 = point12_t::Random(); + pointX_t f1_1 = point12_t::Random(); +- curve_ptr_t force12d_1(new curves::polynomial_t(f0_1, f1_1, min, max)); ++ curve_ptr_t force12d_1(new ndcurves::polynomial_t(f0_1, f1_1, min, max)); + cp2.contactForces().insert( + std::pair<std::string, curve_ptr_t>("right_leg", force12d_1)); // should not have any effect only const getter + BOOST_CHECK(cp2.contactForces().count("right_leg") == 0); +@@ -633,7 +634,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + nf0 << 56.3; + pointX_t nf1(1); + nf1 << 5893.2; +- curve_ptr_t force1d(new curves::polynomial_t(nf0, nf1, min, max)); ++ curve_ptr_t force1d(new ndcurves::polynomial_t(nf0, nf1, min, max)); + newTraj = cp2.addContactNormalForceTrajectory("left_leg", force1d); + BOOST_CHECK(newTraj); + newTraj = cp2.addContactNormalForceTrajectory("left_leg", force1d); +@@ -648,7 +649,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + nf0_1 << 147.2; + pointX_t nf1_1(1); + nf1_1 << 562; +- curve_ptr_t force1d_1(new curves::polynomial_t(nf0_1, nf1_1, min, max)); ++ curve_ptr_t force1d_1(new ndcurves::polynomial_t(nf0_1, nf1_1, min, max)); + cp2.contactNormalForces().insert( + std::pair<std::string, curve_ptr_t>("right_leg", force1d_1)); // should not have any effect only const getter + BOOST_CHECK(cp2.contactNormalForces().count("right_leg") == 0); +@@ -1798,12 +1799,12 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_com_traj) { + time_points2.push_back(t3); + time_points2.push_back(t4); + +- curves::piecewise_t c1 = +- curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points1, time_points1); +- curve_ptr_t c1_ptr(new curves::piecewise_t(c1)); +- curves::piecewise_t c2 = +- curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points2, time_points2); +- curve_ptr_t c2_ptr(new curves::piecewise_t(c2)); ++ ndcurves::piecewise_t c1 = ++ ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points1, time_points1); ++ curve_ptr_t c1_ptr(new ndcurves::piecewise_t(c1)); ++ ndcurves::piecewise_t c2 = ++ ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points2, time_points2); ++ curve_ptr_t c2_ptr(new ndcurves::piecewise_t(c2)); + cp0.m_c = c1_ptr; + cp1.m_c = c2_ptr; + BOOST_CHECK(cp0.m_c->min() == 0.); +@@ -1837,8 +1838,8 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_effector_traj) { + pointX_t p1 = point3_t::Random(); + pointX_t p2 = point3_t::Random(); + +- curve_SE3_ptr_t traj_0(new curves::SE3Curve_t(p0, p1, q0, q1, 0., 2.)); +- curve_SE3_ptr_t traj_2(new curves::SE3Curve_t(p1, p2, q1, q2, 4., 8.)); ++ curve_SE3_ptr_t traj_0(new ndcurves::SE3Curve_t(p0, p1, q0, q1, 0., 2.)); ++ curve_SE3_ptr_t traj_2(new ndcurves::SE3Curve_t(p1, p2, q1, q2, 4., 8.)); + cp0.addEffectorTrajectory("right_leg", traj_0); + cp2.addEffectorTrajectory("right_leg", traj_2); + +diff --git a/unittest/serialization_versionning.cpp b/unittest/serialization_versionning.cpp +index ef6a935..fc58353 100644 +--- unittest/serialization_versionning.cpp ++++ unittest/serialization_versionning.cpp +@@ -9,14 +9,14 @@ + + #include "multicontact-api/scenario/contact-sequence.hpp" + #include "multicontact-api/scenario/fwd.hpp" +-#include "curves/fwd.h" +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + /** + * This unit test try to deserialize the ContactSequences in the examples/previous_versions folder +@@ -62,5 +62,4 @@ BOOST_AUTO_TEST_CASE(api_1) { + BOOST_CHECK(cs.haveContactModelDefined()); + } + +- + BOOST_AUTO_TEST_SUITE_END() diff --git a/multicontact-api/patches/patch-aa b/multicontact-api/patches/patch-aa new file mode 100644 index 0000000000000000000000000000000000000000..665cb186a42241b22f630cc7e9234a227214d92a --- /dev/null +++ b/multicontact-api/patches/patch-aa @@ -0,0 +1,31 @@ +From 0c941d6ee950e498e3c7d473e6d1c6b1af0a2668 Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Wed, 17 Mar 2021 12:19:20 +0100 +Subject: [PATCH] [tests] add extension + +--- + unittest/CMakeLists.txt | 4 ++-- + 1 file changed, 2 insertions(+), 2 deletions(-) + +diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt +index c777772..998934b 100644 +--- unittest/CMakeLists.txt ++++ unittest/CMakeLists.txt +@@ -1,4 +1,4 @@ +-# Copyright (c) 2015-2020, CNRS ++# Copyright (c) 2015-2021, CNRS + # Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel + + ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK) +@@ -11,7 +11,7 @@ SET(${PROJECT_NAME}_TESTS + ) + + FOREACH(TEST ${${PROJECT_NAME}_TESTS}) +- ADD_UNIT_TEST(${TEST} ${TEST}) ++ ADD_UNIT_TEST(${TEST} ${TEST}.cpp) + TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} ${Boost_LIBRARIES}) + ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS}) + +-- +2.17.1 + diff --git a/py-hpp-bezier-com-traj/Makefile b/py-hpp-bezier-com-traj/Makefile index a940f463ce803d89103ea325d29e83109004476c..d4d6e7c0f8230622ce199d8ee791c2d77fde8521 100644 --- a/py-hpp-bezier-com-traj/Makefile +++ b/py-hpp-bezier-com-traj/Makefile @@ -13,8 +13,8 @@ PYTHON_NOTAG_CONFLICT= yes include ../../meta-pkgs/hpp/Makefile.common -include ../../math/curves/depend.mk -include ../../math/py-curves/depend.mk +include ../../math/ndcurves/depend.mk +include ../../math/py-ndcurves/depend.mk include ../../wip/py-hpp-centroidal-dynamics/depend.mk diff --git a/py-hpp-bezier-com-traj/distinfo b/py-hpp-bezier-com-traj/distinfo index 2b966b8a376f249416bf2bc17f1082222234c177..78616c76bab5e75473b3409db87637bbf6bed469 100644 --- a/py-hpp-bezier-com-traj/distinfo +++ b/py-hpp-bezier-com-traj/distinfo @@ -1,3 +1,4 @@ SHA1 (hpp-bezier-com-traj-4.10.1.tar.gz) = d39e2e066b4ad42a269822429ac838834b4e00c4 RMD160 (hpp-bezier-com-traj-4.10.1.tar.gz) = d3e5b01ea32d2eee66478b35e0800a02d5d949b4 Size (hpp-bezier-com-traj-4.10.1.tar.gz) = 877562 bytes +SHA1 (patch-12) = 9af3e593368182c0e82f4e587b0e3d47c3dee7ed diff --git a/py-hpp-bezier-com-traj/patches/patch-12 b/py-hpp-bezier-com-traj/patches/patch-12 new file mode 100644 index 0000000000000000000000000000000000000000..0ec7c82bd52456591d4b8a6a1c933a1be5bc3b17 --- /dev/null +++ b/py-hpp-bezier-com-traj/patches/patch-12 @@ -0,0 +1,397 @@ +From 531c52178faa8843e9e2c89a084d98c8ce949cb8 Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Tue, 16 Mar 2021 16:24:52 +0100 +Subject: [PATCH] =?UTF-8?q?curves=20=E2=86=92=20ndcurves?= +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +--- + CMakeLists.txt | 4 +-- + README.md | 4 +-- + .../bezier-com-traj/common_solve_methods.hh | 2 +- + include/hpp/bezier-com-traj/data.hh | 2 +- + include/hpp/bezier-com-traj/definitions.hh | 8 +++--- + include/hpp/bezier-com-traj/utils.hh | 15 +++++----- + python/test/binding_tests.py | 2 +- + src/CMakeLists.txt | 4 +-- + src/common_solve_methods.cpp | 2 +- + src/computeCOMTraj.cpp | 8 +++--- + src/solve_0_step.cpp | 10 +++---- + src/utils.cpp | 6 ++-- + tests/test-bezier-symbolic.cpp | 28 +++++++++---------- + 13 files changed, 48 insertions(+), 47 deletions(-) + +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 4529abc..693d652 100644 +--- CMakeLists.txt ++++ CMakeLists.txt +@@ -34,7 +34,7 @@ ENDIF(BUILD_PYTHON_INTERFACE) + # Project dependencies + + ADD_PROJECT_DEPENDENCY(hpp-centroidal-dynamics REQUIRED) +-ADD_PROJECT_DEPENDENCY(curves REQUIRED) ++ADD_PROJECT_DEPENDENCY(ndcurves 1.0.0 REQUIRED) + + IF(USE_GLPK) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake/find-external/glpk") +@@ -85,7 +85,7 @@ endif(USE_GLPK) + ADD_LIBRARY(${PROJECT_NAME} SHARED + ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) + TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>) +-TARGET_LINK_LIBRARIES(${PROJECT_NAME} curves::curves hpp-centroidal-dynamics::hpp-centroidal-dynamics) ++TARGET_LINK_LIBRARIES(${PROJECT_NAME} ndcurves::ndcurves hpp-centroidal-dynamics::hpp-centroidal-dynamics) + + if(USE_GLPK) + TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC ${glpk_INCLUDE_DIR}) +diff --git a/README.md b/README.md +index 92cb957..41bfa02 100644 +--- README.md ++++ README.md +@@ -28,7 +28,7 @@ https://hal.archives-ouvertes.fr/hal-01726155v1 + + ## Dependencies + * [centroidal-dynamics-lib](https://github.com/stonneau/centroidal-dynamics-lib) Centroidal dynamics computation library +-* [curves](https://github.com/loco-3d/curves) Bezier curves library ++* [ndcurves](https://github.com/loco-3d/ndcurves) Bezier curves library + * [glpk](https://www.gnu.org/software/glpk/) GNU Linear Programming Kit + + ## Additional dependencies for python bindings +@@ -77,7 +77,7 @@ to python. + For the zero step capturability, we will first define a contact phase using the objects from centroidal_dynamics: + ``` + #importing the libraries of interest +-import curves # noqa - necessary to register curves::bezier_curve ++import ndcurves # noqa - necessary to register ndcurves::bezier_curve + import numpy as np + from numpy import array + from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP +diff --git a/include/hpp/bezier-com-traj/common_solve_methods.hh b/include/hpp/bezier-com-traj/common_solve_methods.hh +index 6f073e8..4719bae 100644 +--- include/hpp/bezier-com-traj/common_solve_methods.hh ++++ include/hpp/bezier-com-traj/common_solve_methods.hh +@@ -24,7 +24,7 @@ namespace bezier_com_traj { + * @return a vector of waypoint representing the discretization of the curve + */ + BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints( +- const std::vector<waypoint6_t>& wps, const std::vector<curves::Bern<double> >& bernstein, int numSteps); ++ const std::vector<waypoint6_t>& wps, const std::vector<ndcurves::Bern<double> >& bernstein, int numSteps); + + /** + * @brief compute6dControlPointInequalities Given linear and angular control waypoints, +diff --git a/include/hpp/bezier-com-traj/data.hh b/include/hpp/bezier-com-traj/data.hh +index 72f2caf..c32ccda 100644 +--- include/hpp/bezier-com-traj/data.hh ++++ include/hpp/bezier-com-traj/data.hh +@@ -11,7 +11,7 @@ + #include <hpp/bezier-com-traj/definitions.hh> + #include <hpp/bezier-com-traj/utils.hh> + #include <hpp/bezier-com-traj/solver/solver-abstract.hpp> +-#include <curves/bezier_curve.h> ++#include <ndcurves/bezier_curve.h> + #include <hpp/centroidal-dynamics/centroidal_dynamics.hh> + #include <Eigen/Dense> + +diff --git a/include/hpp/bezier-com-traj/definitions.hh b/include/hpp/bezier-com-traj/definitions.hh +index 37a0bab..9f3c647 100644 +--- include/hpp/bezier-com-traj/definitions.hh ++++ include/hpp/bezier-com-traj/definitions.hh +@@ -7,7 +7,7 @@ + #define BEZIER_COM_TRAJ_DEFINITIONS_H + + #include <hpp/centroidal-dynamics/centroidal_dynamics.hh> +-#include <curves/bezier_curve.h> ++#include <ndcurves/bezier_curve.h> + #include <Eigen/Dense> + + namespace bezier_com_traj { +@@ -51,9 +51,9 @@ typedef std::pair<matrix3_t, point3_t> waypoint3_t; + typedef std::pair<Matrix39, point3_t> waypoint9_t; + struct waypoint_t; // forward declaration + +-typedef curves::bezier_curve<double, double, true, point_t> bezier_t; +-typedef curves::bezier_curve<double, double, true, waypoint_t> bezier_wp_t; +-typedef curves::bezier_curve<double, double, true, point6_t> bezier6_t; ++typedef ndcurves::bezier_curve<double, double, true, point_t> bezier_t; ++typedef ndcurves::bezier_curve<double, double, true, waypoint_t> bezier_wp_t; ++typedef ndcurves::bezier_curve<double, double, true, point6_t> bezier6_t; + + typedef std::vector<std::pair<double, int> > T_time; + typedef T_time::const_iterator CIT_time; +diff --git a/include/hpp/bezier-com-traj/utils.hh b/include/hpp/bezier-com-traj/utils.hh +index 94ec423..879322d 100644 +--- include/hpp/bezier-com-traj/utils.hh ++++ include/hpp/bezier-com-traj/utils.hh +@@ -34,15 +34,16 @@ struct waypoint_t { + + static waypoint_t Zero(size_t dim) { return initwp(dim, dim); } + +- size_t size() const{return second.size();} ++ size_t size() const { return second.size(); } + +- bool isApprox(const waypoint_t& other, const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const{ +- return first.isApprox(other.first,prec) && second.isApprox(other.second,prec); ++ bool isApprox(const waypoint_t& other, ++ const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const { ++ return first.isApprox(other.first, prec) && second.isApprox(other.second, prec); + } + +- bool operator==(const waypoint_t& other) const{ return isApprox(other); } ++ bool operator==(const waypoint_t& other) const { return isApprox(other); } + +- bool operator!=(const waypoint_t& other) const{ return !(*this == other); } ++ bool operator!=(const waypoint_t& other) const { return !(*this == other); } + }; + + /** +@@ -50,7 +51,7 @@ struct waypoint_t { + * @param degree required degree + * @return the bernstein polynoms + */ +-BEZIER_COM_TRAJ_DLLAPI std::vector<curves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree); ++BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree); + + /** + * @brief given the constraints of the problem, and a set of waypoints, return +@@ -141,7 +142,7 @@ Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const dou + i++; + } + } +- return Bezier(wps.begin(), wps.end(), 0.,T); ++ return Bezier(wps.begin(), wps.end(), 0., T); + } + + #endif +diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py +index 44e29eb..f21667a 100644 +--- python/test/binding_tests.py ++++ python/test/binding_tests.py +@@ -1,4 +1,4 @@ +-import curves # noqa - necessary to register curves::bezier_curve ++import ndcurves # noqa - necessary to register ndcurves::bezier_curve + import numpy as np + from numpy import array + from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP +diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt +index bfb7bf5..a4ffd0e 100644 +--- src/CMakeLists.txt ++++ src/CMakeLists.txt +@@ -19,11 +19,11 @@ if (USE_GLPK) + endif(USE_GLPK) + + ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES}) ++TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ndcurves::ndcurves ++ hpp-centroidal-dynamics::hpp-centroidal-dynamics) + + if (USE_GLPK) + TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${glpk_LIBRARY}) + endif(USE_GLPK) +-PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-centroidal-dynamics) +-PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} curves) + + INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib) +diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp +index 4ea7f7a..a74de59 100644 +--- src/common_solve_methods.cpp ++++ src/common_solve_methods.cpp +@@ -6,7 +6,7 @@ + + namespace bezier_com_traj { + std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6_t>& wps, +- const std::vector<curves::Bern<double> >& berns, int numSteps) { ++ const std::vector<ndcurves::Bern<double> >& berns, int numSteps) { + double dt = 1. / double(numSteps); + std::vector<waypoint6_t> res; + for (int i = 0; i < numSteps + 1; ++i) { +diff --git a/src/computeCOMTraj.cpp b/src/computeCOMTraj.cpp +index 08fed94..f8619e4 100644 +--- src/computeCOMTraj.cpp ++++ src/computeCOMTraj.cpp +@@ -25,7 +25,7 @@ bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData, do + bezier_wp_t::t_point_t wps = computeWwaypoints(pData, T); + bezier_wp_t::t_point_t res; + const int DIM_VAR = (int)dimVar(pData); +- std::vector<curves::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size() - 1); ++ std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size() - 1); + double t, b; + for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) { + waypoint_t w = initwp(6, DIM_VAR); +@@ -277,7 +277,7 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub + res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(pData.constraints_.flag_, T, pis, res.x); + computeFinalVelocity(res); + computeFinalAcceleration(res); +- res.dL_of_t_ = bezier_t::zero(3,T); ++ res.dL_of_t_ = bezier_t::zero(3, T); + } + return res; + } +@@ -354,9 +354,9 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa + // create the curves for c and w with symbolic waypoints (ie. depend on y) + bezier_wp_t::t_point_t wps_c = computeConstantWaypointsSymbolic(pData, T); + bezier_wp_t::t_point_t wps_w = computeWwaypoints(pData, T); +- bezier_wp_t c(wps_c.begin(), wps_c.end(),0., T); ++ bezier_wp_t c(wps_c.begin(), wps_c.end(), 0., T); + bezier_wp_t ddc = c.compute_derivate(2); +- bezier_wp_t w(wps_w.begin(), wps_w.end(),0., T); ++ bezier_wp_t w(wps_w.begin(), wps_w.end(), 0., T); + + // for each splitted curves : add the constraints for each waypoints + const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_, (int)w.degree_, useDD); +diff --git a/src/solve_0_step.cpp b/src/solve_0_step.cpp +index 77a4322..309477f 100644 +--- src/solve_0_step.cpp ++++ src/solve_0_step.cpp +@@ -121,7 +121,7 @@ std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, poin + wps.push_back(w3(p0, p1, g, p0X, p1X, gX, alpha)); + wps.push_back(w4(p0, p1, g, p0X, p1X, gX, alpha)); + if (numSteps > 0) { +- std::vector<curves::Bern<double> > berns = ComputeBersteinPolynoms(4); ++ std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms(4); + wps = ComputeDiscretizedWaypoints(wps, berns, numSteps); + } + return wps; +@@ -137,7 +137,7 @@ std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const + wps.push_back(u3(l0, alpha)); + wps.push_back(u4(l0, alpha)); + if (numSteps > 0) { +- std::vector<curves::Bern<double> > berns = ComputeBersteinPolynoms(4); ++ std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms(4); + wps = ComputeDiscretizedWaypoints(wps, berns, numSteps); + } + return wps; +@@ -196,7 +196,7 @@ void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, Resu + wps.push_back(pData.dc0_ * Ts[0] / 3 + pData.c0_); + wps.push_back(res.x.head(3)); + wps.push_back(res.x.head(3)); +- res.c_of_t_ = bezier_t(wps.begin(), wps.end(),0., Ts[0]); ++ res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0]); + } + + void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) { +@@ -205,9 +205,9 @@ void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, Res + wps.push_back(3 * (res.x.tail(3) - pData.l0_)); + wps.push_back(3 * (-res.x.tail(3))); + wps.push_back(Vector3::Zero()); +- res.dL_of_t_ = bezier_t(wps.begin(), wps.end(),0.,Ts[0], 1. / Ts[0]); ++ res.dL_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0], 1. / Ts[0]); + } else +- res.dL_of_t_ = bezier_t::zero(3,Ts[0]); ++ res.dL_of_t_ = bezier_t::zero(3, Ts[0]); + } + + // no angular momentum for now +diff --git a/src/utils.cpp b/src/utils.cpp +index 2034708..ea84d2a 100644 +--- src/utils.cpp ++++ src/utils.cpp +@@ -64,9 +64,9 @@ Matrix3 skew(point_t_tC x) { + return res; + } + +-std::vector<curves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) { +- std::vector<curves::Bern<double> > res; +- for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(curves::Bern<double>(degree, i)); ++std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) { ++ std::vector<ndcurves::Bern<double> > res; ++ for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(ndcurves::Bern<double>(degree, i)); + return res; + } + +diff --git a/tests/test-bezier-symbolic.cpp b/tests/test-bezier-symbolic.cpp +index fe1c487..3154d05 100644 +--- tests/test-bezier-symbolic.cpp ++++ tests/test-bezier-symbolic.cpp +@@ -23,7 +23,7 @@ + #include <hpp/bezier-com-traj/data.hh> + #include <hpp/centroidal-dynamics/centroidal_dynamics.hh> + #include "test_helper.hh" +-#include <curves/bezier_curve.h> ++#include <ndcurves/bezier_curve.h> + + using namespace bezier_com_traj; + const double T = 1.5; +@@ -70,8 +70,8 @@ BOOST_AUTO_TEST_CASE(symbolic_eval_c) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_t c(pts.begin(), pts.end(),0., T); +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_t c(pts.begin(), pts.end(), 0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + + double t = 0.; + while (t < T) { +@@ -86,9 +86,9 @@ BOOST_AUTO_TEST_CASE(symbolic_eval_dc) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_t c(pts.begin(), pts.end(),0., T); ++ bezier_t c(pts.begin(), pts.end(), 0., T); + bezier_t dc = c.compute_derivate(1); +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + bezier_wp_t dc_sym = c_sym.compute_derivate(1); + + double t = 0.; +@@ -104,9 +104,9 @@ BOOST_AUTO_TEST_CASE(symbolic_eval_ddc) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_t c(pts.begin(), pts.end(),0., T); ++ bezier_t c(pts.begin(), pts.end(), 0., T); + bezier_t ddc = c.compute_derivate(2); +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + bezier_wp_t ddc_sym = c_sym.compute_derivate(2); + + double t = 0.; +@@ -122,9 +122,9 @@ BOOST_AUTO_TEST_CASE(symbolic_eval_jc) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_t c(pts.begin(), pts.end(),0., T); ++ bezier_t c(pts.begin(), pts.end(), 0., T); + bezier_t jc = c.compute_derivate(3); +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + bezier_wp_t jc_sym = c_sym.compute_derivate(3); + + double t = 0.; +@@ -140,8 +140,8 @@ BOOST_AUTO_TEST_CASE(symbolic_split_c) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_t c(pts.begin(), pts.end(),0., T); +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_t c(pts.begin(), pts.end(), 0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + + double a, b, t, t1, t2; + for (size_t i = 0; i < 100; ++i) { +@@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(symbolic_split_c_bench) { + point_t y(1, 0.2, 4.5); + pts[2] = y; + +- bezier_wp_t c_sym(wps.begin(), wps.end(),0., T); ++ bezier_wp_t c_sym(wps.begin(), wps.end(), 0., T); + + std::vector<double> values; + for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX); +@@ -190,7 +190,7 @@ BOOST_AUTO_TEST_CASE(symbolic_split_w) { + bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T); + point_t y(1, 0.2, 4.5); + +- bezier_wp_t w(wps.begin(), wps.end(),0., T); ++ bezier_wp_t w(wps.begin(), wps.end(), 0., T); + + double a, b, t, t1, t2; + for (size_t i = 0; i < 100; ++i) { +@@ -212,7 +212,7 @@ BOOST_AUTO_TEST_CASE(symbolic_split_w_bench) { + bezier_wp_t::t_point_t wps = computeWwaypoints(buildPData(), T); + point_t y(1, 0.2, 4.5); + +- bezier_wp_t w(wps.begin(), wps.end(),0., T); ++ bezier_wp_t w(wps.begin(), wps.end(), 0., T); + + std::vector<double> values; + for (int i = 0; i < 100000; ++i) values.push_back((double)rand() / RAND_MAX); diff --git a/py-hpp-rbprm/Makefile b/py-hpp-rbprm/Makefile index 95f0b1d7cc27d16d8a13e05c565c01aa692dfc7a..7c5259fdd65ba38dee297c3c7d097835883d9b26 100644 --- a/py-hpp-rbprm/Makefile +++ b/py-hpp-rbprm/Makefile @@ -18,8 +18,8 @@ include ../../wip/py-simple-humanoid-rbprm/depend.mk include ../../wip/py-talos-rbprm/depend.mk include ../../simulation/hpp-environments/depend.mk include ../../simulation/py-hpp-environments/depend.mk -include ../../math/curves/depend.mk -include ../../math/py-curves/depend.mk +include ../../math/ndcurves/depend.mk +include ../../math/py-ndcurves/depend.mk include ../../path/hpp-core/depend.mk include ../../path/hpp-util/depend.mk diff --git a/py-hpp-rbprm/distinfo b/py-hpp-rbprm/distinfo index 823545ddbdec7e8400e66799f5b47c56b52142fd..b344ead9a13b726aa3726be580ccee42a95e7394 100644 --- a/py-hpp-rbprm/distinfo +++ b/py-hpp-rbprm/distinfo @@ -1,3 +1,4 @@ SHA1 (hpp-rbprm-4.10.1.tar.gz) = 776424323331448bcd7a3d41d1a9a78959334037 RMD160 (hpp-rbprm-4.10.1.tar.gz) = 59c934f62b85e121be026f6d7558452e168e1489 Size (hpp-rbprm-4.10.1.tar.gz) = 1269777 bytes +SHA1 (patch-84) = 0c9994248d9fd6ce110a17936cb1790962fedcf7 diff --git a/py-hpp-rbprm/patches/patch-84 b/py-hpp-rbprm/patches/patch-84 new file mode 100644 index 0000000000000000000000000000000000000000..ce48a30aeea4545373d14857426be2ad261d5d83 --- /dev/null +++ b/py-hpp-rbprm/patches/patch-84 @@ -0,0 +1,129 @@ +From 3207cbd40dc03460222fcec5953dd0acc37de3ae Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Tue, 16 Mar 2021 16:59:45 +0100 +Subject: [PATCH] =?UTF-8?q?curves=20=E2=86=92=20ndcurves?= +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +ref https://github.com/loco-3d/ndcurves/pull/58 +--- + CMakeLists.txt | 1 + + include/hpp/rbprm/interpolation/polynom-trajectory.hh | 4 ++-- + include/hpp/rbprm/interpolation/spline/bezier-path.hh | 4 ++-- + include/hpp/rbprm/interpolation/spline/effector-rrt.hh | 10 +++++----- + src/CMakeLists.txt | 2 +- + src/interpolation/effector-rrt.cc | 4 ++-- + 6 files changed, 13 insertions(+), 12 deletions(-) + +diff --git a/CMakeLists.txt b/CMakeLists.txt +index c213720..f839a66 100644 +--- CMakeLists.txt ++++ CMakeLists.txt +@@ -43,6 +43,7 @@ ADD_PROJECT_DEPENDENCY("hyq-rbprm" REQUIRED) + ADD_PROJECT_DEPENDENCY("simple-humanoid-rbprm" REQUIRED) + ADD_PROJECT_DEPENDENCY("talos-rbprm" REQUIRED) + ADD_PROJECT_DEPENDENCY("example-robot-data" REQUIRED) ++ADD_PROJECT_DEPENDENCY("ndcurves" REQUIRED) + + if(OPENMP_FOUND) + message("OPENMP FOUND") +diff --git a/include/hpp/rbprm/interpolation/polynom-trajectory.hh b/include/hpp/rbprm/interpolation/polynom-trajectory.hh +index d6d95ea..a207fc1 100644 +--- include/hpp/rbprm/interpolation/polynom-trajectory.hh ++++ include/hpp/rbprm/interpolation/polynom-trajectory.hh +@@ -23,14 +23,14 @@ + #include <hpp/core/config.hh> + #include <hpp/core/path.hh> + #include <hpp/rbprm/interpolation/time-dependant.hh> +-#include <curves/curve_abc.h> ++#include <ndcurves/curve_abc.h> + + namespace hpp { + namespace rbprm { + namespace interpolation { + HPP_PREDEF_CLASS(PolynomTrajectory); + typedef boost::shared_ptr<PolynomTrajectory> PolynomTrajectoryPtr_t; +-typedef curves::curve_abc<core::value_type, core::value_type, true, Eigen::Vector3d> Polynom; ++typedef ndcurves::curve_abc<core::value_type, core::value_type, true, Eigen::Vector3d> Polynom; + typedef boost::shared_ptr<Polynom> PolynomPtr_t; + /// Linear interpolation between two configurations + /// +diff --git a/include/hpp/rbprm/interpolation/spline/bezier-path.hh b/include/hpp/rbprm/interpolation/spline/bezier-path.hh +index 87ce090..fe81e33 100644 +--- include/hpp/rbprm/interpolation/spline/bezier-path.hh ++++ include/hpp/rbprm/interpolation/spline/bezier-path.hh +@@ -19,7 +19,7 @@ + #ifndef HPP_RBPRM_BEZIER_PATH_HH + #define HPP_RBPRM_BEZIER_PATH_HH + +-#include <curves/bezier_curve.h> ++#include <ndcurves/bezier_curve.h> + #include <hpp/core/path.hh> + #include <vector> + #include <map> +@@ -27,7 +27,7 @@ + namespace hpp { + namespace rbprm { + +-typedef curves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t; ++typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t; + typedef boost::shared_ptr<bezier_t> bezier_Ptr; + HPP_PREDEF_CLASS(BezierPath); + typedef boost::shared_ptr<BezierPath> BezierPathPtr_t; +diff --git a/include/hpp/rbprm/interpolation/spline/effector-rrt.hh b/include/hpp/rbprm/interpolation/spline/effector-rrt.hh +index 0431b56..26ffcf4 100644 +--- include/hpp/rbprm/interpolation/spline/effector-rrt.hh ++++ include/hpp/rbprm/interpolation/spline/effector-rrt.hh +@@ -32,9 +32,9 @@ + #include <hpp/core/problem.hh> + #include <hpp/core/config-projector.hh> + #include <hpp/rbprm/interpolation/spline/bezier-path.hh> +-#include <curves/exact_cubic.h> +-#include <curves/bezier_curve.h> +-#include <curves/curve_constraint.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/curve_constraint.h> + #include <vector> + #include <map> + +@@ -123,8 +123,8 @@ std::vector<core::PathVectorPtr_t> fitBeziersToPath(RbPrmFullBodyPtr_t fullbody, + const value_type comPathLength, const PathPtr_t fullBodyComPath, + const State& startState, const State& nextState); + +-typedef curves::exact_cubic<double, double, true, Eigen::Matrix<value_type, 3, 1> > exact_cubic_t; +-typedef curves::curve_constraints<Eigen::Matrix<value_type, 3, 1> > curve_constraint_t; ++typedef ndcurves::exact_cubic<double, double, true, Eigen::Matrix<value_type, 3, 1> > exact_cubic_t; ++typedef ndcurves::curve_constraints<Eigen::Matrix<value_type, 3, 1> > curve_constraint_t; + typedef boost::shared_ptr<exact_cubic_t> exact_cubic_Ptr; + + struct SetEffectorRRTConstraints { +diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt +index 024fb9a..a77739b 100644 +--- src/CMakeLists.txt ++++ src/CMakeLists.txt +@@ -84,7 +84,7 @@ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-statistics) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-fcl) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-pinocchio) +-PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} curves) ++PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} ndcurves) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio) + + INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib) +diff --git a/src/interpolation/effector-rrt.cc b/src/interpolation/effector-rrt.cc +index a155e0b..9fd2c55 100644 +--- src/interpolation/effector-rrt.cc ++++ src/interpolation/effector-rrt.cc +@@ -26,8 +26,8 @@ + #include <hpp/constraints/generic-transformation.hh> + #include <hpp/bezier-com-traj/solve_end_effector.hh> + #include <hpp/core/problem-solver.hh> +-#include <curves/helpers/effector_spline.h> +-#include <curves/bezier_curve.h> ++#include <ndcurves/helpers/effector_spline.h> ++#include <ndcurves/bezier_curve.h> + #include <hpp/pinocchio/joint-collection.hh> + + namespace hpp { diff --git a/py-multicontact-api/Makefile b/py-multicontact-api/Makefile index 596cfc4e26ffe701dce874a868bf1ce27cfd21d0..600a5d72e55612971626014d0af250139144a809 100644 --- a/py-multicontact-api/Makefile +++ b/py-multicontact-api/Makefile @@ -15,8 +15,8 @@ USE_PYTHON_ONLY+= yes include ../../devel/jrl-cmakemodules/Makefile.common include ../../wip/${NAME}/depend.mk -include ../../math/curves/depend.mk -include ../../math/py-curves/depend.mk +include ../../math/ndcurves/depend.mk +include ../../math/py-ndcurves/depend.mk include ../../math/py-eigenpy/depend.mk include ../../math/py-pinocchio/depend.mk diff --git a/py-multicontact-api/distinfo b/py-multicontact-api/distinfo index 51a16359e3aa403b0b8360e1139a5fb2f58047a7..3a8c5be8e507fdc446ad62acc05536114429652c 100644 --- a/py-multicontact-api/distinfo +++ b/py-multicontact-api/distinfo @@ -1,3 +1,5 @@ SHA1 (multicontact-api-2.1.0.tar.gz) = 7639cefdb28c5eb0f992b6bb604f97a4cf82dec6 RMD160 (multicontact-api-2.1.0.tar.gz) = 6ae2f6c5214a1da4c4680ed80c7b2c27de32b43d Size (multicontact-api-2.1.0.tar.gz) = 82404503 bytes +SHA1 (patch-21) = 4948a8981ed94c766bf440371f4bd8074042894d +SHA1 (patch-aa) = 0eaa23c5ef1e463afe91d8b6302f2dd1ef9caecc diff --git a/py-multicontact-api/patches/patch-21 b/py-multicontact-api/patches/patch-21 new file mode 100644 index 0000000000000000000000000000000000000000..3d4ad0d3075c2bb21da7819e5cb24a1130fb3183 --- /dev/null +++ b/py-multicontact-api/patches/patch-21 @@ -0,0 +1,633 @@ +From 017a69a2932c08c5515c4d5f46d9d6a25026c6f3 Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Tue, 16 Mar 2021 17:25:23 +0100 +Subject: [PATCH] =?UTF-8?q?curves=20=E2=86=92=20ndcurves?= +MIME-Version: 1.0 +Content-Type: text/plain; charset=UTF-8 +Content-Transfer-Encoding: 8bit + +ref. https://github.com/loco-3d/ndcurves/pull/58 +--- + CMakeLists.txt | 4 +- + bindings/python/scenario/contact-phase.cpp | 16 ++-- + bindings/python/scenario/contact-sequence.cpp | 16 ++-- + examples/display_gepetto_gui.py | 2 +- + .../python/scenario/contact-phase.hpp | 32 ++++---- + .../multicontact-api/geometry/curve-map.hpp | 4 +- + .../scenario/contact-phase.hpp | 62 +++++++-------- + .../scenario/contact-sequence.hpp | 26 +++---- + unittest/examples.cpp | 16 ++-- + unittest/python/scenario.py | 2 +- + unittest/python/serialization_examples.py | 2 +- + unittest/scenario.cpp | 75 ++++++++++--------- + unittest/serialization_versionning.cpp | 17 ++--- + 13 files changed, 137 insertions(+), 137 deletions(-) + +diff --git a/CMakeLists.txt b/CMakeLists.txt +index d687991..1f8ff44 100644 +--- CMakeLists.txt ++++ CMakeLists.txt +@@ -36,7 +36,7 @@ SET(CMAKE_CXX_STANDARD 11) + + # Project dependencies + ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED PKG_CONFIG_REQUIRES "pinocchio >= 2.0.0") +-ADD_PROJECT_DEPENDENCY(curves 0.5.1 REQUIRED PKG_CONFIG_REQUIRES "curves >= 0.5.1") ++ADD_PROJECT_DEPENDENCY(ndcurves 1.0.0 REQUIRED PKG_CONFIG_REQUIRES "ndcurves >= 0.5.1") + IF(NOT CURVES_WITH_PINOCCHIO_SUPPORT) + MESSAGE(FATAL_ERROR "you need to use a curves version compiled with pinocchio support") + ENDIF(NOT CURVES_WITH_PINOCCHIO_SUPPORT) +@@ -59,7 +59,7 @@ INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS}) + + ADD_LIBRARY(${PROJECT_NAME} INTERFACE) + TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} INTERFACE $<INSTALL_INTERFACE:include>) +-TARGET_LINK_LIBRARIES(${PROJECT_NAME} INTERFACE pinocchio::pinocchio curves::curves) ++TARGET_LINK_LIBRARIES(${PROJECT_NAME} INTERFACE pinocchio::pinocchio ndcurves::ndcurves) + + IF(NOT INSTALL_PYTHON_INTERFACE_ONLY) + INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) +diff --git a/bindings/python/scenario/contact-phase.cpp b/bindings/python/scenario/contact-phase.cpp +index 84edd71..cb5a712 100644 +--- bindings/python/scenario/contact-phase.cpp ++++ bindings/python/scenario/contact-phase.cpp +@@ -5,14 +5,14 @@ + #include "multicontact-api/bindings/python/scenario/contact-phase.hpp" + + // required because of the serialization of the curves pointer : +-#include <curves/fwd.h> +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + namespace multicontact_api { + namespace python { +diff --git a/bindings/python/scenario/contact-sequence.cpp b/bindings/python/scenario/contact-sequence.cpp +index c046bb6..411636f 100644 +--- bindings/python/scenario/contact-sequence.cpp ++++ bindings/python/scenario/contact-sequence.cpp +@@ -5,14 +5,14 @@ + #include "multicontact-api/bindings/python/scenario/contact-sequence.hpp" + + // required because of the serialization of the curves pointer : +-#include <curves/fwd.h> +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + namespace multicontact_api { + namespace python { +diff --git a/include/multicontact-api/bindings/python/scenario/contact-phase.hpp b/include/multicontact-api/bindings/python/scenario/contact-phase.hpp +index bf559e8..3004c46 100644 +--- include/multicontact-api/bindings/python/scenario/contact-phase.hpp ++++ include/multicontact-api/bindings/python/scenario/contact-phase.hpp +@@ -11,7 +11,7 @@ + #include "multicontact-api/scenario/contact-phase.hpp" + #include "multicontact-api/bindings/python/serialization/archive.hpp" + #include "multicontact-api/bindings/python/utils/printable.hpp" +-#include <curves/python/python_definitions.h> ++#include <ndcurves/python/python_definitions.h> + #include <boost/python/suite/indexing/map_indexing_suite.hpp> + #include <boost/python/suite/indexing/vector_indexing_suite.hpp> + +@@ -31,10 +31,10 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + typedef typename ContactPhase::point3_t point3_t; + typedef typename ContactPhase::point6_t point6_t; + typedef typename ContactPhase::pointX_t pointX_t; +- typedef curves::t_pointX_t t_pointX_t; +- typedef curves::t_time_t t_time_t; +- typedef curves::pointX_list_t pointX_list_t; +- typedef curves::time_waypoints_t time_waypoints_t; ++ typedef ndcurves::t_pointX_t t_pointX_t; ++ typedef ndcurves::t_time_t t_time_t; ++ typedef ndcurves::pointX_list_t pointX_list_t; ++ typedef ndcurves::time_waypoints_t time_waypoints_t; + + // call macro for all ContactPhase methods that can be overloaded + BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isConsistent_overloads, ContactPhase::isConsistent, 0, 1) +@@ -291,11 +291,11 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + const pointX_list_t& points_derivative, + const pointX_list_t& points_second_derivative, + const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); + t_pointX_t points_second_derivative_list = +- curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setCOMtrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, + time_points_list); + return; +@@ -305,11 +305,11 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + const pointX_list_t& points_derivative, + const pointX_list_t& points_second_derivative, + const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); + t_pointX_t points_second_derivative_list = +- curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setJointsTrajectoryFromPoints(points_list, points_derivative_list, points_second_derivative_list, + time_points_list); + return; +@@ -317,9 +317,9 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi + + static void setAMtrajectoryFromPoints(ContactPhase& self, const pointX_list_t& points, + const pointX_list_t& points_derivative, const time_waypoints_t& time_points) { +- t_pointX_t points_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); +- t_pointX_t points_derivative_list = curves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); +- t_time_t time_points_list = curves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); ++ t_pointX_t points_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points); ++ t_pointX_t points_derivative_list = ndcurves::vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative); ++ t_time_t time_points_list = ndcurves::vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points); + self.setAMtrajectoryFromPoints(points_list, points_derivative_list, time_points_list); + return; + } +diff --git a/include/multicontact-api/geometry/curve-map.hpp b/include/multicontact-api/geometry/curve-map.hpp +index 9dcb7e3..f3db303 100644 +--- include/multicontact-api/geometry/curve-map.hpp ++++ include/multicontact-api/geometry/curve-map.hpp +@@ -4,7 +4,7 @@ + #ifndef __multicontact_api_geometry_curve_map_hpp__ + #define __multicontact_api_geometry_curve_map_hpp__ + +-#include <curves/curve_abc.h> ++#include <ndcurves/curve_abc.h> + #include <map> + #include <string> + +@@ -15,7 +15,7 @@ + #include <boost/serialization/string.hpp> + #include <boost/serialization/map.hpp> + #include <boost/serialization/base_object.hpp> +-#include <curves/serialization/registeration.hpp> ++#include <ndcurves/serialization/registeration.hpp> + + template <typename Curve> + struct CurveMap : public std::map<std::string, Curve> { +diff --git a/include/multicontact-api/scenario/contact-phase.hpp b/include/multicontact-api/scenario/contact-phase.hpp +index 3a1b4d1..da2bfea 100644 +--- include/multicontact-api/scenario/contact-phase.hpp ++++ include/multicontact-api/scenario/contact-phase.hpp +@@ -9,9 +9,9 @@ + #include "multicontact-api/serialization/eigen-matrix.hpp" + #include "multicontact-api/serialization/spatial.hpp" + +-#include <curves/fwd.h> +-#include <curves/piecewise_curve.h> +-#include <curves/serialization/curves.hpp> ++#include <ndcurves/fwd.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/serialization/curves.hpp> + #include <map> + #include <vector> + #include <set> +@@ -31,22 +31,22 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + typedef _Scalar Scalar; + + // Eigen types +- typedef curves::pointX_t pointX_t; +- typedef curves::point3_t point3_t; +- typedef curves::point6_t point6_t; +- typedef curves::t_point3_t t_point3_t; +- typedef curves::t_pointX_t t_pointX_t; +- typedef curves::transform_t transform_t; ++ typedef ndcurves::pointX_t pointX_t; ++ typedef ndcurves::point3_t point3_t; ++ typedef ndcurves::point6_t point6_t; ++ typedef ndcurves::t_point3_t t_point3_t; ++ typedef ndcurves::t_pointX_t t_pointX_t; ++ typedef ndcurves::transform_t transform_t; + + // Curves types +- typedef curves::curve_abc_t curve_t; +- // typedef curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t; +- typedef curves::curve_SE3_t curve_SE3_t; +- typedef curves::curve_ptr_t curve_ptr; ++ typedef ndcurves::curve_abc_t curve_t; ++ // typedef ndcurves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t; ++ typedef ndcurves::curve_SE3_t curve_SE3_t; ++ typedef ndcurves::curve_ptr_t curve_ptr; + // typedef boost::shared_ptr<curve_3_t> curve_3_ptr; +- typedef curves::curve_SE3_ptr_t curve_SE3_ptr; +- typedef curves::piecewise3_t piecewise3_t; +- typedef curves::piecewise_t piecewise_t; ++ typedef ndcurves::curve_SE3_ptr_t curve_SE3_ptr; ++ typedef ndcurves::piecewise3_t piecewise3_t; ++ typedef ndcurves::piecewise_t piecewise_t; + typedef piecewise_t::t_time_t t_time_t; + + typedef std::vector<std::string> t_strings; +@@ -452,7 +452,7 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setCOMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_pointX_t& points_second_derivative, const t_time_t& time_points) { + /* +- piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t c_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); + if (c_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + m_c = curve_ptr(new piecewise_t(c_t)); +@@ -460,10 +460,10 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + m_ddc = curve_ptr(c_t.compute_derivate_ptr(2)); + */ + m_c = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dc = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); +- m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); ++ m_ddc = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points_second_derivative, time_points))); + if (m_c->dim() != 3 || m_dc->dim() != 3 || m_ddc->dim() != 3) + throw std::invalid_argument("Dimension of the points must be 3."); +@@ -489,16 +489,16 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setAMtrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_time_t& time_points) { + /* +- piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t L_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, time_points); + if (L_t.dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + m_L = curve_ptr(new piecewise_t(L_t)); + m_dL = curve_ptr(L_t.compute_derivate_ptr(1)); + */ + m_L = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dL = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); + if (m_L->dim() != 3 || m_dL->dim() != 3) throw std::invalid_argument("Dimension of the points must be 3."); + + m_L_init = point3_t(points.front()); +@@ -521,17 +521,17 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + void setJointsTrajectoryFromPoints(const t_pointX_t& points, const t_pointX_t& points_derivative, + const t_pointX_t& points_second_derivative, const t_time_t& time_points) { + /* +- piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t q_t = piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); + m_q = curve_ptr(new piecewise_t(q_t)); + m_dq = curve_ptr(q_t.compute_derivate_ptr(1)); + m_ddq = curve_ptr(q_t.compute_derivate_ptr(2)); + */ + m_q = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points, time_points))); ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points, time_points))); + m_dq = curve_ptr(new piecewise_t( +- piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points_derivative, time_points))); +- m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points_derivative, time_points))); ++ m_ddq = curve_ptr(new piecewise_t(piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points_second_derivative, time_points))); + m_q_init = points.front(); + m_q_final = points.back(); +@@ -660,13 +660,13 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca + template <class Archive> + void serialize(Archive& ar, const unsigned int version) { + // ar& boost::serialization::make_nvp("placement", m_placement); +- unsigned int curve_version; // Curves API version related to the archive multicontact-api API version +- if(version <2){ ++ unsigned int curve_version; // Curves API version related to the archive multicontact-api API version ++ if (version < 2) { + curve_version = 0; +- }else{ ++ } else { + curve_version = 1; + } +- curves::serialization::register_types<Archive>(ar, curve_version); ++ ndcurves::serialization::register_types<Archive>(ar, curve_version); + ar& boost::serialization::make_nvp("c_init", m_c_init); + ar& boost::serialization::make_nvp("dc_init", m_dc_init); + ar& boost::serialization::make_nvp("ddc_init", m_ddc_init); +diff --git a/include/multicontact-api/scenario/contact-sequence.hpp b/include/multicontact-api/scenario/contact-sequence.hpp +index 21a5022..690aea0 100644 +--- include/multicontact-api/scenario/contact-sequence.hpp ++++ include/multicontact-api/scenario/contact-sequence.hpp +@@ -3,11 +3,11 @@ + + #include "multicontact-api/scenario/fwd.hpp" + #include "multicontact-api/scenario/contact-phase.hpp" +-#include <curves/fwd.h> +-#include <curves/curve_abc.h> +-#include <curves/piecewise_curve.h> +-#include <curves/polynomial.h> +-#include <curves/se3_curve.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/curve_abc.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/se3_curve.h> + #include "multicontact-api/serialization/archive.hpp" + + #include <vector> +@@ -25,16 +25,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp + typedef typename ContactPhase::SE3 SE3; + typedef std::vector<ContactPhase> ContactPhaseVector; + +- typedef curves::pointX_t pointX_t; +- typedef curves::transform_t transform_t; +- typedef curves::curve_abc_t curve_t; +- typedef curves::curve_SE3_t curve_SE3_t; ++ typedef ndcurves::pointX_t pointX_t; ++ typedef ndcurves::transform_t transform_t; ++ typedef ndcurves::curve_abc_t curve_t; ++ typedef ndcurves::curve_SE3_t curve_SE3_t; + typedef boost::shared_ptr<curve_t> curve_ptr; + typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr; +- typedef curves::piecewise_t piecewise_t; +- typedef curves::piecewise_SE3_t piecewise_SE3_t; +- typedef curves::SE3Curve_t SE3Curve_t; +- typedef curves::polynomial_t polynomial_t; ++ typedef ndcurves::piecewise_t piecewise_t; ++ typedef ndcurves::piecewise_SE3_t piecewise_SE3_t; ++ typedef ndcurves::SE3Curve_t SE3Curve_t; ++ typedef ndcurves::polynomial_t polynomial_t; + + ContactSequenceTpl(const size_t size = 0) : m_contact_phases(size) {} + +diff --git a/unittest/examples.cpp b/unittest/examples.cpp +index 4db65e2..37698ca 100644 +--- unittest/examples.cpp ++++ unittest/examples.cpp +@@ -9,14 +9,14 @@ + + #include "multicontact-api/scenario/contact-sequence.hpp" + #include "multicontact-api/scenario/fwd.hpp" +-#include "curves/fwd.h" +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + /** + * This unit test try to deserialize the ContactSequences in the examples folder +diff --git a/unittest/python/scenario.py b/unittest/python/scenario.py +index 32162f1..4b46cd9 100644 +--- unittest/python/scenario.py ++++ unittest/python/scenario.py +@@ -5,7 +5,7 @@ + from random import uniform + + import numpy as np +-from curves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial ++from ndcurves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial + from numpy import array, array_equal, isclose, random + + import pinocchio as pin +diff --git a/unittest/python/serialization_examples.py b/unittest/python/serialization_examples.py +index d0158b2..956f4a4 100644 +--- unittest/python/serialization_examples.py ++++ unittest/python/serialization_examples.py +@@ -1,7 +1,7 @@ + import pathlib + import unittest + +-import curves # noqa: requiered to get C++ type exposition ++import ndcurves # noqa: requiered to get C++ type exposition + + import pinocchio as pin + from multicontact_api import ContactSequence +diff --git a/unittest/scenario.cpp b/unittest/scenario.cpp +index 6df4481..c5fc643 100644 +--- unittest/scenario.cpp ++++ unittest/scenario.cpp +@@ -12,25 +12,25 @@ + #include "multicontact-api/scenario/contact-phase.hpp" + #include "multicontact-api/scenario/contact-sequence.hpp" + +-#include <curves/fwd.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> + + typedef Eigen::Matrix<double, 1, 1> point1_t; +-using curves::point3_t; +-using curves::point6_t; ++using ndcurves::point3_t; ++using ndcurves::point6_t; + typedef Eigen::Matrix<double, 12, 1> point12_t; +-using curves::curve_ptr_t; +-using curves::curve_SE3_ptr_t; +-using curves::matrix3_t; +-using curves::piecewise_SE3_t; +-using curves::piecewise_t; +-using curves::pointX_t; +-using curves::quaternion_t; +-using curves::t_point3_t; +-using curves::t_pointX_t; ++using ndcurves::curve_ptr_t; ++using ndcurves::curve_SE3_ptr_t; ++using ndcurves::matrix3_t; ++using ndcurves::piecewise_SE3_t; ++using ndcurves::piecewise_t; ++using ndcurves::pointX_t; ++using ndcurves::quaternion_t; ++using ndcurves::t_point3_t; ++using ndcurves::t_pointX_t; + using namespace multicontact_api::scenario; + typedef ContactSequence::ContactPhaseVector ContactPhaseVector; + typedef ContactModel::Matrix3X Matrix3X; +@@ -76,9 +76,9 @@ curve_ptr_t buildPiecewisePolynomialC2() { + time_points.push_back(t2); + time_points.push_back(t3); + +- curves::piecewise_t ppc_C2 = curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>( ++ ndcurves::piecewise_t ppc_C2 = ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>( + points, points_derivative, points_second_derivative, time_points); +- curve_ptr_t res(new curves::piecewise_t(ppc_C2)); ++ curve_ptr_t res(new ndcurves::piecewise_t(ppc_C2)); + return res; + } + +@@ -99,7 +99,7 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) { + vec.push_back(b); + vec.push_back(c); + vec.push_back(d); +- curve_ptr_t res(new curves::polynomial_t(vec.begin(), vec.end(), min, max)); ++ curve_ptr_t res(new ndcurves::polynomial_t(vec.begin(), vec.end(), min, max)); + return res; + } + +@@ -123,7 +123,7 @@ curve_SE3_ptr_t buildPiecewiseSE3() { + quaternion_t q1(0., 1., 0, 0); + pointX_t p0 = point3_t(1., 1.5, -2.); + pointX_t p1 = point3_t(3., 0, 1.); +- curve_SE3_ptr_t cLinear(new curves::SE3Curve_t(p0, p1, q0, q1, min, mid)); ++ curve_SE3_ptr_t cLinear(new ndcurves::SE3Curve_t(p0, p1, q0, q1, min, mid)); + point3_t a(1, 2, 3); + point3_t b(2, 3, 4); + point3_t c(3, 4, 5); +@@ -133,12 +133,13 @@ curve_SE3_ptr_t buildPiecewiseSE3() { + params.push_back(b); + params.push_back(c); + params.push_back(d); +- boost::shared_ptr<curves::bezier_t> translation_bezier(new curves::bezier_t(params.begin(), params.end(), mid, max)); +- curve_SE3_ptr_t cBezier(new curves::SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix())); ++ boost::shared_ptr<ndcurves::bezier_t> translation_bezier( ++ new ndcurves::bezier_t(params.begin(), params.end(), mid, max)); ++ curve_SE3_ptr_t cBezier(new ndcurves::SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix())); + +- curves::piecewise_SE3_t piecewiseSE3(cLinear); ++ ndcurves::piecewise_SE3_t piecewiseSE3(cLinear); + piecewiseSE3.add_curve_ptr(cBezier); +- curve_SE3_ptr_t res = boost::make_shared<curves::piecewise_SE3_t>(piecewiseSE3); ++ curve_SE3_ptr_t res = boost::make_shared<ndcurves::piecewise_SE3_t>(piecewiseSE3); + return res; + } + +@@ -157,7 +158,7 @@ curve_SE3_ptr_t buildRandomSE3LinearTraj(const double min, const double max) { + pointX_t p0 = point3_t::Random(); + pointX_t p1 = point3_t::Random(); + +- curve_SE3_ptr_t res(new curves::SE3Curve_t(p0, p1, q0, q1, min, max)); ++ curve_SE3_ptr_t res(new ndcurves::SE3Curve_t(p0, p1, q0, q1, min, max)); + return res; + } + +@@ -599,7 +600,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + pointX_t f1 = point12_t::Random(); + const double min = 0.5; + const double max = 2.; +- curve_ptr_t force12d(new curves::polynomial_t(f0, f1, min, max)); ++ curve_ptr_t force12d(new ndcurves::polynomial_t(f0, f1, min, max)); + bool newTraj = cp2.addContactForceTrajectory("left_leg", force12d); + BOOST_CHECK(newTraj); + newTraj = cp2.addContactForceTrajectory("left_leg", force12d); +@@ -612,7 +613,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + BOOST_CHECK((*cp2.contactForces("left_leg"))(1.2) == (*force12d)(1.2)); + pointX_t f0_1 = point12_t::Random(); + pointX_t f1_1 = point12_t::Random(); +- curve_ptr_t force12d_1(new curves::polynomial_t(f0_1, f1_1, min, max)); ++ curve_ptr_t force12d_1(new ndcurves::polynomial_t(f0_1, f1_1, min, max)); + cp2.contactForces().insert( + std::pair<std::string, curve_ptr_t>("right_leg", force12d_1)); // should not have any effect only const getter + BOOST_CHECK(cp2.contactForces().count("right_leg") == 0); +@@ -633,7 +634,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + nf0 << 56.3; + pointX_t nf1(1); + nf1 << 5893.2; +- curve_ptr_t force1d(new curves::polynomial_t(nf0, nf1, min, max)); ++ curve_ptr_t force1d(new ndcurves::polynomial_t(nf0, nf1, min, max)); + newTraj = cp2.addContactNormalForceTrajectory("left_leg", force1d); + BOOST_CHECK(newTraj); + newTraj = cp2.addContactNormalForceTrajectory("left_leg", force1d); +@@ -648,7 +649,7 @@ BOOST_AUTO_TEST_CASE(contact_phase) { + nf0_1 << 147.2; + pointX_t nf1_1(1); + nf1_1 << 562; +- curve_ptr_t force1d_1(new curves::polynomial_t(nf0_1, nf1_1, min, max)); ++ curve_ptr_t force1d_1(new ndcurves::polynomial_t(nf0_1, nf1_1, min, max)); + cp2.contactNormalForces().insert( + std::pair<std::string, curve_ptr_t>("right_leg", force1d_1)); // should not have any effect only const getter + BOOST_CHECK(cp2.contactNormalForces().count("right_leg") == 0); +@@ -1798,12 +1799,12 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_com_traj) { + time_points2.push_back(t3); + time_points2.push_back(t4); + +- curves::piecewise_t c1 = +- curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points1, time_points1); +- curve_ptr_t c1_ptr(new curves::piecewise_t(c1)); +- curves::piecewise_t c2 = +- curves::piecewise_t::convert_discrete_points_to_polynomial<curves::polynomial_t>(points2, time_points2); +- curve_ptr_t c2_ptr(new curves::piecewise_t(c2)); ++ ndcurves::piecewise_t c1 = ++ ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points1, time_points1); ++ curve_ptr_t c1_ptr(new ndcurves::piecewise_t(c1)); ++ ndcurves::piecewise_t c2 = ++ ndcurves::piecewise_t::convert_discrete_points_to_polynomial<ndcurves::polynomial_t>(points2, time_points2); ++ curve_ptr_t c2_ptr(new ndcurves::piecewise_t(c2)); + cp0.m_c = c1_ptr; + cp1.m_c = c2_ptr; + BOOST_CHECK(cp0.m_c->min() == 0.); +@@ -1837,8 +1838,8 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_effector_traj) { + pointX_t p1 = point3_t::Random(); + pointX_t p2 = point3_t::Random(); + +- curve_SE3_ptr_t traj_0(new curves::SE3Curve_t(p0, p1, q0, q1, 0., 2.)); +- curve_SE3_ptr_t traj_2(new curves::SE3Curve_t(p1, p2, q1, q2, 4., 8.)); ++ curve_SE3_ptr_t traj_0(new ndcurves::SE3Curve_t(p0, p1, q0, q1, 0., 2.)); ++ curve_SE3_ptr_t traj_2(new ndcurves::SE3Curve_t(p1, p2, q1, q2, 4., 8.)); + cp0.addEffectorTrajectory("right_leg", traj_0); + cp2.addEffectorTrajectory("right_leg", traj_2); + +diff --git a/unittest/serialization_versionning.cpp b/unittest/serialization_versionning.cpp +index ef6a935..fc58353 100644 +--- unittest/serialization_versionning.cpp ++++ unittest/serialization_versionning.cpp +@@ -9,14 +9,14 @@ + + #include "multicontact-api/scenario/contact-sequence.hpp" + #include "multicontact-api/scenario/fwd.hpp" +-#include "curves/fwd.h" +-#include <curves/so3_linear.h> +-#include <curves/se3_curve.h> +-#include <curves/polynomial.h> +-#include <curves/bezier_curve.h> +-#include <curves/piecewise_curve.h> +-#include <curves/exact_cubic.h> +-#include <curves/cubic_hermite_spline.h> ++#include <ndcurves/fwd.h> ++#include <ndcurves/so3_linear.h> ++#include <ndcurves/se3_curve.h> ++#include <ndcurves/polynomial.h> ++#include <ndcurves/bezier_curve.h> ++#include <ndcurves/piecewise_curve.h> ++#include <ndcurves/exact_cubic.h> ++#include <ndcurves/cubic_hermite_spline.h> + + /** + * This unit test try to deserialize the ContactSequences in the examples/previous_versions folder +@@ -62,5 +62,4 @@ BOOST_AUTO_TEST_CASE(api_1) { + BOOST_CHECK(cs.haveContactModelDefined()); + } + +- + BOOST_AUTO_TEST_SUITE_END() diff --git a/py-multicontact-api/patches/patch-aa b/py-multicontact-api/patches/patch-aa new file mode 100644 index 0000000000000000000000000000000000000000..665cb186a42241b22f630cc7e9234a227214d92a --- /dev/null +++ b/py-multicontact-api/patches/patch-aa @@ -0,0 +1,31 @@ +From 0c941d6ee950e498e3c7d473e6d1c6b1af0a2668 Mon Sep 17 00:00:00 2001 +From: Guilhem Saurel <guilhem.saurel@laas.fr> +Date: Wed, 17 Mar 2021 12:19:20 +0100 +Subject: [PATCH] [tests] add extension + +--- + unittest/CMakeLists.txt | 4 ++-- + 1 file changed, 2 insertions(+), 2 deletions(-) + +diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt +index c777772..998934b 100644 +--- unittest/CMakeLists.txt ++++ unittest/CMakeLists.txt +@@ -1,4 +1,4 @@ +-# Copyright (c) 2015-2020, CNRS ++# Copyright (c) 2015-2021, CNRS + # Authors: Justin Carpentier <jcarpent@laas.fr>, Guilhem Saurel + + ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK) +@@ -11,7 +11,7 @@ SET(${PROJECT_NAME}_TESTS + ) + + FOREACH(TEST ${${PROJECT_NAME}_TESTS}) +- ADD_UNIT_TEST(${TEST} ${TEST}) ++ ADD_UNIT_TEST(${TEST} ${TEST}.cpp) + TARGET_LINK_LIBRARIES(${TEST} ${PROJECT_NAME} ${Boost_LIBRARIES}) + ENDFOREACH(TEST ${${PROJECT_NAME}_TESTS}) + +-- +2.17.1 +