Skip to content

Commit 5d1ce4a

Browse files
author
earlaud
committed
Rename 6D foot into Quad
1 parent b3e583a commit 5d1ce4a

File tree

9 files changed

+46
-27
lines changed

9 files changed

+46
-27
lines changed

‎benchmark/talos.cpp‎

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

7474
// Add feet
75-
model_handler.add6DFoot("left_sole_link", base_joint);
76-
model_handler.add6DFoot("right_sole_link", base_joint);
75+
model_handler.addQuadFoot("left_sole_link", base_joint);
76+
model_handler.addQuadFoot("right_sole_link", base_joint);
7777

7878
model_handler.setFootReferencePlacement(
7979
"left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));

‎bindings/expose-robot-handler.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +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("addPointFoot", &RobotModelHandler::addPointFoot)
27-
.def("add6DFoot", &RobotModelHandler::add6DFoot)
27+
.def("addQuadFoot", &RobotModelHandler::addQuadFoot)
2828
.def("setFootReferencePlacement", &RobotModelHandler::setFootReferencePlacement)
2929
.def("difference", &RobotModelHandler::difference)
3030
.def("getBaseFrameName", &RobotModelHandler::getBaseFrameName)

‎examples/talos_centroidal.py‎

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

1616
# Create Model and Data handler
1717
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
18-
model_handler.add6DFoot("left_sole_link", base_joint_name)
19-
model_handler.add6DFoot("right_sole_link", base_joint_name)
18+
model_handler.addQuadFoot("left_sole_link", base_joint_name)
19+
model_handler.addQuadFoot("right_sole_link", base_joint_name)
2020
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
2121
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
2222

‎examples/talos_fulldynamics.py‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,15 @@
2121
rmodelComplete, rmodel, qComplete, q0 = loadTalos()
2222

2323
# Create Model and Data handler
24+
foot_points = np.array([
25+
[0.1, 0.075, 0],
26+
[-0.1, 0.075, 0],
27+
[-0.1, -0.075, 0],
28+
[0.1, -0.075, 0],
29+
])
2430
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
25-
model_handler.add6DFoot("left_sole_link", base_joint_name)
26-
model_handler.add6DFoot("right_sole_link", base_joint_name)
31+
model_handler.addQuadFoot("left_sole_link", base_joint_name, foot_points)
32+
model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points)
2733
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
2834
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
2935

‎examples/talos_kinodynamics.py‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,15 @@
1414
rmodelComplete, rmodel, qComplete, q0 = loadTalos()
1515

1616
# Create Model and Data handler
17+
foot_points = np.array([
18+
[0.1, 0.075, 0],
19+
[-0.1, 0.075, 0],
20+
[-0.1, -0.075, 0],
21+
[0.1, -0.075, 0],
22+
])
1723
model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
18-
model_handler.add6DFoot("left_sole_link", base_joint_name)
19-
model_handler.add6DFoot("right_sole_link", base_joint_name)
24+
model_handler.addQuadFoot("left_sole_link", base_joint_name, foot_points)
25+
model_handler.addQuadFoot("right_sole_link", base_joint_name, foot_points)
2026
model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
2127
model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
2228
data_handler = RobotDataHandler(model_handler)

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@ namespace simple_mpc
3030
enum FootType
3131
{
3232
POINT,
33-
SIX_D
33+
QUAD
3434
};
3535
typedef Eigen::Matrix<double, 4, 3> ContactPointsMatrix;
3636

@@ -118,7 +118,7 @@ namespace simple_mpc
118118
*
119119
* @note The foot placement will be set by default using the reference configuration of the RobotModelHandler
120120
*/
121-
size_t add6DFoot(
121+
size_t addQuadFoot(
122122
const std::string & foot_name,
123123
const std::string & reference_frame_name,
124124
const ContactPointsMatrix & contactPoints);

‎src/inverse-dynamics.cpp‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -37,11 +37,11 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
3737
tsid_contacts.push_back(contact_point);
3838
break;
3939
}
40-
case RobotModelHandler::FootType::SIX_D: {
40+
case RobotModelHandler::FootType::QUAD: {
4141
auto contact_6D = std::make_shared<tsid::contacts::Contact6d>(
4242
frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i), normal,
4343
settings_.friction_coefficient, min_f, max_f);
44-
contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
44+
contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(6));
4545
contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt());
4646
tsid_contacts.push_back(contact_6D);
4747
break;
@@ -92,17 +92,27 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
9292
solver_ = tsid::solvers::SolverHQPFactory::createNewSolver(tsid::solvers::SOLVER_HQP_PROXQP, "solver-proxqp");
9393
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
9494

