Skip to content

Commit 41abd90

Browse files
EtienneArEtienneAr
authored
Merge pull request #66 from Simple-Robotics/RMH_addFoot
Split model handler addFoot function in two
2 parents cb5ffc9 + 43fb903 commit 41abd90

File tree

12 files changed

+114
-42
lines changed

12 files changed

+114
-42
lines changed

‎benchmark/go2.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -40,10 +40,14 @@ int main()
4040
ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0);
4141
ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0);
4242
ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0);
43-
model_handler.addFoot("FL_foot", base_joint_name, ref_FL_foot);
44-
model_handler.addFoot("FR_foot", base_joint_name, ref_FR_foot);
45-
model_handler.addFoot("RL_foot", base_joint_name, ref_RL_foot);
46-
model_handler.addFoot("RR_foot", base_joint_name, ref_RR_foot);
43+
model_handler.addFoot("FL_foot", base_joint_name);
44+
model_handler.addFoot("FR_foot", base_joint_name);
45+
model_handler.addFoot("RL_foot", base_joint_name);
46+
model_handler.addFoot("RR_foot", base_joint_name);
47+
model_handler.setFootReferencePlacement("FL_foot", ref_FL_foot);
48+
model_handler.setFootReferencePlacement("FR_foot", ref_FR_foot);
49+
model_handler.setFootReferencePlacement("RL_foot", ref_RL_foot);
50+
model_handler.setFootReferencePlacement("RR_foot", ref_RR_foot);
4751

4852
RobotDataHandler data_handler(model_handler);
4953

‎benchmark/talos.cpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -72,10 +72,13 @@ int main()
7272
RobotModelHandler model_handler(model, "half_sitting", base_joint);
7373

