1 #ifndef __multicontact_api_scenario_contact_sequence_hpp__ 2 #define __multicontact_api_scenario_contact_sequence_hpp__ 4 #include <ndcurves/curve_abc.h> 5 #include <ndcurves/fwd.h> 6 #include <ndcurves/piecewise_curve.h> 7 #include <ndcurves/polynomial.h> 8 #include <ndcurves/se3_curve.h> 10 #include <boost/serialization/vector.hpp> 20 template <
class _ContactPhase>
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 return !(*
this == other);
89 throw std::invalid_argument(
"Given Id is greater than the vector size");
99 throw std::invalid_argument(
"Given Id is greater than the vector size");
122 const double phaseDuration = -1) {
125 throw std::invalid_argument(
126 "In breakContact : effector is not currently in contact : " + eeName);
127 if (phaseDuration > 0) {
129 throw std::invalid_argument(
130 "In breakContact : duration is specified but current phase " 131 "interval in not initialised.");
148 if (name != eeName) {
172 const double phaseDuration = -1) {
175 throw std::invalid_argument(
176 "In createContact : effector is already in contact : " + eeName);
177 if (phaseDuration > 0) {
179 throw std::invalid_argument(
180 "In createContact : duration is specified but current phase " 181 "interval in not initialised.");
227 const double durationBreak = -1,
228 const double durationCreate = -1) {
230 throw std::invalid_argument(
231 "In moveEffectorToPlacement : effector is not currently in contact " 267 const double durationBreak = -1,
268 const double durationCreate = -1) {
270 throw std::invalid_argument(
271 "In moveEffectorToPlacement : effector is not currently in contact " 289 if (current_t < 0.) {
290 std::cout <<
"Initial time is negative." << std::endl;
293 for (
size_t i = 0; i <
size(); ++i) {
296 std::cout <<
"For phase " << i
297 <<
" initial time is not equal to previous final time." 302 std::cout <<
"For phase " << i <<
" final time is before initial time." 321 std::cout <<
"Phase without any contact at id : 0" << std::endl;
331 if (variations > 1) {
332 std::cout <<
"Several contact changes between adjacents phases at id : " 340 <<
"Contact repositionning without intermediate phase at id : " << i
346 std::cout <<
"Phase without any contact at id : " << i << std::endl;
349 if (variations == 0) {
350 std::cout <<
"No contact change between adjacents phases at id : " << i
366 std::cout <<
"Initial CoM position not defined." << std::endl;
371 std::cout <<
"Intermediate CoM position not defined for phase : " << i
377 std::cout <<
"Init CoM position do not match final CoM of previous " 385 std::cout <<
"Init CoM velocity do not match final velocity of " 386 "previous phase for id : " 394 std::cout <<
"Init CoM acceleration do not match final acceleration " 395 "of previous phase for id : " 402 std::cout <<
"Final CoM position not defined." << std::endl;
418 std::cout <<
"Init AM value do not match final value of previous phase " 425 std::cout <<
"Init AM derivative do not match final AM derivative of " 426 "previous phase for id : " 452 std::cout <<
"Initial configuration not defined." << std::endl;
457 std::cout <<
"Intermediate configuration not defined for phase : " << i
463 std::cout <<
"Init configuration do not match final configuration of " 464 "previous phase for id : " 470 std::cout <<
"Final configuration not defined." << std::endl;
488 std::cout <<
"CoM position trajectory not defined for phase : " << i
493 std::cout <<
"CoM velocity trajectory not defined for phase : " << i
498 std::cout <<
"CoM acceleration trajectory not defined for phase : " << i
502 if (phase.m_c->dim() != 3) {
503 std::cout <<
"CoM trajectory is not of dimension 3 for phase : " << i
507 if (phase.m_dc->dim() != 3) {
509 <<
"CoM velocity trajectory is not of dimension 3 for phase : " << i
513 if (phase.m_ddc->dim() != 3) {
515 <<
"CoM acceleration trajectory is not of dimension 3 for phase : " 519 if (phase.m_c->min() != phase.timeInitial()) {
520 std::cout <<
"CoM trajectory do not start at t_init for phase : " << i
524 if (phase.m_dc->min() != phase.timeInitial()) {
526 <<
"CoM velocity trajectory do not start at t_init for phase : " 530 if (phase.m_ddc->min() != phase.timeInitial()) {
532 <<
"CoM acceleration trajectory do not start at t_init for phase : " 536 if (phase.m_c->max() != phase.timeFinal()) {
537 std::cout <<
"CoM trajectory do not end at t_final for phase : " << i
541 if (phase.m_dc->max() != phase.timeFinal()) {
543 <<
"CoM velocity trajectory do not end at t_final for phase : " << i
547 if (phase.m_ddc->max() != phase.timeFinal()) {
549 <<
"CoM acceleration trajectory do not end at t_final for phase : " 553 if (!(*phase.m_c)(phase.m_c->min()).isApprox(phase.m_c_init)) {
554 std::cout <<
"CoM trajectory do not start at c_init for phase : " << i
558 if (phase.m_dc_init.isZero()) {
559 if (!(*phase.m_dc)(phase.m_dc->min()).isZero()) {
561 <<
"CoM velocity trajectory do not start at dc_init for phase : " 567 else if (!(*phase.m_dc)(phase.m_dc->min())
568 .isApprox(phase.m_dc_init, 1e-6)) {
570 <<
"CoM velocity trajectory do not start at dc_init for phase : " 574 if (phase.m_ddc_init.isZero()) {
575 if (!(*phase.m_ddc)(phase.m_ddc->min()).isZero()) {
576 std::cout <<
"CoM acceleration trajectory do not start at ddc_init " 581 }
else if (!(*phase.m_ddc)(phase.m_ddc->min())
582 .isApprox(phase.m_ddc_init, 1e-6)) {
583 std::cout <<
"CoM acceleration trajectory do not start at ddc_init for " 588 if (!(*phase.m_c)(phase.m_c->max()).isApprox(phase.m_c_final)) {
589 std::cout <<
"CoM trajectory do not end at c_final for phase : " << i
593 if (phase.m_dc_final.isZero()) {
594 if (!(*phase.m_dc)(phase.m_dc->max()).isZero()) {
596 <<
"CoM velocity trajectory do not end at dc_final for phase : " 600 }
else if (!(*phase.m_dc)(phase.m_dc->max())
601 .isApprox(phase.m_dc_final, 1e-6)) {
603 <<
"CoM velocity trajectory do not end at dc_final for phase : " 607 if (phase.m_ddc_final.isZero()) {
608 if (!(*phase.m_ddc)(phase.m_ddc->max()).isZero()) {
609 std::cout <<
"CoM acceleration trajectory do not end at ddc_final " 614 }
else if (!(*phase.m_ddc)(phase.m_ddc->max())
615 .isApprox(phase.m_ddc_final, 1e-6)) {
616 std::cout <<
"CoM acceleration trajectory do not end at ddc_final for " 638 std::cout <<
"AM position trajectory not defined for phase : " << i
643 std::cout <<
"AM velocity trajectory not defined for phase : " << i
647 if (phase.m_L->dim() != 3) {
648 std::cout <<
"AM trajectory is not of dimension 3 for phase : " << i
652 if (phase.m_dL->dim() != 3) {
654 <<
"AM derivative trajectory is not of dimension 3 for phase : " 658 if (phase.m_L->min() != phase.timeInitial()) {
659 std::cout <<
"AM trajectory do not start at t_init for phase : " << i
663 if (phase.m_dL->min() != phase.timeInitial()) {
665 <<
"AM derivative trajectory do not start at t_init for phase : " 669 if (phase.m_L->max() != phase.timeFinal()) {
670 std::cout <<
"AM trajectory do not end at t_final for phase : " << i
674 if (phase.m_dL->max() != phase.timeFinal()) {
676 <<
"AM derivative trajectory do not end at t_final for phase : " 680 if (phase.m_L_init.isZero()) {
681 if (!(*phase.m_L)(phase.m_L->min()).isZero()) {
682 std::cout <<
"AM trajectory do not start at L_init for phase : " << i
686 }
else if (!(*phase.m_L)(phase.m_L->min()).isApprox(phase.m_L_init)) {
687 std::cout <<
"AM trajectory do not start at L_init for phase : " << i
691 if (phase.m_dL_init.isZero()) {
692 if (!(*phase.m_dL)(phase.m_dL->min()).isZero()) {
694 <<
"AM derivative trajectory do not start at dL_init for phase : " 698 }
else if (!(*phase.m_dL)(phase.m_dL->min())
699 .isApprox(phase.m_dL_init, 1e-6)) {
701 <<
"AM derivative trajectory do not start at dL_init for phase : " 705 if (phase.m_L_final.isZero()) {
706 if (!(*phase.m_L)(phase.m_L->max()).isZero()) {
707 std::cout <<
"AM trajectory do not end at L_final for phase : " << i
711 }
else if (!(*phase.m_L)(phase.m_L->max())
712 .isApprox(phase.m_L_final, 1e-6)) {
713 std::cout <<
"AM trajectory do not end at L_final for phase : " << i
717 if (phase.m_dL_final.isZero()) {
718 if (!(*phase.m_dL)(phase.m_dL->max()).isZero()) {
720 <<
"AM derivative trajectory do not end at dL_final for phase : " 724 }
else if (!(*phase.m_dL)(phase.m_dL->max()).isApprox(phase.m_dL_final)) {
726 <<
"AM derivative trajectory do not end at dL_final for phase : " 757 const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision(),
758 const bool use_rotation =
true)
const {
764 std::cout <<
"No end effector trajectory for " << eeName
765 <<
" at phase " << i <<
" but it is in contact at phase " 766 << i + 1 << std::endl;
772 std::cout <<
"Effector trajectory for " << eeName
773 <<
" do not start at t_init in phase " << i << std::endl;
777 std::cout <<
"Effector trajectory for " << eeName
778 <<
" do not end at t_final in phase " << i << std::endl;
794 std::cout <<
"Effector trajectory for " << eeName
795 <<
" do not end at it's contact placement in the next " 798 std::cout <<
"Last point : " << std::endl
800 <<
"Next contact : " << std::endl
822 std::cout <<
"Effector trajectory for " << eeName
823 <<
" do not start at it's contact placement in the " 824 "previous phase, for phase " 826 std::cout <<
"First point : " << std::endl
828 <<
"Previous contact : " << std::endl
854 std::cout <<
"joint position trajectory not defined for phase : " << i
858 if (phase.m_q->min() != phase.timeInitial()) {
859 std::cout <<
"joint trajectory do not start at t_init for phase : " << i
863 if (phase.m_q->max() != phase.timeFinal()) {
864 std::cout <<
"joint trajectory do not end at t_final for phase : " << i
868 if (!(*phase.m_q)(phase.m_q->min()).isApprox(phase.m_q_init)) {
869 std::cout <<
"joint trajectory do not start at q_init for phase : " << i
873 if (!(*phase.m_q)(phase.m_q->max()).isApprox(phase.m_q_final)) {
874 std::cout <<
"joint trajectory do not end at q_final for phase : " << i
895 std::cout <<
"joint velocity trajectory not defined for phase : " << i
900 std::cout <<
"joint acceleration trajectory not defined for phase : " 904 if (phase.m_dq->min() != phase.timeInitial()) {
906 <<
"joint velocity trajectory do not start at t_init for phase : " 910 if (phase.m_ddq->min() != phase.timeInitial()) {
911 std::cout <<
"joint acceleration trajectory do not start at t_init for " 916 if (phase.m_dq->max() != phase.timeFinal()) {
918 <<
"joint velocity trajectory do not end at t_final for phase : " 922 if ((phase.m_ddq->max() != phase.timeFinal()) &&
924 std::cout <<
"joint acceleration trajectory do not end at t_final for " 946 std::cout <<
"Torque trajectory not defined for phase : " << i
951 if (phase.m_tau->min() != phase.timeInitial()) {
952 std::cout <<
"Torque trajectory do not start at t_init for phase : " 956 if ((phase.m_tau->max() != phase.timeFinal()) && i <
size() - 1) {
957 std::cout <<
"Torque trajectory do not end at t_final for phase : " << i
977 for (std::string eeName : phase.effectorsInContact()) {
978 if (phase.contactForces().count(eeName) == 0) {
979 std::cout <<
"No contact forces trajectory for effector " << eeName
980 <<
" at phase " << i << std::endl;
983 if (phase.contactNormalForces().count(eeName) == 0) {
984 std::cout <<
"No contact normal force trajectory for effector " 985 << eeName <<
" for phase " << i << std::endl;
988 if (phase.contactForces().at(eeName)->min() != phase.timeInitial()) {
989 std::cout <<
"Contact forces trajectory for effector " << eeName
990 <<
" do not start at t_init for phase " << i << std::endl;
993 if (phase.contactForces().at(eeName)->max() != phase.timeFinal()) {
994 std::cout <<
"Contact forces trajectory for effector " << eeName
995 <<
" do not end at t_final for phase " << i << std::endl;
998 if (phase.contactNormalForces().at(eeName)->min() !=
999 phase.timeInitial()) {
1000 std::cout <<
"Contact normal force trajectory for effector " << eeName
1001 <<
" do not start at t_init for phase " << i << std::endl;
1004 if (phase.contactNormalForces().at(eeName)->max() !=
1005 phase.timeFinal()) {
1006 std::cout <<
"Contact normal force trajectory for effector " << eeName
1007 <<
" do not end at t_final for phase " << i << std::endl;
1025 if (!phase.m_root) {
1026 std::cout <<
"Root trajectory not defined for phase : " << i
1030 if (phase.m_root->min() != phase.timeInitial()) {
1031 std::cout <<
"Root trajectory do not start at t_init for phase : " << i
1035 if (phase.m_root->max() != phase.timeFinal()) {
1036 std::cout <<
"Root trajectory do not start at t_final for phase : " << i
1053 for (
const std::string& eeName : phase.effectorsInContact()) {
1054 if (phase.contactPatches().at(eeName).friction() <= 0.) {
1055 std::cout <<
"Friction not defined for phase " << i
1056 <<
" and effector " << eeName << std::endl;
1073 for (
const std::string& eeName : phase.effectorsInContact()) {
1074 if (phase.contactPatches().at(eeName).m_contact_model.m_contact_type ==
1076 std::cout <<
"ContactModel not defined for phase " << i
1077 <<
" and effector " << eeName << std::endl;
1095 std::cout <<
"ZMP trajectory not defined for phase : " << i
1099 if (phase.m_zmp->dim() != 3) {
1100 std::cout <<
"ZMP trajectory is not of dimension 3 for phase : " << i
1104 if (phase.m_zmp->min() != phase.timeInitial()) {
1105 std::cout <<
"ZMP trajectory do not start at t_init for phase : " << i
1109 if (phase.m_zmp->max() != phase.timeFinal()) {
1110 std::cout <<
"ZMP trajectory do not end at t_final for phase : " << i
1127 std::set<std::string> res_set;
1129 for (
const std::string& eeName : phase.effectorsInContact()) {
1130 res_set.insert(eeName);
1133 return t_strings(res_set.begin(), res_set.end());
1144 res.add_curve_ptr(phase.m_c);
1157 res.add_curve_ptr(phase.m_dc);
1170 res.add_curve_ptr(phase.m_ddc);
1183 res.add_curve_ptr(phase.m_L);
1196 res.add_curve_ptr(phase.m_dL);
1209 res.add_curve_ptr(phase.m_zmp);
1223 res.add_curve_ptr(phase.m_wrench);
1236 res.add_curve_ptr(phase.m_q);
1249 res.add_curve_ptr(phase.m_dq);
1262 res.add_curve_ptr(phase.m_ddq);
1275 res.add_curve_ptr(phase.m_tau);
1288 res.add_curve_ptr(phase.m_root);
1303 const std::string& eeName)
const {
1308 size_t last_phase = 0;
1312 if (first_phase > i) {
1316 first_placement = curve->operator()(curve->min());
1321 throw std::invalid_argument(
1322 "The contact sequence doesn't have any phase with an effector " 1324 " for the given effector name");
1325 if (first_phase > 0) {
1328 new SE3Curve_t(first_placement, first_placement,
1331 res.add_curve_ptr(ptr_init);
1334 for (
size_t i = first_phase; i <= last_phase; ++i) {
1338 last_placement = res(res.max());
1345 res.add_curve_ptr(ptr);
1351 new SE3Curve_t(last_placement, last_placement,
1354 res.add_curve_ptr(ptr_final);
1369 const std::string& eeName)
const {
1374 if (phase.contactForces().count(eeName) > 0) {
1375 dim = phase.contactForces().at(eeName)->dim();
1382 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(dim, 1);
1385 if (phase.contactForces().count(eeName) > 0) {
1386 res.add_curve_ptr(phase.contactForces().at(eeName));
1389 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1390 res.add_curve_ptr(ptr);
1406 const std::string& eeName)
const {
1408 Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(1, 1);
1411 if (phase.contactNormalForces().count(eeName) > 0) {
1412 res.add_curve_ptr(phase.contactNormalForces().at(eeName));
1415 new polynomial_t(zeros, phase.timeInitial(), phase.timeFinal()));
1416 res.add_curve_ptr(ptr);
1435 throw std::invalid_argument(
"No phase found for the given time.");
1468 template <
class Archive>
1469 void save(Archive& ar,
const unsigned int )
const {
1470 const size_t m_size =
size();
1471 ar& boost::serialization::make_nvp(
"size", m_size);
1472 for (
typename ContactPhaseVector::const_iterator it =
1475 ar& boost::serialization::make_nvp(
"contact_phase", *it);
1479 template <
class Archive>
1480 void load(Archive& ar,
const unsigned int ) {
1482 ar >> boost::serialization::make_nvp(
"size", m_size);
1487 ar >> boost::serialization::make_nvp(
"contact_phase", *it);
1491 BOOST_SERIALIZATION_SPLIT_MEMBER()
1501 #endif // __multicontact_api_scenario_contact_sequence_hpp__
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:22