Fix 6D foot contact point error

earlaud · earlaud · commit 793adbae7b15 · 2025-08-11T19:02:37.000+02:00
diff --git a/src/inverse-dynamics.cpp b/src/inverse-dynamics.cpp
@@ -39,7 +39,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
     }
     case RobotModelHandler::FootType::QUAD: {
       auto contact_6D = std::make_shared<tsid::contacts::Contact6d>(
-        frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i), normal,
+        frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i).transpose(), normal,
         settings_.friction_coefficient, min_f, max_f);
       contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(6));
       contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt());

Original file line number	Diff line number	Diff line change
`@@ -39,7 +39,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c`
`39`	`39`	`}`
`40`	`40`	`case RobotModelHandler::FootType::QUAD: {`
`41`	`41`	`auto contact_6D = std::make_shared<tsid::contacts::Contact6d>(`
`42`		`- frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i), normal,`
	`42`	`+ frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i).transpose(), normal,`
`43`	`43`	`settings_.friction_coefficient, min_f, max_f);`
`44`	`44`	`contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(6));`
`45`	`45`	`contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt());`