Add assert on contact forces size

earlaud · earlaud · commit 758c3b536ec7 · 2025-08-19T11:05:27.000+02:00
diff --git a/src/inverse-dynamics/kinodynamics.cpp b/src/inverse-dynamics/kinodynamics.cpp
@@ -155,11 +155,13 @@ void KinodynamicsID::setTarget(
       switch (model_handler_.getFootType(foot_nb))
       {
       case RobotModelHandler::FootType::POINT: {
+        assert(f_target.at(foot_nb).size() == 3);
         std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[foot_nb])
           ->setForceReference(f_target.at(foot_nb));
         break;
       }
       case RobotModelHandler::FootType::QUAD: {
+        assert(f_target.at(foot_nb).size() == 6);
         std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[foot_nb])
           ->setForceReference(f_target.at(foot_nb));
         break;

Original file line number	Diff line number	Diff line change
`@@ -155,11 +155,13 @@ void KinodynamicsID::setTarget(`
`155`	`155`	`switch (model_handler_.getFootType(foot_nb))`
`156`	`156`	`{`
`157`	`157`	`case RobotModelHandler::FootType::POINT: {`
	`158`	`+ assert(f_target.at(foot_nb).size() == 3);`
`158`	`159`	`std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[foot_nb])`
`159`	`160`	`->setForceReference(f_target.at(foot_nb));`
`160`	`161`	`break;`
`161`	`162`	`}`
`162`	`163`	`case RobotModelHandler::FootType::QUAD: {`
	`164`	`+ assert(f_target.at(foot_nb).size() == 6);`
`163`	`165`	`std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[foot_nb])`
`164`	`166`	`->setForceReference(f_target.at(foot_nb));`
`165`	`167`	`break;`