Skip to content

Split model handler addFoot function in two #66

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Jul 16, 2025
Merged

Split model handler addFoot function in two #66

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
16c3d80
Split model handler addFoot function into addFoot and setFootReferenc…
Jul 15, 2025
43fb903
Add testcase for default foot ref placement
Jul 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 8 additions & 4 deletions benchmark/go2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
11 changes: 7 additions & 4 deletions benchmark/talos.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
1 change: 1 addition & 0 deletions bindings/expose-robot-handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
14 changes: 10 additions & 4 deletions examples/go2_fulldynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 8 additions & 4 deletions examples/go2_kinodynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 5 additions & 2 deletions examples/talos_centroidal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
7 changes: 5 additions & 2 deletions examples/talos_fulldynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
6 changes: 4 additions & 2 deletions examples/talos_kinodynamics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
17 changes: 12 additions & 5 deletions include/simple-mpc/robot-handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
26 changes: 20 additions & 6 deletions src/robot-handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
17 changes: 16 additions & 1 deletion tests/robot_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}

/*********/
Expand Down Expand Up @@ -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)
Expand Down
26 changes: 18 additions & 8 deletions tests/test_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand Down
Loading