Skip to content
GitLab
Commit 5e88b794 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge remote-tracking branch 'main/devel' into devel

parents 55a22160 c0d4e0e5
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 5e88b794
......@@ -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)
......
bindings/python/scenario/contact-phase.cpp
View file @ 5e88b794
......@@ -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 {
......
bindings/python/scenario/contact-sequence.cpp
View file @ 5e88b794
......@@ -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 {
......
examples/display_gepetto_gui.py
View file @ 5e88b794
import curves
from multicontact_api import ContactSequence
import gepetto.corbaserver
import pinocchio as pin
from rospkg import RosPack
import time
import argparse
import subprocess
import atexit
import os
import subprocess
import time
import ndcurves
import gepetto.corbaserver
from multicontact_api import ContactSequence
from rospkg import RosPack
import pinocchio as pin
# Define robot model
# Define robot model
robot_package_name = "talos_data"
urdf_name = "talos_reduced"
# Define environment
env_package_name = "hpp_environments"
env_name = "multicontact/ground" # default value, may be defined with argument
env_name = "multicontact/ground" # default value, may be defined with argument
scene_name = "world"
# timestep used to display the configurations
DT_DISPLAY = 0.04 # 25 fps
DT_DISPLAY = 0.04 # 25 fps
def display_wb(robot, q_t):
t = q_t.min()
while t <= q_t.max():
t_start = time.time()
robot.display(q_t(t))
t += DT_DISPLAY
elapsed = time.time() - t_start
if elapsed > DT_DISPLAY:
print("Warning : display not real time ! choose a greater time step for the display.")
else:
time.sleep(DT_DISPLAY - elapsed)
# display last config if the total duration is not a multiple of the dt
robot.display(q_t(q_t.max()))
t = q_t.min()
while t <= q_t.max():
t_start = time.time()
robot.display(q_t(t))
t += DT_DISPLAY
elapsed = time.time() - t_start
if elapsed > DT_DISPLAY:
print("Warning : display not real time ! choose a greater time step for the display.")
else:
time.sleep(DT_DISPLAY - elapsed)
# display last config if the total duration is not a multiple of the dt
robot.display(q_t(q_t.max()))
if __name__ == '__main__':
# Get cs_name from the arguments:
parser = argparse.ArgumentParser(description="Load a ContactSequence and display the joint-trajectory in gepetto-gui")
parser.add_argument('cs_name', type=str, help="The name of the serialized ContactSequence file")
parser.add_argument('--env_name', type=str, help="The name of environment urdf file in hpp_environments")
args = parser.parse_args()
cs_name = args.cs_name
if args.env_name:
env_name = args.env_name
# Start the gepetto-gui background process
subprocess.run(["killall", "gepetto-gui"])
process_viewer = subprocess.Popen("gepetto-gui",
# Get cs_name from the arguments:
parser = argparse.ArgumentParser(
description="Load a ContactSequence and display the joint-trajectory in gepetto-gui")
parser.add_argument('cs_name', type=str, help="The name of the serialized ContactSequence file")
parser.add_argument('--env_name', type=str, help="The name of environment urdf file in hpp_environments")
args = parser.parse_args()
cs_name = args.cs_name
if args.env_name:
env_name = args.env_name
# Start the gepetto-gui background process
subprocess.run(["killall", "gepetto-gui"])
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
atexit.register(process_viewer.kill)
# Load robot model in pinocchio
rp = RosPack()
urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf'
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
# Load environment model
cl = gepetto.corbaserver.Client()
gui = cl.gui
env_package_path = rp.get_path(env_package_name)
env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf'
gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True)
# Load the motion from the multicontact-api file
cs = ContactSequence()
cs.loadFromBinary(cs_name)
assert cs.haveJointsTrajectories(), "The file loaded do not have joint trajectories stored."
q_t = cs.concatenateQtrajectories()
display_wb(robot, q_t)
atexit.register(process_viewer.kill)
# Load robot model in pinocchio
rp = RosPack()
urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf'
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer())
robot.initViewer(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
# Load environment model
cl = gepetto.corbaserver.Client()
gui = cl.gui
env_package_path = rp.get_path(env_package_name)
env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf'
gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True)
# Load the motion from the multicontact-api file
cs = ContactSequence()
cs.loadFromBinary(cs_name)
assert cs.haveJointsTrajectories(), "The file loaded do not have joint trajectories stored."
q_t = cs.concatenateQtrajectories()
display_wb(robot, q_t)
include/multicontact-api/bindings/python/scenario/contact-phase.hpp
View file @ 5e88b794
......@@ -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;
}
......
include/multicontact-api/geometry/curve-map.hpp
View file @ 5e88b794
......@@ -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> {
......
include/multicontact-api/scenario/contact-phase.hpp
View file @ 5e88b794
......@@ -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);
......
include/multicontact-api/scenario/contact-sequence.hpp
View file @ 5e88b794
......@@ -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) {}
......
unittest/examples.cpp
View file @ 5e88b794
......@@ -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
......
unittest/python/scenario.py
View file @ 5e88b794
......@@ -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
......
unittest/python/serialization_examples.py
View file @ 5e88b794
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
......
unittest/scenario.cpp
View file @ 5e88b794
......@@ -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()));