Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
loco-3d
Multicontact-api
Commits
c0d4e0e5
Unverified
Commit
c0d4e0e5
authored
Mar 19, 2021
by
Guilhem Saurel
Committed by
GitHub
Mar 19, 2021
Browse files
Merge pull request #21 from nim65s/devel
curves → ndcurves
parents
1d063f50
017a69a2
Pipeline
#13652
passed with stage
in 12 minutes and 24 seconds
Changes
13
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
c0d4e0e5
...
...
@@ -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
(
nd
curves
1.0.0
REQUIRED PKG_CONFIG_REQUIRES
"
nd
curves >= 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
nd
curves::
nd
curves
)
IF
(
NOT INSTALL_PYTHON_INTERFACE_ONLY
)
INSTALL
(
TARGETS
${
PROJECT_NAME
}
EXPORT
${
TARGETS_EXPORT_NAME
}
DESTINATION lib
)
...
...
bindings/python/scenario/contact-phase.cpp
View file @
c0d4e0e5
...
...
@@ -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
<
nd
curves/fwd.h>
#include
<
nd
curves/so3_linear.h>
#include
<
nd
curves/se3_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/bezier_curve.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/exact_cubic.h>
#include
<
nd
curves/cubic_hermite_spline.h>
namespace
multicontact_api
{
namespace
python
{
...
...
bindings/python/scenario/contact-sequence.cpp
View file @
c0d4e0e5
...
...
@@ -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
<
nd
curves/fwd.h>
#include
<
nd
curves/so3_linear.h>
#include
<
nd
curves/se3_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/bezier_curve.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/exact_cubic.h>
#include
<
nd
curves/cubic_hermite_spline.h>
namespace
multicontact_api
{
namespace
python
{
...
...
examples/display_gepetto_gui.py
View file @
c0d4e0e5
...
...
@@ -4,7 +4,7 @@ import os
import
subprocess
import
time
import
curves
import
nd
curves
import
gepetto.corbaserver
from
multicontact_api
import
ContactSequence
from
rospkg
import
RosPack
...
...
include/multicontact-api/bindings/python/scenario/contact-phase.hpp
View file @
c0d4e0e5
...
...
@@ -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
<
nd
curves/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
nd
curves
::
t_pointX_t
t_pointX_t
;
typedef
nd
curves
::
t_time_t
t_time_t
;
typedef
nd
curves
::
pointX_list_t
pointX_list_t
;
typedef
nd
curves
::
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
=
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points
);
t_pointX_t
points_derivative_list
=
nd
curves
::
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
);
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points_second_derivative
);
t_time_t
time_points_list
=
nd
curves
::
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
=
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points
);
t_pointX_t
points_derivative_list
=
nd
curves
::
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
);
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points_second_derivative
);
t_time_t
time_points_list
=
nd
curves
::
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
=
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points
);
t_pointX_t
points_derivative_list
=
nd
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points_derivative
);
t_time_t
time_points_list
=
nd
curves
::
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 @
c0d4e0e5
...
...
@@ -4,7 +4,7 @@
#ifndef __multicontact_api_geometry_curve_map_hpp__
#define __multicontact_api_geometry_curve_map_hpp__
#include
<curves/curve_abc.h>
#include
<
nd
curves/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
<
nd
curves/serialization/registeration.hpp>
template
<
typename
Curve
>
struct
CurveMap
:
public
std
::
map
<
std
::
string
,
Curve
>
{
...
...
include/multicontact-api/scenario/contact-phase.hpp
View file @
c0d4e0e5
...
...
@@ -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
<
nd
curves/fwd.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/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
nd
curves
::
pointX_t
pointX_t
;
typedef
nd
curves
::
point3_t
point3_t
;
typedef
nd
curves
::
point6_t
point6_t
;
typedef
nd
curves
::
t_point3_t
t_point3_t
;
typedef
nd
curves
::
t_pointX_t
t_pointX_t
;
typedef
nd
curves
::
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
nd
curves
::
curve_abc_t
curve_t
;
// typedef
nd
curves::curve_abc<Scalar, Scalar, true, point3_t> curve_3_t;
typedef
nd
curves
::
curve_SE3_t
curve_SE3_t
;
typedef
nd
curves
::
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
nd
curves
::
curve_SE3_ptr_t
curve_SE3_ptr
;
typedef
nd
curves
::
piecewise3_t
piecewise3_t
;
typedef
nd
curves
::
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<
nd
curves::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
<
nd
curves
::
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
<
nd
curves
::
polynomial_t
>
(
points_derivative
,
time_points
)));
m_ddc
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
nd
curves
::
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<
nd
curves::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
<
nd
curves
::
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
<
nd
curves
::
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<
nd
curves::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
<
nd
curves
::
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
<
nd
curves
::
polynomial_t
>
(
points_derivative
,
time_points
)));
m_ddq
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
nd
curves
::
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
);
nd
curves
::
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 @
c0d4e0e5
...
...
@@ -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
<
nd
curves/fwd.h>
#include
<
nd
curves/curve_abc.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/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
nd
curves
::
pointX_t
pointX_t
;
typedef
nd
curves
::
transform_t
transform_t
;
typedef
nd
curves
::
curve_abc_t
curve_t
;
typedef
nd
curves
::
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
nd
curves
::
piecewise_t
piecewise_t
;
typedef
nd
curves
::
piecewise_SE3_t
piecewise_SE3_t
;
typedef
nd
curves
::
SE3Curve_t
SE3Curve_t
;
typedef
nd
curves
::
polynomial_t
polynomial_t
;
ContactSequenceTpl
(
const
size_t
size
=
0
)
:
m_contact_phases
(
size
)
{}
...
...
unittest/examples.cpp
View file @
c0d4e0e5
...
...
@@ -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
<nd
curves/fwd.h
>
#include
<
nd
curves/so3_linear.h>
#include
<
nd
curves/se3_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/bezier_curve.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/exact_cubic.h>
#include
<
nd
curves/cubic_hermite_spline.h>
/**
* This unit test try to deserialize the ContactSequences in the examples folder
...
...
unittest/python/scenario.py
View file @
c0d4e0e5
...
...
@@ -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
nd
curves
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 @
c0d4e0e5
import
pathlib
import
unittest
import
curves
# noqa: requiered to get C++ type exposition
import
nd
curves
# noqa: requiered to get C++ type exposition
import
pinocchio
as
pin
from
multicontact_api
import
ContactSequence
...
...
unittest/scenario.cpp
View file @
c0d4e0e5
...
...
@@ -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
<
nd
curves/fwd.h>
#include
<
nd
curves/se3_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/bezier_curve.h>
#include
<
nd
curves/piecewise_curve.h>
typedef
Eigen
::
Matrix
<
double
,
1
,
1
>
point1_t
;
using
curves
::
point3_t
;
using
curves
::
point6_t
;
using
nd
curves
::
point3_t
;
using
nd
curves
::
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
nd
curves
::
curve_ptr_t
;
using
nd
curves
::
curve_SE3_ptr_t
;
using
nd
curves
::
matrix3_t
;
using
nd
curves
::
piecewise_SE3_t
;
using
nd
curves
::
piecewise_t
;
using
nd
curves
::
pointX_t
;
using
nd
curves
::
quaternion_t
;
using
nd
curves
::
t_point3_t
;
using
nd
curves
::
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
>
(
nd
curves
::
piecewise_t
ppc_C2
=
nd
curves
::
piecewise_t
::
convert_discrete_points_to_polynomial
<
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
);
nd
curves
::
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
<
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
nd
curves
::
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
));
nd
curves
::
piecewise_t
c1
=
nd
curves
::
piecewise_t
::
convert_discrete_points_to_polynomial
<
nd
curves
::
polynomial_t
>
(
points1
,
time_points1
);
curve_ptr_t
c1_ptr
(
new
nd
curves
::
piecewise_t
(
c1
));
nd
curves
::
piecewise_t
c2
=
nd
curves
::
piecewise_t
::
convert_discrete_points_to_polynomial
<
nd
curves
::
polynomial_t
>
(
points2
,
time_points2
);
curve_ptr_t
c2_ptr
(
new
nd
curves
::
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
nd
curves
::
SE3Curve_t
(
p0
,
p1
,
q0
,
q1
,
0.
,
2.
));
curve_SE3_ptr_t
traj_2
(
new
nd
curves
::
SE3Curve_t
(
p1
,
p2
,
q1
,
q2
,
4.
,
8.
));
cp0
.
addEffectorTrajectory
(
"right_leg"
,
traj_0
);
cp2
.
addEffectorTrajectory
(
"right_leg"
,
traj_2
);
...
...
unittest/serialization_versionning.cpp
View file @
c0d4e0e5
...
...
@@ -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
<nd
curves/fwd.h
>
#include
<
nd
curves/so3_linear.h>
#include
<
nd
curves/se3_curve.h>
#include
<
nd
curves/polynomial.h>
#include
<
nd
curves/bezier_curve.h>
#include
<
nd
curves/piecewise_curve.h>
#include
<
nd
curves/exact_cubic.h>
#include
<
nd
curves/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
()
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment