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
026241bc
Unverified
Commit
026241bc
authored
May 11, 2020
by
Fernbach Pierre
Committed by
GitHub
May 11, 2020
Browse files
Merge pull request #9 from pFernbach/topic/fix_concatenate_effector
fix concatenate effector
parents
f7f0dda2
0e4a58ea
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/multicontact-api/scenario/contact-sequence.hpp
View file @
026241bc
...
...
@@ -1043,7 +1043,7 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
*/
piecewise_SE3_t
concatenateEffectorTrajectories
(
const
std
::
string
&
eeName
)
const
{
piecewise_SE3_t
res
=
piecewise_SE3_t
();
transform_t
last_placement
;
transform_t
last_placement
,
first_placement
;
// first find the first and last phase with a trajectory for this effector
size_t
first_phase
=
m_contact_phases
.
size
();
size_t
last_phase
=
0
;
...
...
@@ -1052,9 +1052,20 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
last_phase
=
i
;
if
(
first_phase
>
i
)
{
first_phase
=
i
;
curve_SE3_ptr
curve
=
m_contact_phases
.
at
(
i
).
effectorTrajectories
().
at
(
eeName
);
first_placement
=
curve
->
operator
()(
curve
->
min
());
}
}
}
if
(
first_phase
==
m_contact_phases
.
size
())
throw
std
::
invalid_argument
(
"The contact sequence doesn't have any phase with an effector trajectory"
" for the given effector name"
);
if
(
first_phase
>
0
){
// add a first constant phase at the initial placement
curve_SE3_ptr
ptr_init
(
new
SE3Curve_t
(
first_placement
,
first_placement
,
m_contact_phases
.
at
(
0
).
timeInitial
(),
m_contact_phases
.
at
(
first_phase
).
timeInitial
()));
res
.
add_curve_ptr
(
ptr_init
);
}
// loop over this phases to concatenate the trajectories
for
(
size_t
i
=
first_phase
;
i
<=
last_phase
;
++
i
)
{
if
(
m_contact_phases
.
at
(
i
).
effectorHaveAtrajectory
(
eeName
))
{
...
...
@@ -1067,6 +1078,12 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
res
.
add_curve_ptr
(
ptr
);
}
}
if
(
last_phase
<
m_contact_phases
.
size
()
-
1
){
// add a last constant phase until the end of the contact sequence
curve_SE3_ptr
ptr_final
(
new
SE3Curve_t
(
last_placement
,
last_placement
,
m_contact_phases
.
at
(
last_phase
).
timeFinal
(),
m_contact_phases
.
back
().
timeFinal
()));
res
.
add_curve_ptr
(
ptr_final
);
}
return
res
;
}
...
...
unittest/scenario.cpp
View file @
026241bc
...
...
@@ -1859,6 +1859,29 @@ BOOST_AUTO_TEST_CASE(contact_sequence_concatenate_effector_traj) {
BOOST_CHECK
(
traj
(
8.
).
isApprox
(
traj_2
->
operator
()(
8.
)));
BOOST_CHECK
(
traj
(
2.5
).
isApprox
(
traj_0
->
operator
()(
2.
)));
BOOST_CHECK
(
traj
(
3.8
).
isApprox
(
traj_0
->
operator
()(
2.
)));
BOOST_CHECK_THROW
(
cs1
.
concatenateEffectorTrajectories
(
"test"
),
std
::
invalid_argument
);
ContactPhase
cp3
=
ContactPhase
(
8
,
12.
);
cs1
.
append
(
cp3
);
traj
=
cs1
.
concatenateEffectorTrajectories
(
"right_leg"
);
BOOST_CHECK
(
traj
.
min
()
==
0.
);
BOOST_CHECK
(
traj
.
max
()
==
12.
);
ContactPhase
cpm1
=
ContactPhase
(
-
2.
,
0.
);
ContactSequence
cs2
=
ContactSequence
(
0
);
cs2
.
append
(
cpm1
);
cs2
.
append
(
cp0
);
cs2
.
append
(
cp1
);
cs2
.
append
(
cp2
);
traj
=
cs2
.
concatenateEffectorTrajectories
(
"right_leg"
);
BOOST_CHECK
(
traj
.
min
()
==
-
2.
);
BOOST_CHECK
(
traj
.
max
()
==
8.
);
cs2
.
append
(
cp3
);
traj
=
cs2
.
concatenateEffectorTrajectories
(
"right_leg"
);
BOOST_CHECK
(
traj
.
min
()
==
-
2.
);
BOOST_CHECK
(
traj
.
max
()
==
12.
);
}
BOOST_AUTO_TEST_CASE
(
contact_sequence_concatenate_force_traj
)
{
...
...
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