Skip to content
GitLab
Menu
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
3ea4d75a
Commit
3ea4d75a
authored
May 06, 2020
by
Pierre Fernbach
Browse files
[Format] fix format
parent
e30cdfec
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/multicontact-api/bindings/python/scenario/contact-sequence.hpp
View file @
3ea4d75a
...
@@ -262,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
...
@@ -262,7 +262,6 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
static
bp
::
list
getAllEffectorsInContactAsList
(
CS
&
self
)
{
static
bp
::
list
getAllEffectorsInContactAsList
(
CS
&
self
)
{
return
toPythonList
<
std
::
string
>
(
self
.
getAllEffectorsInContact
());
return
toPythonList
<
std
::
string
>
(
self
.
getAllEffectorsInContact
());
}
}
};
};
}
// namespace python
}
// namespace python
}
// namespace multicontact_api
}
// namespace multicontact_api
...
...
include/multicontact-api/bindings/python/serialization/archive.hpp
View file @
3ea4d75a
...
@@ -13,22 +13,20 @@ namespace bp = boost::python;
...
@@ -13,22 +13,20 @@ namespace bp = boost::python;
template
<
typename
Derived
>
template
<
typename
Derived
>
struct
cs_pickle_suite
:
bp
::
pickle_suite
{
struct
cs_pickle_suite
:
bp
::
pickle_suite
{
static
bp
::
object
getstate
(
const
Derived
&
cs
)
{
std
::
ostringstream
os
;
boost
::
archive
::
text_oarchive
oa
(
os
);
oa
<<
cs
;
return
bp
::
str
(
os
.
str
());
}
static
bp
::
object
getstate
(
const
Derived
&
cs
)
{
static
void
setstate
(
Derived
&
cs
,
bp
::
object
entries
)
{
std
::
ostringstream
os
;
bp
::
str
s
=
bp
::
extract
<
bp
::
str
>
(
entries
)();
boost
::
archive
::
text_oarchive
oa
(
os
);
std
::
string
st
=
bp
::
extract
<
std
::
string
>
(
s
)();
oa
<<
cs
;
std
::
istringstream
is
(
st
);
return
bp
::
str
(
os
.
str
());
boost
::
archive
::
text_iarchive
ia
(
is
);
}
ia
>>
cs
;
}
static
void
setstate
(
Derived
&
cs
,
bp
::
object
entries
)
{
bp
::
str
s
=
bp
::
extract
<
bp
::
str
>
(
entries
)();
std
::
string
st
=
bp
::
extract
<
std
::
string
>
(
s
)();
std
::
istringstream
is
(
st
);
boost
::
archive
::
text_iarchive
ia
(
is
);
ia
>>
cs
;
}
};
};
template
<
typename
Derived
>
template
<
typename
Derived
>
...
...
include/multicontact-api/scenario/contact-sequence.hpp
View file @
3ea4d75a
...
@@ -415,16 +415,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
...
@@ -415,16 +415,16 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"CoM acceleration trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"CoM acceleration trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_c
->
dim
()
!=
3
){
if
(
phase
.
m_c
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"CoM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_dc
->
dim
()
!=
3
){
if
(
phase
.
m_dc
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM velocity trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"CoM velocity trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_ddc
->
dim
()
!=
3
){
if
(
phase
.
m_ddc
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM acceleration trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"CoM acceleration trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_c
->
min
()
!=
phase
.
timeInitial
())
{
if
(
phase
.
m_c
->
min
()
!=
phase
.
timeInitial
())
{
...
@@ -520,12 +520,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
...
@@ -520,12 +520,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"AM velocity trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"AM velocity trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_L
->
dim
()
!=
3
){
if
(
phase
.
m_L
->
dim
()
!=
3
)
{
std
::
cout
<<
"AM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"AM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_dL
->
dim
()
!=
3
){
if
(
phase
.
m_dL
->
dim
()
!=
3
)
{
std
::
cout
<<
"AM derivative trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"AM derivative trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_L
->
min
()
!=
phase
.
timeInitial
())
{
if
(
phase
.
m_L
->
min
()
!=
phase
.
timeInitial
())
{
...
@@ -622,8 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
...
@@ -622,8 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return
false
;
return
false
;
}
}
ContactPatch
::
SE3
pMax
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
max
()).
matrix
());
ContactPatch
::
SE3
pMax
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
max
()).
matrix
());
if
((
use_rotation
&&
!
pMax
.
isApprox
(
m_contact_phases
.
at
(
i
+
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
if
((
use_rotation
&&
||
(
!
pMax
.
translation
().
isApprox
(
m_contact_phases
.
at
(
i
+
1
).
contactPatches
().
at
(
eeName
).
placement
().
translation
(),
prec
)))
{
!
pMax
.
isApprox
(
m_contact_phases
.
at
(
i
+
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
||
(
!
pMax
.
translation
().
isApprox
(
m_contact_phases
.
at
(
i
+
1
).
contactPatches
().
at
(
eeName
).
placement
().
translation
(),
prec
)))
{
std
::
cout
<<
"Effector trajectory for "
<<
eeName
std
::
cout
<<
"Effector trajectory for "
<<
eeName
<<
" do not end at it's contact placement in the next phase, for phase "
<<
i
<<
std
::
endl
;
<<
" do not end at it's contact placement in the next phase, for phase "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"Last point : "
<<
std
::
endl
std
::
cout
<<
"Last point : "
<<
std
::
endl
...
@@ -634,9 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
...
@@ -634,9 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
}
}
if
(
i
>
0
&&
m_contact_phases
.
at
(
i
-
1
).
isEffectorInContact
(
eeName
))
{
if
(
i
>
0
&&
m_contact_phases
.
at
(
i
-
1
).
isEffectorInContact
(
eeName
))
{
ContactPatch
::
SE3
pMin
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
min
()).
matrix
());
ContactPatch
::
SE3
pMin
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
min
()).
matrix
());
if
((
use_rotation
&&
!
pMin
.
isApprox
(
m_contact_phases
.
at
(
i
-
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
if
((
use_rotation
&&
||
(
!
pMin
.
translation
().
isApprox
(
m_contact_phases
.
at
(
i
-
1
).
contactPatches
().
at
(
eeName
).
placement
().
translation
(),
prec
)))
!
pMin
.
isApprox
(
m_contact_phases
.
at
(
i
-
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
||
{
(
!
pMin
.
translation
().
isApprox
(
m_contact_phases
.
at
(
i
-
1
).
contactPatches
().
at
(
eeName
).
placement
().
translation
(),
prec
)))
{
std
::
cout
<<
"Effector trajectory for "
<<
eeName
std
::
cout
<<
"Effector trajectory for "
<<
eeName
<<
" do not start at it's contact placement in the previous phase, for phase "
<<
i
<<
std
::
endl
;
<<
" do not start at it's contact placement in the previous phase, for phase "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"First point : "
<<
std
::
endl
std
::
cout
<<
"First point : "
<<
std
::
endl
...
@@ -853,8 +856,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
...
@@ -853,8 +856,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"ZMP trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"ZMP trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_zmp
->
dim
()
!=
3
){
if
(
phase
.
m_zmp
->
dim
()
!=
3
)
{
std
::
cout
<<
"ZMP trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"ZMP trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
return
false
;
}
}
if
(
phase
.
m_zmp
->
min
()
!=
phase
.
timeInitial
())
{
if
(
phase
.
m_zmp
->
min
()
!=
phase
.
timeInitial
())
{
...
...
unittest/scenario.cpp
View file @
3ea4d75a
...
@@ -34,7 +34,6 @@ using curves::t_pointX_t;
...
@@ -34,7 +34,6 @@ using curves::t_pointX_t;
using
namespace
multicontact_api
::
scenario
;
using
namespace
multicontact_api
::
scenario
;
typedef
ContactSequence
::
ContactPhaseVector
ContactPhaseVector
;
typedef
ContactSequence
::
ContactPhaseVector
ContactPhaseVector
;
typedef
pinocchio
::
SE3Tpl
<
double
>
SE3
;
typedef
pinocchio
::
SE3Tpl
<
double
>
SE3
;
curve_ptr_t
buildPiecewisePolynomialC2
()
{
curve_ptr_t
buildPiecewisePolynomialC2
()
{
...
@@ -1815,25 +1814,25 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
...
@@ -1815,25 +1814,25 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
BOOST_CHECK_THROW
(
cs1
.
phaseAtTime
(
10.
),
std
::
invalid_argument
);
BOOST_CHECK_THROW
(
cs1
.
phaseAtTime
(
10.
),
std
::
invalid_argument
);
}
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory
){
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_dc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
);
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
);
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
);
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
);
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_L
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_L
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
);
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
);
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
);
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
);
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_zmp
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_zmp
=
buildRandomPolynomial3D
(
0
,
2
);
cs1
.
append
(
cp0
);
cs1
.
append
(
cp0
);
BOOST_CHECK
(
cs1
.
haveCOMtrajectories
());
BOOST_CHECK
(
cs1
.
haveCOMtrajectories
());
...
@@ -1841,33 +1840,33 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory){
...
@@ -1841,33 +1840,33 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_trajectory){
BOOST_CHECK
(
cs1
.
haveZMPtrajectories
());
BOOST_CHECK
(
cs1
.
haveZMPtrajectories
());
}
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory_wrong_dimension
){
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory_wrong_dimension
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_dc
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_dc
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_L
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_L
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_zmp
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_zmp
=
buildRandomPolynomial12D
(
0
,
2
);
cs1
.
append
(
cp0
);
cs1
.
append
(
cp0
);
BOOST_CHECK
(
!
cs1
.
haveCOMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveCOMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveAMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveAMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveZMPtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveZMPtrajectories
());
}
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory
){
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
...
@@ -1886,7 +1885,7 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory){
...
@@ -1886,7 +1885,7 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory){
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-3
,
true
));
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-3
,
true
));
}
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory_no_rotation
){
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory_no_rotation
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
...
@@ -1912,5 +1911,4 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation){
...
@@ -1912,5 +1911,4 @@ BOOST_AUTO_TEST_CASE(contact_sequence_have_effector_trajectory_no_rotation){
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-6
,
false
));
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-6
,
false
));
}
}
BOOST_AUTO_TEST_SUITE_END
()
BOOST_AUTO_TEST_SUITE_END
()
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a 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