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
8c81b3e9
Unverified
Commit
8c81b3e9
authored
May 11, 2020
by
Fernbach Pierre
Committed by
GitHub
May 11, 2020
Browse files
Merge pull request #10 from pFernbach/topic/fix_checks
fix contactSequence check methods
parents
026241bc
506ad2f2
Pipeline
#9601
passed with stage
in 14 minutes and 32 seconds
Changes
5
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
include/multicontact-api/bindings/python/scenario/contact-sequence.hpp
View file @
8c81b3e9
...
...
@@ -24,7 +24,7 @@ struct ContactSequencePythonVisitor : public bp::def_visitor<ContactSequencePyth
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
cs_createContact_overloads
,
CS
::
createContact
,
2
,
3
)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
cs_moveEffectorToPlacement_overloads
,
CS
::
moveEffectorToPlacement
,
2
,
4
)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
cs_moveEffectorOf_overloads
,
CS
::
moveEffectorOf
,
2
,
4
)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
cs_haveEffectorTrajectories_overloads
,
CS
::
haveEffectorsTrajectories
,
0
,
1
)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS
(
cs_haveEffectorTrajectories_overloads
,
CS
::
haveEffectorsTrajectories
,
0
,
2
)
template
<
class
PyClass
>
void
visit
(
PyClass
&
cl
)
const
{
...
...
@@ -130,14 +130,16 @@ 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
()),
(
bp
::
args
(
"precision_treshold"
)
=
Eigen
::
NumTraits
<
typename
CS
::
Scalar
>::
dummy_precision
(),
bp
::
args
(
"use_rotation"
)
=
true
),
"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."
))
"it check that the trajectory start at the previous contact placement.
\n
"
"If use_rotation == false, only the translation part of the transforms are compared. "
))
.
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
"
...
...
include/multicontact-api/scenario/contact-sequence.hpp
View file @
8c81b3e9
...
...
@@ -415,6 +415,18 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"CoM acceleration trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_c
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_dc
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM velocity trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_ddc
->
dim
()
!=
3
)
{
std
::
cout
<<
"CoM acceleration trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_c
->
min
()
!=
phase
.
timeInitial
())
{
std
::
cout
<<
"CoM trajectory do not start at t_init for phase : "
<<
i
<<
std
::
endl
;
return
false
;
...
...
@@ -508,6 +520,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"AM velocity trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_L
->
dim
()
!=
3
)
{
std
::
cout
<<
"AM trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_dL
->
dim
()
!=
3
)
{
std
::
cout
<<
"AM derivative trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_L
->
min
()
!=
phase
.
timeInitial
())
{
std
::
cout
<<
"AM trajectory do not start at t_init for phase : "
<<
i
<<
std
::
endl
;
return
false
;
...
...
@@ -582,7 +602,8 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
* placement.
* @return
*/
bool
haveEffectorsTrajectories
(
const
Scalar
prec
=
Eigen
::
NumTraits
<
Scalar
>::
dummy_precision
())
const
{
bool
haveEffectorsTrajectories
(
const
Scalar
prec
=
Eigen
::
NumTraits
<
Scalar
>::
dummy_precision
(),
const
bool
use_rotation
=
true
)
const
{
if
(
!
haveTimings
())
return
false
;
for
(
size_t
i
=
0
;
i
<
m_contact_phases
.
size
()
-
1
;
++
i
)
{
for
(
std
::
string
eeName
:
m_contact_phases
.
at
(
i
).
getContactsCreated
(
m_contact_phases
.
at
(
i
+
1
)))
{
...
...
@@ -601,7 +622,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
return
false
;
}
ContactPatch
::
SE3
pMax
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
max
()).
matrix
());
if
(
!
pMax
.
isApprox
(
m_contact_phases
.
at
(
i
+
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
{
if
((
use_rotation
&&
!
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
<<
" do not end at it's contact placement in the next phase, for phase "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"Last point : "
<<
std
::
endl
...
...
@@ -612,7 +636,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
}
if
(
i
>
0
&&
m_contact_phases
.
at
(
i
-
1
).
isEffectorInContact
(
eeName
))
{
ContactPatch
::
SE3
pMin
=
ContactPatch
::
SE3
((
*
traj
)(
traj
->
min
()).
matrix
());
if
(
!
pMin
.
isApprox
(
m_contact_phases
.
at
(
i
-
1
).
contactPatches
().
at
(
eeName
).
placement
(),
prec
))
{
if
((
use_rotation
&&
!
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
<<
" do not start at it's contact placement in the previous phase, for phase "
<<
i
<<
std
::
endl
;
std
::
cout
<<
"First point : "
<<
std
::
endl
...
...
@@ -847,6 +874,10 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
std
::
cout
<<
"ZMP trajectory not defined for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_zmp
->
dim
()
!=
3
)
{
std
::
cout
<<
"ZMP trajectory is not of dimension 3 for phase : "
<<
i
<<
std
::
endl
;
return
false
;
}
if
(
phase
.
m_zmp
->
min
()
!=
phase
.
timeInitial
())
{
std
::
cout
<<
"ZMP trajectory do not start at t_init for phase : "
<<
i
<<
std
::
endl
;
return
false
;
...
...
unittest/examples.cpp
View file @
8c81b3e9
...
...
@@ -80,6 +80,7 @@ BOOST_AUTO_TEST_CASE(step_in_place_REF) {
BOOST_CHECK
(
cs
.
haveCentroidalValues
());
BOOST_CHECK
(
cs
.
haveCentroidalTrajectories
());
BOOST_CHECK
(
cs
.
haveEffectorsTrajectories
());
BOOST_CHECK
(
cs
.
haveEffectorsTrajectories
(
1e-6
,
false
));
}
BOOST_AUTO_TEST_CASE
(
step_in_place_WB
)
{
...
...
unittest/python/serialization_examples.py
View file @
8c81b3e9
...
...
@@ -149,6 +149,7 @@ class ExamplesSerialization(unittest.TestCase):
self
.
assertTrue
(
cs
.
haveCentroidalValues
())
self
.
assertTrue
(
cs
.
haveCentroidalTrajectories
())
self
.
assertTrue
(
cs
.
haveEffectorsTrajectories
())
self
.
assertTrue
(
cs
.
haveEffectorsTrajectories
(
1e-6
,
False
))
checkCS
(
self
,
cs
,
root
=
True
,
effector
=
True
,
wholeBody
=
False
)
def
test_step_in_place_WB
(
self
):
...
...
unittest/scenario.cpp
View file @
8c81b3e9
...
...
@@ -1974,4 +1974,101 @@ BOOST_AUTO_TEST_CASE(contact_sequence_phase_at_time) {
BOOST_CHECK_THROW
(
cs1
.
phaseAtTime
(
10.
),
std
::
invalid_argument
);
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_dc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
);
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
);
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_L
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
);
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
);
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_zmp
=
buildRandomPolynomial3D
(
0
,
2
);
cs1
.
append
(
cp0
);
BOOST_CHECK
(
cs1
.
haveCOMtrajectories
());
BOOST_CHECK
(
cs1
.
haveAMtrajectories
());
BOOST_CHECK
(
cs1
.
haveZMPtrajectories
());
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_trajectory_wrong_dimension
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
2
);
cp0
.
m_c
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_c_init
=
cp0
.
m_c
->
operator
()(
0
);
cp0
.
m_c_final
=
cp0
.
m_c
->
operator
()(
2
);
cp0
.
m_dc
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_dc_init
=
cp0
.
m_dc
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_dc_final
=
cp0
.
m_dc
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_ddc
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_ddc_init
=
cp0
.
m_ddc
->
operator
()(
0
);
cp0
.
m_ddc_final
=
cp0
.
m_ddc
->
operator
()(
2
);
cp0
.
m_L
=
buildRandomPolynomial12D
(
0
,
2
);
cp0
.
m_L_init
=
cp0
.
m_L
->
operator
()(
0
).
head
<
3
>
();
cp0
.
m_L_final
=
cp0
.
m_L
->
operator
()(
2
).
head
<
3
>
();
cp0
.
m_dL
=
buildRandomPolynomial3D
(
0
,
2
);
cp0
.
m_dL_init
=
cp0
.
m_dL
->
operator
()(
0
);
cp0
.
m_dL_final
=
cp0
.
m_dL
->
operator
()(
2
);
cp0
.
m_zmp
=
buildRandomPolynomial12D
(
0
,
2
);
cs1
.
append
(
cp0
);
BOOST_CHECK
(
!
cs1
.
haveCOMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveAMtrajectories
());
BOOST_CHECK
(
!
cs1
.
haveZMPtrajectories
());
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
ContactPhase
cp2
=
ContactPhase
(
2
,
3
);
cp1
.
addEffectorTrajectory
(
"left_foot"
,
buildRandomSE3LinearTraj
(
1
,
2
));
cp0
.
addContact
(
"left_foot"
,
ContactPatch
(
SE3
(
cp1
.
effectorTrajectories
(
"left_foot"
)
->
operator
()(
1.
).
matrix
())));
cp2
.
addContact
(
"left_foot"
,
ContactPatch
(
SE3
(
cp1
.
effectorTrajectories
(
"left_foot"
)
->
operator
()(
2.
).
matrix
())));
cs1
.
append
(
cp0
);
cs1
.
append
(
cp1
);
cs1
.
append
(
cp2
);
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
());
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-3
));
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-3
,
true
));
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_have_effector_trajectory_no_rotation
)
{
ContactSequence
cs1
=
ContactSequence
(
0
);
ContactPhase
cp0
=
ContactPhase
(
0
,
1
);
ContactPhase
cp1
=
ContactPhase
(
1
,
2
);
ContactPhase
cp2
=
ContactPhase
(
2
,
3
);
cp1
.
addEffectorTrajectory
(
"left_foot"
,
buildRandomSE3LinearTraj
(
1
,
2
));
SE3
p0
(
cp1
.
effectorTrajectories
(
"left_foot"
)
->
operator
()(
1.
).
matrix
());
SE3
p1
(
cp1
.
effectorTrajectories
(
"left_foot"
)
->
operator
()(
2.
).
matrix
());
SE3
p0_r
=
SE3
::
Identity
().
setRandom
();
SE3
p1_r
=
SE3
::
Identity
().
setRandom
();
p0
.
rotation
()
=
p0_r
.
rotation
();
p1
.
rotation
()
=
p1_r
.
rotation
();
cp0
.
addContact
(
"left_foot"
,
ContactPatch
(
p0
));
cp2
.
addContact
(
"left_foot"
,
ContactPatch
(
p1
));
cs1
.
append
(
cp0
);
cs1
.
append
(
cp1
);
cs1
.
append
(
cp2
);
BOOST_CHECK
(
!
cs1
.
haveEffectorsTrajectories
());
BOOST_CHECK
(
!
cs1
.
haveEffectorsTrajectories
(
1e-6
));
BOOST_CHECK
(
!
cs1
.
haveEffectorsTrajectories
(
1e-6
,
true
));
BOOST_CHECK
(
cs1
.
haveEffectorsTrajectories
(
1e-6
,
false
));
}
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