Commit c2030738 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Format] fix format changes introduced by previous merge

parent 80efcd3b
...@@ -97,10 +97,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca ...@@ -97,10 +97,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
*/ */
Matrix6X generatorMatrix() const { Matrix6X generatorMatrix() const {
Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3); Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
for(size_t i=0; i<m_num_contact_points; i++) for (size_t i = 0; i < m_num_contact_points; i++) {
{ gen.block(0, i * 3, 3, 3) = Matrix3::Identity();
gen.block(0, i*3, 3, 3) = Matrix3::Identity(); gen.block(3, i * 3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
gen.block(3, i*3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
} }
return gen; return gen;
} }
......
...@@ -1088,13 +1088,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -1088,13 +1088,14 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
} }
} }
} }
if(first_phase == m_contact_phases.size()) if (first_phase == m_contact_phases.size())
throw std::invalid_argument("The contact sequence doesn't have any phase with an effector trajectory" throw std::invalid_argument(
" for the given effector name"); "The contact sequence doesn't have any phase with an effector trajectory"
if(first_phase > 0){ " for the given effector name");
if (first_phase > 0) {
// add a first constant phase at the initial placement // 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(), 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())); m_contact_phases.at(first_phase).timeInitial()));
res.add_curve_ptr(ptr_init); res.add_curve_ptr(ptr_init);
} }
// loop over this phases to concatenate the trajectories // loop over this phases to concatenate the trajectories
...@@ -1109,10 +1110,11 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp ...@@ -1109,10 +1110,11 @@ struct ContactSequenceTpl : public serialization::Serializable<ContactSequenceTp
res.add_curve_ptr(ptr); res.add_curve_ptr(ptr);
} }
} }
if(last_phase < m_contact_phases.size() - 1){ if (last_phase < m_contact_phases.size() - 1) {
// add a last constant phase until the end of the contact sequence // 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(), curve_SE3_ptr ptr_final(new SE3Curve_t(last_placement, last_placement,
m_contact_phases.back().timeFinal())); m_contact_phases.at(last_phase).timeFinal(),
m_contact_phases.back().timeFinal()));
res.add_curve_ptr(ptr_final); res.add_curve_ptr(ptr_final);
} }
return res; return res;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment