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
41868547
Commit
41868547
authored
Feb 27, 2020
by
Guilhem Saurel
Browse files
format
parent
326738d2
Changes
6
Expand all
Hide whitespace changes
Inline
Side-by-side
include/multicontact-api/bindings/python/scenario/contact-phase.hpp
View file @
41868547
...
...
@@ -36,7 +36,6 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
typedef
curves
::
pointX_list_t
pointX_list_t
;
typedef
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
)
...
...
@@ -179,14 +178,15 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
.
def
(
"setCOMtrajectoryFromPoints"
,
&
setCOMtrajectoryFromPoints
,
bp
::
args
(
"COM_positions"
,
"COM_velocities"
,
"COM_accelerations"
,
"times"
),
"set the c,dc and ddc curves from a list of discrete"
"COM positions, velocity and accelerations.
\n
"
"The trajectories are build with first order polynomials connecting each discrete points given.
\n
"
"This method also set the initial/final values for c, dc and ddc from the first and last discrete point
given."
)
.
def
(
"setAMtrajectoryFromPoints"
,
&
setAMtrajectoryFromPoints
,
bp
::
args
(
"AM_values"
,
"AM_derivatives"
,
"times"
),
"COM positions, velocity and accelerations.
\n
"
"The trajectories are build with first order polynomials connecting each discrete points given.
\n
"
"This method also set the initial/final values for c, dc and ddc from the first and last discrete point
"
"given."
)
.
def
(
"setAMtrajectoryFromPoints"
,
&
setAMtrajectoryFromPoints
,
bp
::
args
(
"AM_values"
,
"AM_derivatives"
,
"times"
),
"set the L and d_L curves from a list of discrete Angular velocity values and their derivatives.
\n
"
"The trajectories are build with first order polynomials connecting each discrete points given.
\n
"
"This method also set the initial/final values for L, and dL from the first and last discrete point given."
)
"This method also set the initial/final values for L, and dL from the first and last discrete point "
"given."
)
.
def
(
"setJointsTrajectoryFromPoints"
,
&
setJointsTrajectoryFromPoints
,
bp
::
args
(
"Joints_poisitions"
,
"Joints_velocities"
,
"Joints_accelerations"
,
"times"
),
"set the q,dq and ddq curves from a list of discrete joints positions, velocity and accelerations."
...
...
@@ -263,7 +263,6 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
return
toPythonList
<
std
::
string
>
(
self
.
getContactsVariations
(
to
));
}
// Converts a C++ map to a python dict
// Note : lot of overhead, should not be used for large map and/or operations called frequently.
// prefer the direct bindings with std_map_* for this cases.
...
...
@@ -288,42 +287,43 @@ struct ContactPhasePythonVisitor : public bp::def_visitor<ContactPhasePythonVisi
return
toPythonDict
<
curve_SE3_ptr
>
(
self
.
effectorTrajectories
());
}
static
void
setCOMtrajectoryFromPoints
(
ContactPhase
&
self
,
const
pointX_list_t
&
points
,
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_second_derivative_list
=
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points_second_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
);
self
.
setCOMtrajectoryFromPoints
(
points_list
,
points_derivative_list
,
points_second_derivative_list
,
time_points_list
);
return
;
self
.
setCOMtrajectoryFromPoints
(
points_list
,
points_derivative_list
,
points_second_derivative_list
,
time_points_list
);
return
;
}
static
void
setJointsTrajectoryFromPoints
(
ContactPhase
&
self
,
const
pointX_list_t
&
points
,
const
pointX_list_t
&
points_derivative
,
const
pointX_list_t
&
points_second_derivative
,
const
time_waypoints_t
&
time_points
)
{
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_second_derivative_list
=
curves
::
vectorFromEigenArray
<
pointX_list_t
,
t_pointX_t
>
(
points_second_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
);
self
.
setJointsTrajectoryFromPoints
(
points_list
,
points_derivative_list
,
points_second_derivative_list
,
time_points_list
);
return
;
self
.
setJointsTrajectoryFromPoints
(
points_list
,
points_derivative_list
,
points_second_derivative_list
,
time_points_list
);
return
;
}
static
void
setAMtrajectoryFromPoints
(
ContactPhase
&
self
,
const
pointX_list_t
&
points
,
const
pointX_list_t
&
points_derivative
,
const
time_waypoints_t
&
time_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
);
self
.
setAMtrajectoryFromPoints
(
points_list
,
points_derivative_list
,
time_points_list
);
return
;
return
;
}
static
ContactPhase
copy
(
const
ContactPhase
&
self
)
{
return
ContactPhase
(
self
);
}
};
}
// namespace python
...
...
include/multicontact-api/bindings/python/scenario/contact-sequence.hpp
View file @
41868547
...
...
@@ -130,13 +130,14 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"and that the trajectories start and end and the correct values defined in each phase."
)
.
def
(
"haveEffectorsTrajectories"
,
&
CS
::
haveEffectorsTrajectories
,
cs_haveEffectorTrajectories_overloads
(
(
bp
::
args
(
"precision_treshold"
)
=
Eigen
::
NumTraits
<
typename
CS
::
Scalar
>::
dummy_precision
()),
"check that for each phase preceeding a contact creation,"
"an SE3 trajectory is defined for the effector that will be in contact.
\n
"
"Also check that this trajectory is defined on the time-interval of the phase.
\n
"
"Also check that the trajectory correctly end at the placement defined for the contact in the next phase.
\n
"
"If this effector was in contact in the previous phase,"
"it check that the trajectory start at the previous contact placement."
))
(
bp
::
args
(
"precision_treshold"
)
=
Eigen
::
NumTraits
<
typename
CS
::
Scalar
>::
dummy_precision
()),
"check that for each phase preceeding a contact creation,"
"an SE3 trajectory is defined for the effector that will be in contact.
\n
"
"Also check that this trajectory is defined on the time-interval of the phase.
\n
"
"Also check that the trajectory correctly end at the placement defined for the contact in the next "
"phase.
\n
"
"If this effector was in contact in the previous phase,"
"it check that the trajectory start at the previous contact placement."
))
.
def
(
"haveJointsTrajectories"
,
&
CS
::
haveJointsTrajectories
,
"Check that a q trajectory is defined for each phases.
\n
"
"Also check that the time interval of this trajectories matches the one of the phase.
\n
"
...
...
@@ -161,7 +162,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
"a friction coefficient initialized."
)
.
def
(
"haveZMPtrajectories"
,
&
CS
::
haveZMPtrajectories
,
"check that all the contact phases have a ZMP trajectory."
)
.
def
(
"getAllEffectorsInContact"
,
&
getAllEffectorsInContactAsList
,
.
def
(
"getAllEffectorsInContact"
,
&
getAllEffectorsInContactAsList
,
"return a list of names of all the effectors used to create contacts during the sequence"
)
.
def
(
"concatenateCtrajectories"
,
&
CS
::
concatenateCtrajectories
,
"Return a piecewise curve wchich is the concatenation of the m_c curves"
...
...
@@ -199,35 +200,30 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
.
def
(
"concatenateRootTrajectories"
,
&
CS
::
concatenateRootTrajectories
,
"Return a piecewise curve wchich is the concatenation of the m_root curves"
" for each contact phases in the sequence."
)
.
def
(
"concatenateEffectorTrajectories"
,
&
CS
::
concatenateEffectorTrajectories
,
bp
::
arg
(
"eeName"
),
.
def
(
"concatenateEffectorTrajectories"
,
&
CS
::
concatenateEffectorTrajectories
,
bp
::
arg
(
"eeName"
),
"Return a piecewise curve which is the concatenation"
"of the effectors trajectories curves for the given effector"
"for each contact phases in the sequence.
\n
"
"During the phases where no effector trajectories are defined,"
"the trajectory is constant with the value of"
"the last phase where it was defined."
)
.
def
(
"concatenateContactForceTrajectories"
,
&
CS
::
concatenateContactForceTrajectories
,
bp
::
arg
(
"eeName"
),
.
def
(
"concatenateContactForceTrajectories"
,
&
CS
::
concatenateContactForceTrajectories
,
bp
::
arg
(
"eeName"
),
"Return a piecewise curve which"
"is the concatenation of the contact forces for the given effector"
"for each contact phases in the sequence.
\n
"
"During the phases where no contact forces are defined,"
"the trajectory is constant with the value of 0."
)
.
def
(
"concatenateNormalForceTrajectories"
,
&
CS
::
concatenateNormalForceTrajectories
,
bp
::
arg
(
"eeName"
),
.
def
(
"concatenateNormalForceTrajectories"
,
&
CS
::
concatenateNormalForceTrajectories
,
bp
::
arg
(
"eeName"
),
"Return a piecewise curve which"
"is the concatenation of the contact normal forces for the given effector"
"for each contact phases in the sequence.
\n
"
"During the phases where no contact normal forces are defined,"
"the trajectory is constant with the value of 0."
)
.
def
(
"phaseIdAtTime"
,
&
CS
::
phaseIdAtTime
,
bp
::
arg
(
"time"
),
.
def
(
"phaseIdAtTime"
,
&
CS
::
phaseIdAtTime
,
bp
::
arg
(
"time"
),
"return the index of a phase in the sequence such that "
"phase.timeInitial <= t < phase.timeFinal
\n
"
"if t equal to the last phase timeFinal, this index is returned."
)
.
def
(
"phaseAtTime"
,
&
CS
::
phaseAtTime
,
bp
::
arg
(
"time"
),
bp
::
return_internal_reference
<>
(),
.
def
(
"phaseAtTime"
,
&
CS
::
phaseAtTime
,
bp
::
arg
(
"time"
),
bp
::
return_internal_reference
<>
(),
"return a phase of the sequence such that "
"phase.timeInitial <= t < phase.timeFinal
\n
"
"if t equal to the last phase timeFinal, this index is returned."
)
...
...
include/multicontact-api/scenario/contact-phase.hpp
View file @
41868547
...
...
@@ -462,13 +462,13 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
m_dc = curve_ptr(c_t.compute_derivate_ptr(1));
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
)));
m_dc
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
curves
::
polynomial_t
>
(
points_derivative
,
time_points
)));
m_c
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
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
>
(
points_second_derivative
,
time_points
)));
if
(
m_c
->
dim
()
!=
3
||
m_dc
->
dim
()
!=
3
||
m_ddc
->
dim
()
!=
3
)
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."
);
m_c_init
=
point3_t
(
points
.
front
());
...
...
@@ -498,12 +498,11 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
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
)));
m_dL
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
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
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
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
)));
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
());
m_L_final
=
point3_t
(
points
.
back
());
...
...
@@ -531,12 +530,12 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
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
)));
m_dq
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
curves
::
polynomial_t
>
(
points_derivative
,
time_points
)));
m_q
=
curve_ptr
(
new
piecewise_t
(
piecewise_t
::
convert_discrete_points_to_polynomial
<
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
>
(
points_second_derivative
,
time_points
)));
points_second_derivative
,
time_points
)));
m_q_init
=
points
.
front
();
m_q_final
=
points
.
back
();
return
;
...
...
@@ -549,9 +548,8 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
*/
t_strings
getContactsBroken
(
const
ContactPhase
&
to
)
const
{
t_strings
res
;
for
(
std
::
string
eeName
:
m_effector_in_contact
){
if
(
!
to
.
isEffectorInContact
(
eeName
))
res
.
push_back
(
eeName
);
for
(
std
::
string
eeName
:
m_effector_in_contact
)
{
if
(
!
to
.
isEffectorInContact
(
eeName
))
res
.
push_back
(
eeName
);
}
return
res
;
}
...
...
@@ -563,9 +561,8 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
*/
t_strings
getContactsCreated
(
const
ContactPhase
&
to
)
const
{
t_strings
res
;
for
(
std
::
string
eeName
:
to
.
effectorsInContact
()){
if
(
!
isEffectorInContact
(
eeName
))
res
.
push_back
(
eeName
);
for
(
std
::
string
eeName
:
to
.
effectorsInContact
())
{
if
(
!
isEffectorInContact
(
eeName
))
res
.
push_back
(
eeName
);
}
return
res
;
}
...
...
@@ -579,10 +576,9 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
t_strings
getContactsRepositioned
(
const
ContactPhase
&
to
)
const
{
t_strings
res
;
const
ContactPatchMap
&
to_patch_map
=
to
.
contactPatches
();
for
(
std
::
string
eeName
:
m_effector_in_contact
){
if
(
to
.
isEffectorInContact
(
eeName
))
if
(
m_contact_patches
.
at
(
eeName
).
placement
()
!=
to_patch_map
.
at
(
eeName
).
placement
())
res
.
push_back
(
eeName
);
for
(
std
::
string
eeName
:
m_effector_in_contact
)
{
if
(
to
.
isEffectorInContact
(
eeName
))
if
(
m_contact_patches
.
at
(
eeName
).
placement
()
!=
to_patch_map
.
at
(
eeName
).
placement
())
res
.
push_back
(
eeName
);
}
return
res
;
}
...
...
@@ -593,20 +589,19 @@ struct ContactPhaseTpl : public serialization::Serializable<ContactPhaseTpl<_Sca
* @return a list of string containing the effectors names
*/
t_strings
getContactsVariations
(
const
ContactPhase
&
to
)
const
{
std
::
set
<
std
::
string
>
set_res
;
// use intermediate set to guarantee uniqueness of element
for
(
std
::
string
eeName
:
getContactsBroken
(
to
)){
std
::
set
<
std
::
string
>
set_res
;
// use intermediate set to guarantee uniqueness of element
for
(
std
::
string
eeName
:
getContactsBroken
(
to
))
{
set_res
.
insert
(
eeName
);
}
for
(
std
::
string
eeName
:
getContactsCreated
(
to
)){
for
(
std
::
string
eeName
:
getContactsCreated
(
to
))
{
set_res
.
insert
(
eeName
);
}
for
(
std
::
string
eeName
:
getContactsRepositioned
(
to
)){
for
(
std
::
string
eeName
:
getContactsRepositioned
(
to
))
{
set_res
.
insert
(
eeName
);
}
return
t_strings
(
set_res
.
begin
(),
set_res
.
end
());
}
/* End Helpers */
void
disp
(
std
::
ostream
&
os
)
const
{
...
...
include/multicontact-api/scenario/contact-sequence.hpp
View file @
41868547
This diff is collapsed.
Click to expand it.
unittest/examples.cpp
View file @
41868547
...
...
@@ -19,11 +19,11 @@
#include
<curves/cubic_hermite_spline.h>
/**
* This unit test try to deserialize the ContactSequences in the examples folder
* and check if they have the given data set.
* If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with
serialized objects
* The objects need to be re-generated.
*/
* This unit test try to deserialize the ContactSequences in the examples folder
* and check if they have the given data set.
* If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with
*
serialized objects
The objects need to be re-generated.
*/
using
namespace
multicontact_api
::
scenario
;
...
...
@@ -32,7 +32,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE
(
com_motion_above_feet_COM
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"com_motion_above_feet_COM.cs"
);
cs
.
loadFromBinary
(
path
+
"com_motion_above_feet_COM.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
1
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -40,10 +40,9 @@ BOOST_AUTO_TEST_CASE(com_motion_above_feet_COM) {
BOOST_CHECK
(
cs
.
haveCentroidalTrajectories
());
}
BOOST_AUTO_TEST_CASE
(
com_motion_above_feet_WB
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"com_motion_above_feet_WB.cs"
);
cs
.
loadFromBinary
(
path
+
"com_motion_above_feet_WB.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
1
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -55,17 +54,16 @@ BOOST_AUTO_TEST_CASE(com_motion_above_feet_WB) {
BOOST_CHECK
(
cs
.
haveZMPtrajectories
());
}
BOOST_AUTO_TEST_CASE
(
step_in_place
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
}
BOOST_AUTO_TEST_CASE
(
step_in_place_COM
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_COM.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_COM.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -75,7 +73,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_COM) {
BOOST_AUTO_TEST_CASE
(
step_in_place_REF
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_REF.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_REF.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -86,7 +84,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_AUTO_TEST_CASE
(
step_in_place_WB
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_WB.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_WB.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -101,14 +99,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_WB) {
BOOST_AUTO_TEST_CASE
(
step_in_place_quasistatic
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
}
BOOST_AUTO_TEST_CASE
(
step_in_place_quasistatic_COM
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_COM.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_COM.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -118,7 +116,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
BOOST_AUTO_TEST_CASE
(
step_in_place_quasistatic_REF
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_REF.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_REF.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -129,7 +127,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
BOOST_AUTO_TEST_CASE
(
step_in_place_quasistatic_WB
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_WB.cs"
);
cs
.
loadFromBinary
(
path
+
"step_in_place_quasistatic_WB.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
9
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -144,14 +142,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
BOOST_AUTO_TEST_CASE
(
walk_20cm
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
}
BOOST_AUTO_TEST_CASE
(
walk_20cm_COM
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_COM.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_COM.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -161,7 +159,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
BOOST_AUTO_TEST_CASE
(
walk_20cm_REF
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_REF.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_REF.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -172,7 +170,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
BOOST_AUTO_TEST_CASE
(
walk_20cm_WB
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_WB.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_WB.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -187,14 +185,14 @@ BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
BOOST_AUTO_TEST_CASE
(
walk_20cm_quasistatic
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
}
BOOST_AUTO_TEST_CASE
(
walk_20cm_quasistatic_COM
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_COM.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_COM.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -204,7 +202,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
BOOST_AUTO_TEST_CASE
(
walk_20cm_quasistatic_REF
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_REF.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_REF.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -215,7 +213,7 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
BOOST_AUTO_TEST_CASE
(
walk_20cm_quasistatic_WB
)
{
ContactSequence
cs
;
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_WB.cs"
);
cs
.
loadFromBinary
(
path
+
"walk_20cm_quasistatic_WB.cs"
);
BOOST_CHECK_EQUAL
(
cs
.
size
(),
23
);
BOOST_CHECK
(
cs
.
haveConsistentContacts
());
BOOST_CHECK
(
cs
.
haveTimings
());
...
...
@@ -228,6 +226,4 @@ BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
BOOST_CHECK
(
cs
.
haveZMPtrajectories
());
}
BOOST_AUTO_TEST_SUITE_END
()
unittest/scenario.cpp
View file @
41868547
...
...
@@ -27,9 +27,9 @@ using curves::point6_t;
typedef
Eigen
::
Matrix
<
double
,
12
,
1
>
point12_t
;
using
curves
::
curve_ptr_t
;
using
curves
::
curve_SE3_ptr_t
;
using
curves
::
piecewise_t
;
using
curves
::
piecewise_SE3_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
;
...
...
@@ -103,7 +103,7 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) {
pointX_t
b
=
Point
::
Random
();
pointX_t
c
=
Point
::
Random
();
pointX_t
d
=
Point
::
Random
();
if
(
min
<
0
&&
max
<
0
){
if
(
min
<
0
&&
max
<
0
)
{
const
double
t1
=
Eigen
::
internal
::
random
<
double
>
(
0.
,
10.
);
const
double
t2
=
Eigen
::
internal
::
random
<
double
>
(
0.
,
10.
);
min
=
std
::
min
(
t1
,
t2
);
...
...
@@ -118,14 +118,17 @@ curve_ptr_t buildRandomPolynomial(double min = -1, double max = -1) {
return
res
;
}
curve_ptr_t
buildRandomPolynomial3D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point3_t
>
(
min
,
max
);
}
curve_ptr_t
buildRandomPolynomial3D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point3_t
>
(
min
,
max
);
}
curve_ptr_t
buildRandomPolynomial12D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point12_t
>
(
min
,
max
);
}
curve_ptr_t
buildRandomPolynomial12D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point12_t
>
(
min
,
max
);
}
curve_ptr_t
buildRandomPolynomial1D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point1_t
>
(
min
,
max
);
}
curve_ptr_t
buildRandomPolynomial1D
(
double
min
=
-
1
,
double
max
=
-
1
)
{
return
buildRandomPolynomial
<
point1_t
>
(
min
,
max
);
}
curve_SE3_ptr_t
buildPiecewiseSE3
()
{
const
double
min
=
0.5
;
...
...
@@ -1123,27 +1126,27 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {
// check that init/final are not modified if they ere already set :
// NOT THE CORRECT BEHAVIOR ANYMORE
// ContactPhase cp2 = buildRandomContactPhase(0, 2);
// point3_t c_init, c_final, dc_init, dc_final, ddc_init, ddc_final;
// c_init = cp2.m_c_init;
// dc_init = cp2.m_dc_init;
// ddc_init = cp2.m_ddc_init;
// c_final = cp2.m_c_final;
// dc_final = cp2.m_dc_final;
// ddc_final = cp2.m_ddc_final;
// cp2.setCOMtrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points);
// BOOST_CHECK(cp2.m_c_init.isApprox(c_init));
// BOOST_CHECK(cp2.m_c_final.isApprox(c_final));
// BOOST_CHECK(cp2.m_dc_init.isApprox(dc_init));
// BOOST_CHECK(cp2.m_dc_final.isApprox(dc_final));
// BOOST_CHECK(cp2.m_ddc_init.isApprox(ddc_init));
// BOOST_CHECK(cp2.m_ddc_final.isApprox(ddc_final));
// BOOST_CHECK(!cp2.m_c_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_c_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dc_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dc_final.isApprox(d3));
// BOOST_CHECK(!cp2.m_ddc_init.isApprox(dd0));
// BOOST_CHECK(!cp2.m_ddc_final.isApprox(dd3));
// ContactPhase cp2 = buildRandomContactPhase(0, 2);
// point3_t c_init, c_final, dc_init, dc_final, ddc_init, ddc_final;
// c_init = cp2.m_c_init;
// dc_init = cp2.m_dc_init;
// ddc_init = cp2.m_ddc_init;
// c_final = cp2.m_c_final;
// dc_final = cp2.m_dc_final;
// ddc_final = cp2.m_ddc_final;
// cp2.setCOMtrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points);
// BOOST_CHECK(cp2.m_c_init.isApprox(c_init));
// BOOST_CHECK(cp2.m_c_final.isApprox(c_final));
// BOOST_CHECK(cp2.m_dc_init.isApprox(dc_init));
// BOOST_CHECK(cp2.m_dc_final.isApprox(dc_final));
// BOOST_CHECK(cp2.m_ddc_init.isApprox(ddc_init));
// BOOST_CHECK(cp2.m_ddc_final.isApprox(ddc_final));
// BOOST_CHECK(!cp2.m_c_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_c_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dc_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dc_final.isApprox(d3));
// BOOST_CHECK(!cp2.m_ddc_init.isApprox(dd0));
// BOOST_CHECK(!cp2.m_ddc_final.isApprox(dd3));
// check AM trajectory
...
...
@@ -1169,21 +1172,21 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {
BOOST_CHECK
(
cp
.
m_dL_final
.
isApprox
(
d3
));
// check that init/final are not modified if they ere already set :
//NOT THE CORRECT BEHAVIOR ANYMORE
// point3_t L_init, L_final, dL_init, dL_final;
// L_init = cp2.m_L_init;
// dL_init = cp2.m_dL_init;
// L_final = cp2.m_L_final;
// dL_final = cp2.m_dL_final;
// cp2.setAMtrajectoryFromPoints(points, points_derivative, time_points);
// BOOST_CHECK(cp2.m_L_init.isApprox(L_init));
// BOOST_CHECK(cp2.m_L_final.isApprox(L_final));
// BOOST_CHECK(cp2.m_dL_init.isApprox(dL_init));
// BOOST_CHECK(cp2.m_dL_final.isApprox(dL_final));
// BOOST_CHECK(!cp2.m_L_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_L_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dL_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dL_final.isApprox(d3));
//
NOT THE CORRECT BEHAVIOR ANYMORE
// point3_t L_init, L_final, dL_init, dL_final;
// L_init = cp2.m_L_init;
// dL_init = cp2.m_dL_init;
// L_final = cp2.m_L_final;
// dL_final = cp2.m_dL_final;
// cp2.setAMtrajectoryFromPoints(points, points_derivative, time_points);
// BOOST_CHECK(cp2.m_L_init.isApprox(L_init));
// BOOST_CHECK(cp2.m_L_final.isApprox(L_final));
// BOOST_CHECK(cp2.m_dL_init.isApprox(dL_init));
// BOOST_CHECK(cp2.m_dL_final.isApprox(dL_final));
// BOOST_CHECK(!cp2.m_L_init.isApprox(p0));
// BOOST_CHECK(!cp2.m_L_final.isApprox(p3));
// BOOST_CHECK(!cp2.m_dL_init.isApprox(d0));
// BOOST_CHECK(!cp2.m_dL_final.isApprox(d3));
// check q trajectory :
pointX_t
q0
=
point12_t
::
Random
();
...
...
@@ -1242,14 +1245,14 @@ BOOST_AUTO_TEST_CASE(contact_phase_helpers_tarjectories) {