diff --git a/benchmark/go2.cpp b/benchmark/go2.cpp
index f4db7269..4dcda296 100644
--- a/benchmark/go2.cpp
+++ b/benchmark/go2.cpp
@@ -40,10 +40,14 @@ int main()
   ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0);
   ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0);
   ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0);
-  model_handler.addFoot("FL_foot", base_joint_name, ref_FL_foot);
-  model_handler.addFoot("FR_foot", base_joint_name, ref_FR_foot);
-  model_handler.addFoot("RL_foot", base_joint_name, ref_RL_foot);
-  model_handler.addFoot("RR_foot", base_joint_name, ref_RR_foot);
+  model_handler.addFoot("FL_foot", base_joint_name);
+  model_handler.addFoot("FR_foot", base_joint_name);
+  model_handler.addFoot("RL_foot", base_joint_name);
+  model_handler.addFoot("RR_foot", base_joint_name);
+  model_handler.setFootReferencePlacement("FL_foot", ref_FL_foot);
+  model_handler.setFootReferencePlacement("FR_foot", ref_FR_foot);
+  model_handler.setFootReferencePlacement("RL_foot", ref_RL_foot);
+  model_handler.setFootReferencePlacement("RR_foot", ref_RR_foot);
 
   RobotDataHandler data_handler(model_handler);
 
diff --git a/benchmark/talos.cpp b/benchmark/talos.cpp
index fcb79b49..502db030 100644
--- a/benchmark/talos.cpp
+++ b/benchmark/talos.cpp
@@ -72,10 +72,13 @@ int main()
   RobotModelHandler model_handler(model, "half_sitting", base_joint);
 
   // Add feet