7474
// Add feet
75-
model_handler.addFoot(
76-
"left_sole_link", base_joint, pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
77-
model_handler.addFoot(
78-
"right_sole_link", base_joint, pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
75+
model_handler.addFoot("left_sole_link", base_joint);
76+
model_handler.addFoot("right_sole_link", base_joint);
77+
78+
model_handler.setFootReferencePlacement(
79+
"left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
80+
model_handler.setFootReferencePlacement(
81+
"right_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
7982

8083
RobotDataHandler data_handler(model_handler);
8184

‎bindings/expose-robot-handler.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ namespace simple_mpc
2424
"RobotModelHandler", bp::init<const pinocchio::Model &, const std::string &, const std::string &>(
2525
bp::args("self", "model", "reference_configuration_name", "base_frame_name")))
2626
.def("addFoot", &RobotModelHandler::addFoot)
27+
.def("setFootReferencePlacement", &RobotModelHandler::setFootReferencePlacement)
2728
.def("difference", &RobotModelHandler::difference)
2829
.def("getBaseFrameId", &RobotModelHandler::getBaseFrameId)
2930
.def("getReferenceState", &RobotModelHandler::getReferenceState)

‎examples/go2_fulldynamics.py‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,16 @@
2323

2424
# Create Model and Data handler
2525
model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
26-
model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
27-
model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
28-
model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
29-
model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
26+
27+
model_handler.addFoot("FL_foot", base_joint_name)
28+
model_handler.addFoot("FR_foot", base_joint_name)
29+
model_handler.addFoot("RL_foot", base_joint_name)
30+
model_handler.addFoot("RR_foot", base_joint_name)
31+
model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
32+
model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
33+
model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
34+
model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
35+
3036
data_handler = RobotDataHandler(model_handler)
3137

3238
nq = model_handler.getModel().nq

‎examples/go2_kinodynamics.py‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,14 @@
1515

1616
# Create Model and Data handler
1717
model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
18-
model_handler.addFoot("FL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
19-
model_handler.addFoot("FR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
20-
model_handler.addFoot("RL_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
21-
model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
18+
model_handler.addFoot("FL_foot", base_joint_name)
19+
model_handler.addFoot("FR_foot", base_joint_name)
20+
model_handler.addFoot("RL_foot", base_joint_name)
21+
model_handler.addFoot("RR_foot", base_joint_name)
22+
model_handler.setFootReferencePlacement("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
23+
model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
24+
model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
25+
model_handler.setFootReferencePlacement("RR_foot", pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
2226
data_handler = RobotDataHandler(model_handler)
2327

2428
nq = model_handler.getModel().nq

‎examples/talos_centroidal.py‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,11 @@
1515

1616
# Create Model and Data handler
1717
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
18-
model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
19-
model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
18+
model_handler.addFoot("left_sole_link", base_joint_name)
19+
model_handler.addFoot("right_sole_link", base_joint_name)
20+
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
21+
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
22+
2023
data_handler = RobotDataHandler(model_handler)
2124

2225
x0 = np.zeros(9)

‎examples/talos_fulldynamics.py‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,11 @@
2222

2323
# Create Model and Data handler
2424
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
25-
model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
26-
model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
25+
model_handler.addFoot("left_sole_link", base_joint_name)
26+
model_handler.addFoot("right_sole_link", base_joint_name)
27+
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
28+
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
29+
2730
data_handler = RobotDataHandler(model_handler)
2831

2932
controlled_joints = rmodel.names[1:].tolist()

‎examples/talos_kinodynamics.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,10 @@
1515

1616
# Create Model and Data handler
1717
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
18-
model_handler.addFoot("left_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
19-
model_handler.addFoot("right_sole_link", base_joint_name, pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
18+
model_handler.addFoot("left_sole_link", base_joint_name)
19+
model_handler.addFoot("right_sole_link", base_joint_name)
20+
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
21+
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
2022
data_handler = RobotDataHandler(model_handler)
2123

2224
nq = model_handler.getModel().nq

‎include/simple-mpc/robot-handler.hpp‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -80,13 +80,20 @@ namespace simple_mpc
8080
* @brief
8181
*
8282
* @param foot_name Frame name that will be used a a foot
83-
* @param placement_reference_frame_name Frame to which the foot reference
83+
* @param reference_frame_name Frame to which the foot reference
8484
* frame will be attached.
85-
* @param placement Transformation from `base_ref_frame_name` to foot
86-
* reference frame
85+
*
86+
* @note The foot placement will be set by default using the reference configuration of the RobotModelHandler
87+
*/
88+
FrameIndex addFoot(const std::string & foot_name, const std::string & reference_frame_name);
89+
90+
/**
91+
* @brief Update the reference placement of the foot wrt to the frame it is attached to
92+
*
93+
* @param foot_name Foot frame name
94+
* @param refMfoot Placement in the local frame
8795
*/
88-
FrameIndex
89-
addFoot(const std::string & foot_name, const std::string & placement_reference_frame_name, const SE3 & placement);
96+
void setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot);
9097

9198
/**
9299
* @brief Perform a finite difference on the sates.

‎src/robot-handler.cpp‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -24,25 +24,39 @@ namespace simple_mpc
2424
mass_ = pinocchio::computeTotalMass(model_);
2525
}
2626

27-
FrameIndex RobotModelHandler::addFoot(
28-
const std::string & foot_name, const std::string & placement_reference_frame_name, const SE3 & placement)
27+
FrameIndex RobotModelHandler::addFoot(const std::string & foot_name, const std::string & reference_frame_name)
2928
{
3029
feet_names_.push_back(foot_name);
3130
feet_ids_.push_back(model_.getFrameId(foot_name));
3231

3332
// Create reference frame
34-
FrameIndex placement_reference_frame_id = model_.getFrameId(placement_reference_frame_name);
35-
JointIndex parent_joint = model_.frames[placement_reference_frame_id].parentJoint;
33+
FrameIndex reference_frame_id = model_.getFrameId(reference_frame_name);
34+
JointIndex parent_joint = model_.frames[reference_frame_id].parentJoint;
3635

37-
auto new_frame =
38-
pinocchio::Frame(foot_name + "_ref", parent_joint, placement_reference_frame_id, placement, pinocchio::OP_FRAME);
36+
auto new_frame = pinocchio::Frame(
37+
foot_name + "_ref", parent_joint, reference_frame_id, pinocchio::SE3::Identity(), pinocchio::OP_FRAME);
3938
auto frame_id = model_.addFrame(new_frame);
4039

40+
// Save foot id
4141
ref_feet_ids_.push_back(frame_id);
4242

43+
// Set placement to default value
44+
pinocchio::Data data(model_);
45+
pinocchio::forwardKinematics(model_, data, getReferenceState().head(model_.nq));
46+
pinocchio::updateFramePlacements(model_, data);
47+
48+
const pinocchio::SE3 default_placement = data.oMf[reference_frame_id].actInv(data.oMf[frame_id]);
49+
50+
setFootReferencePlacement(foot_name, default_placement);
51+
4352
return frame_id;
4453
}
4554

55+
void RobotModelHandler::setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot)
56+
{
57+
model_.frames[model_.getFrameId(foot_name + "_ref", pinocchio::OP_FRAME)].placement = refMfoot;
58+
}
59+
4660
Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const
4761
{
4862
const size_t nq = (size_t)model_.nq;

0 commit comments

Comments
 (0)