95-
// Dry run to initialize solver data & output
95+
// By default initialize target in reference state
9696
const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq);
9797
const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv);
9898
std::vector<bool> c_ref(n_contacts);
99-
MatrixN3d f_ref = MatrixN3d::Zero(n_contacts, 3);
99+
std::vector<TargetContactForce> f_ref;
100100
for (int i = 0; i < n_contacts; i++)
101101
{
102+
// By default initialize all foot in contact with same amount of force
102103
c_ref[i] = true;
103-
f_ref(i, 2) = weight / n_contacts;
104+
const RobotModelHandler::FootType foot_type = model_handler.getFootType(i);
105+
if (foot_type == RobotModelHandler::POINT)
106+
f_ref.push_back(TargetContactForce::Zero(3));
107+
else if (foot_type == RobotModelHandler::QUAD)
108+
f_ref.push_back(TargetContactForce::Zero(6));
109+
else
110+
assert(false);
111+
f_ref[i][2] = weight / n_contacts; // Weight on Z axis
104112
}
105113
setTarget(q_ref, v_ref, v_ref, c_ref, f_ref);
114+
115+
// Dry run to initialize solver data & output
106116
const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref);
107117
last_solution_ = solver_->solve(solver_data);
108118
}
@@ -186,7 +196,7 @@ void KinodynamicsID::solve(
186196
->setReference(data_handler_.getFootPose(i));
187197
break;
188198
}
189-
case RobotModelHandler::FootType::SIX_D: {
199+
case RobotModelHandler::FootType::QUAD: {
190200
std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[i])
191201
->setReference(data_handler_.getFootPose(i));
192202
break;

‎src/robot-handler.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -54,16 +54,16 @@ namespace simple_mpc
5454
{
5555
addFootFrames(foot_name, reference_frame_name);
5656
feet_types_.push_back(FootType::POINT);
57-
const size_t foot_nb = feet_types_.size();
57+
const size_t foot_nb = feet_types_.size() - 1;
5858
return foot_nb;
5959
}
6060

61-
size_t RobotModelHandler::add6DFoot(
61+
size_t RobotModelHandler::addQuadFoot(
6262
const std::string & foot_name, const std::string & reference_frame_name, const ContactPointsMatrix & contactPoints)
6363
{
6464
addFootFrames(foot_name, reference_frame_name);
65-
feet_types_.push_back(FootType::SIX_D);
66-
const size_t foot_nb = feet_types_.size();
65+
feet_types_.push_back(FootType::QUAD);
66+
const size_t foot_nb = feet_types_.size() - 1;
6767
feet_contact_points_.insert({foot_nb, contactPoints});
6868
return foot_nb;
6969
}

‎tests/test_utils.cpp‎

Lines changed: 3 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -66,12 +66,9 @@ RobotModelHandler getTalosModelHandler()
6666
RobotModelHandler model_handler(model, "half_sitting", base_joint);
6767

6868
// Add feet
69-
model_handler.addPointFoot("left_sole_link", base_joint);
70-
model_handler.addPointFoot("right_sole_link", base_joint);
71-
model_handler.setFootReferencePlacement(
72-
"left_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
73-
model_handler.setFootReferencePlacement(
74-
"right_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
69+
Eigen::Matrix<double, 4, 3> foot_quad{{0.1, 0.075, 0}, {-0.1, 0.075, 0}, {-0.1, -0.075, 0}, {0.1, -0.075, 0}};
70+
model_handler.addQuadFoot("left_sole_link", base_joint, foot_quad);
71+
model_handler.addQuadFoot("right_sole_link", base_joint, foot_quad);
7572

7673
return model_handler;
7774
}

0 commit comments

Comments
 (0)