-  model_handler.addFoot(
-    "left_sole_link", base_joint, pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
-  model_handler.addFoot(
-    "right_sole_link", base_joint, pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
+  model_handler.addFoot("left_sole_link", base_joint);
+  model_handler.addFoot("right_sole_link", base_joint);
+
+  model_handler.setFootReferencePlacement(
+    "left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
+  model_handler.setFootReferencePlacement(
+    "right_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
 
   RobotDataHandler data_handler(model_handler);
 
diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp
index 52fb6f9c..2e55ddfa 100644
--- a/bindings/expose-robot-handler.cpp
+++ b/bindings/expose-robot-handler.cpp
@@ -24,6 +24,7 @@ namespace simple_mpc
         "RobotModelHandler", bp::init<const pinocchio::Model &, const std::string &, const std::string &>(
                                bp::args("self", "model", "reference_configuration_name", "base_frame_name")))
         .def("addFoot", &RobotModelHandler::addFoot)
+        .def("setFootReferencePlacement", &RobotModelHandler::setFootReferencePlacement)
         .def("difference", &RobotModelHandler::difference)
         .def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
         .def("getReferenceState", &RobotModelHandler::getReferenceState)
diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py
index c575cfd0..38fe5c56 100644
--- a/examples/go2_fulldynamics.py
+++ b/examples/go2_fulldynamics.py
@@ -23,10 +23,16 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
-model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
+
+model_handler.addFoot("FL_foot", base_joint_name)
+model_handler.addFoot("FR_foot", base_joint_name)
+model_handler.addFoot("RL_foot", base_joint_name)
+model_handler.addFoot("RR_foot", base_joint_name)
+model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
+
 data_handler = RobotDataHandler(model_handler)
 
 nq = model_handler.getModel().nq
diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py
index 7d21feda..261b335d 100644
--- a/examples/go2_kinodynamics.py
+++ b/examples/go2_kinodynamics.py
@@ -15,10 +15,14 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
-model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
-model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
+model_handler.addFoot("FL_foot", base_joint_name)
+model_handler.addFoot("FR_foot", base_joint_name)
+model_handler.addFoot("RL_foot", base_joint_name)
+model_handler.addFoot("RR_foot", base_joint_name)
+model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
 
 nq = model_handler.getModel().nq
diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py
index 12580c42..e948e520 100644
--- a/examples/talos_centroidal.py
+++ b/examples/talos_centroidal.py
@@ -15,8 +15,11 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
-model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
+model_handler.addFoot("left_sole_link",  base_joint_name)
+model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
+
 data_handler = RobotDataHandler(model_handler)
 
 x0 = np.zeros(9)
diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py
index 41c8c69b..cee4a643 100644
--- a/examples/talos_fulldynamics.py
+++ b/examples/talos_fulldynamics.py
@@ -22,8 +22,11 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
-model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
+model_handler.addFoot("left_sole_link",  base_joint_name)
+model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
+
 data_handler = RobotDataHandler(model_handler)
 
 controlled_joints = rmodel.names[1:].tolist()
diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py
index 9a6c27e0..a47aafe2 100644
--- a/examples/talos_kinodynamics.py
+++ b/examples/talos_kinodynamics.py
@@ -15,8 +15,10 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
-model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
+model_handler.addFoot("left_sole_link",  base_joint_name)
+model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
+model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
 
 nq = model_handler.getModel().nq
diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp
index c4f2557f..734ae3ec 100644
--- a/include/simple-mpc/robot-handler.hpp
+++ b/include/simple-mpc/robot-handler.hpp
@@ -80,13 +80,20 @@ namespace simple_mpc
      * @brief
      *
      * @param foot_name Frame name that will be used a a foot
-     * @param placement_reference_frame_name Frame to which the foot reference
+     * @param reference_frame_name Frame to which the foot reference
      * frame will be attached.
-     * @param placement Transformation from `base_ref_frame_name` to foot
-     * reference frame
+     *
+     * @note The foot placement will be set by default using the reference configuration of the RobotModelHandler
+     */
+    FrameIndex addFoot(const std::string & foot_name, const std::string & reference_frame_name);
+
+    /**
+     * @brief Update the reference placement of the foot wrt to the frame it is attached to
+     *
+     * @param foot_name Foot frame name
+     * @param refMfoot Placement in the local frame
      */
-    FrameIndex
-    addFoot(const std::string & foot_name, const std::string & placement_reference_frame_name, const SE3 & placement);
+    void setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot);
 
     /**
      * @brief Perform a finite difference on the sates.
diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp
index b4477c7c..f77c4015 100644
--- a/src/robot-handler.cpp
+++ b/src/robot-handler.cpp
@@ -24,25 +24,39 @@ namespace simple_mpc
     mass_ = pinocchio::computeTotalMass(model_);
   }
 
-  FrameIndex RobotModelHandler::addFoot(
-    const std::string & foot_name, const std::string & placement_reference_frame_name, const SE3 & placement)
+  FrameIndex RobotModelHandler::addFoot(const std::string & foot_name, const std::string & reference_frame_name)
   {
     feet_names_.push_back(foot_name);
     feet_ids_.push_back(model_.getFrameId(foot_name));
 
     // Create reference frame
-    FrameIndex placement_reference_frame_id = model_.getFrameId(placement_reference_frame_name);
-    JointIndex parent_joint = model_.frames[placement_reference_frame_id].parentJoint;
+    FrameIndex reference_frame_id = model_.getFrameId(reference_frame_name);
+    JointIndex parent_joint = model_.frames[reference_frame_id].parentJoint;
 
-    auto new_frame =
-      pinocchio::Frame(foot_name + "_ref", parent_joint, placement_reference_frame_id, placement, pinocchio::OP_FRAME);
+    auto new_frame = pinocchio::Frame(
+      foot_name + "_ref", parent_joint, reference_frame_id, pinocchio::SE3::Identity(), pinocchio::OP_FRAME);
     auto frame_id = model_.addFrame(new_frame);
 
+    // Save foot id
     ref_feet_ids_.push_back(frame_id);
 
+    // Set placement to default value
+    pinocchio::Data data(model_);
+    pinocchio::forwardKinematics(model_, data, getReferenceState().head(model_.nq));
+    pinocchio::updateFramePlacements(model_, data);
+
+    const pinocchio::SE3 default_placement = data.oMf[reference_frame_id].actInv(data.oMf[frame_id]);
+
+    setFootReferencePlacement(foot_name, default_placement);
+
     return frame_id;
   }
 
+  void RobotModelHandler::setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot)
+  {
+    model_.frames[model_.getFrameId(foot_name + "_ref", pinocchio::OP_FRAME)].placement = refMfoot;
+  }
+
   Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const
   {
     const size_t nq = (size_t)model_.nq;
diff --git a/tests/robot_handler.cpp b/tests/robot_handler.cpp
index f10b3ae4..49191566 100644
--- a/tests/robot_handler.cpp
+++ b/tests/robot_handler.cpp
@@ -48,7 +48,8 @@ BOOST_AUTO_TEST_CASE(model_handler)
   // Add feet
   for (size_t i = 0; i < feet_names.size(); i++)
   {
-    model_handler.addFoot(feet_names.at(i), base_frame, feet_refs.at(i));
+    model_handler.addFoot(feet_names.at(i), base_frame);
+    model_handler.setFootReferencePlacement(feet_names.at(i), feet_refs.at(i));
   }
 
   /*********/
@@ -149,6 +150,20 @@ BOOST_AUTO_TEST_CASE(model_handler)
     pinocchio::computeTotalMass(model, data);
     BOOST_CHECK_EQUAL(model_handler.getMass(), data.mass[0]);
   }
+
+  // Check that the default foot pose actually match the reference configuration
+  {
+    const std::string foot_name = "FR_calf";
+    model_handler.addFoot(foot_name, base_frame);
+    // Do not set foot reference placement to check the default one
+
+    RobotDataHandler data_handler(model_handler);
+
+    data_handler.updateInternalData(model_handler.getReferenceState(), false);
+
+    // Foot and ref should be coincident with reference configuration
+    data_handler.getFootPose(foot_name).isApprox(data_handler.getRefFootPose(foot_name));
+  }
 }
 
 BOOST_AUTO_TEST_CASE(data_handler)
diff --git a/tests/test_utils.cpp b/tests/test_utils.cpp
index b78b84b2..cbf39d35 100644
--- a/tests/test_utils.cpp
+++ b/tests/test_utils.cpp
@@ -66,10 +66,12 @@ RobotModelHandler getTalosModelHandler()
   RobotModelHandler model_handler(model, "half_sitting", base_joint);
 
   // Add feet
-  model_handler.addFoot(
-    "left_sole_link", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
-  model_handler.addFoot(
-    "right_sole_link", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
+  model_handler.addFoot("left_sole_link", base_joint);
+  model_handler.addFoot("right_sole_link", base_joint);
+  model_handler.setFootReferencePlacement(
+    "left_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
+  model_handler.setFootReferencePlacement(
+    "right_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
 
   return model_handler;
 }
@@ -89,10 +91,18 @@ RobotModelHandler getSoloHandler()
   RobotModelHandler model_handler(model, "straight_standing", base_joint);
 
   // Add feet
-  model_handler.addFoot("FR_FOOT", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.)));
-  model_handler.addFoot("FL_FOOT", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, 0.1, 0.)));
-  model_handler.addFoot("HR_FOOT", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, -0.1, 0.)));
-  model_handler.addFoot("HL_FOOT", base_joint, SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, 0.1, 0.)));
+  model_handler.addFoot("FR_FOOT", base_joint);
+  model_handler.addFoot("FL_FOOT", base_joint);
+  model_handler.addFoot("HR_FOOT", base_joint);
+  model_handler.addFoot("HL_FOOT", base_joint);
+  model_handler.setFootReferencePlacement(
+    "FR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.)));
+  model_handler.setFootReferencePlacement(
+    "FL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, 0.1, 0.)));
+  model_handler.setFootReferencePlacement(
+    "HR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, -0.1, 0.)));
+  model_handler.setFootReferencePlacement(
+    "HL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, 0.1, 0.)));
 
   return model_handler;
 }