Fix default reference foot frame placement

earlaud · earlaud · commit 354c58522424 · 2025-09-27T18:05:25.000+02:00
diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp
@@ -48,7 +48,7 @@ namespace simple_mpc
     pinocchio::forwardKinematics(model_, data, getReferenceState().head(model_.nq));
     pinocchio::updateFramePlacements(model_, data);
 
-    const pinocchio::SE3 default_placement = data.oMf[reference_parent_frame_id].actInv(data.oMf[frame_id]);
+    const pinocchio::SE3 default_placement = data.oMf[reference_parent_frame_id].actInv(data.oMf[foot_frame_id]);
 
     setFootReferencePlacement(new_foot_index, default_placement);
   }

Original file line number	Diff line number	Diff line change
`@@ -48,7 +48,7 @@ namespace simple_mpc`
`48`	`48`	`pinocchio::forwardKinematics(model_, data, getReferenceState().head(model_.nq));`
`49`	`49`	`pinocchio::updateFramePlacements(model_, data);`
`50`	`50`
`51`		`- const pinocchio::SE3 default_placement = data.oMf[reference_parent_frame_id].actInv(data.oMf[frame_id]);`
	`51`	`+ const pinocchio::SE3 default_placement = data.oMf[reference_parent_frame_id].actInv(data.oMf[foot_frame_id]);`
`52`	`52`
`53`	`53`	`setFootReferencePlacement(new_foot_index, default_placement);`
`54`	`54`	`}`