Commit 017a69a2 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files
parent 1d063f50
Pipeline #13589 passed with stage
in 15 minutes and 3 seconds
......@@ -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)
......
......@@ -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 {
......
......@@ -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 {
......
......@@ -4,7 +4,7 @@ import os
import subprocess
import time
import curves
import ndcurves
import gepetto.corbaserver
from multicontact_api import ContactSequence
from rospkg import RosPack
......
......@@ -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;
}
......
......@@ -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> {
......
......@@ -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);
......
......@@ -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) {}
......
......@@ -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
......
......@@ -5,7 +5,7 @@ from math import cos, sin, sqrt
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
......
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
......
......@@ -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);
......
......@@ -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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment