Skip to content

Commit 71c6cbf

Browse files
author
earlaud
committed
WiP: properly add foot contacts in tsid
1 parent 1822f56 commit 71c6cbf

File tree

4 files changed

+84
-37
lines changed

4 files changed

+84
-37
lines changed

‎bindings/expose-robot-handler.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,10 @@ namespace simple_mpc
4141

4242
bp::class_<RobotDataHandler>(
4343
"RobotDataHandler", bp::init<const RobotModelHandler &>(bp::args("self", "model_handler")))
44-
.def("updateInternalData", &RobotDataHandler::updateInternalData)
44+
// .def("updateInternalData", &RobotDataHandler::updateInternalData)
4545
.def("updateJacobiansMassMatrix", &RobotDataHandler::updateJacobiansMassMatrix)
46-
.def("getRefFootPose", &RobotDataHandler::getRefFootPose, bp::return_internal_reference<>())
47-
.def("getFootPose", &RobotDataHandler::getFootPose, bp::return_internal_reference<>())
46+
// .def("getRefFootPose", &RobotDataHandler::getRefFootPose, bp::return_internal_reference<>())
47+
// .def("getFootPose", &RobotDataHandler::getFootPose, bp::return_internal_reference<>())
4848
.def("getBaseFramePose", &RobotDataHandler::getBaseFramePose, bp::return_internal_reference<>())
4949
.def("getModelHandler", &RobotDataHandler::getModelHandler, bp::return_internal_reference<>())
5050
.def("getData", &RobotDataHandler::getData, bp::return_internal_reference<>())

‎include/simple-mpc/inverse-dynamics.hpp‎

Lines changed: 60 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -26,10 +26,23 @@ namespace simple_mpc
2626
return *this; \
2727
}
2828

29+
// Physical quantities
30+
DEFINE_FIELD(double, friction_coefficient, 0.3)
31+
DEFINE_FIELD(
32+
double,
33+
contact_weight_ratio_max,
34+
2.0) // Max force for one foot contact (express as a multiple of the robot weight)
35+
DEFINE_FIELD(
36+
double,
37+
contact_weight_ratio_min,
38+
0.01) // Min force for one foot contact (express as a multiple of the robot weight)
39+
40+
// Tasks gains
2941
DEFINE_FIELD(double, kp_posture, 1.0)
3042
DEFINE_FIELD(double, kp_base, 0.0)
3143
DEFINE_FIELD(double, kp_contact, 0.0)
3244

45+
// Tasks weights
3346
DEFINE_FIELD(double, w_posture, 1e2)
3447
DEFINE_FIELD(double, w_base, 0.)
3548
DEFINE_FIELD(double, w_contact_force, 0.)
@@ -56,17 +69,24 @@ namespace simple_mpc
5669
formulation_.computeProblemData(0, model_handler.getReferenceState().head(nq), Eigen::VectorXd::Zero(nv));
5770

5871
// Prepare contact point task
59-
// const Eigen::Vector3d normal {0, 0, 1};
60-
// for (std::string foot_name : model_handler_.getFeetNames()) {
61-
// std::shared_ptr<tsid::contacts::ContactPoint> cp =
62-
// std::make_shared<tsid::contacts::ContactPoint>(foot_name, robot_, foot_name, normal, 0.3, 1.0, 100.);
63-
// cp->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
64-
// cp->Kd(2.0 * cp->Kp().cwiseSqrt());
65-
// cp->setReference(data_handler_.getFootPose(foot_name));
66-
// cp->useLocalFrame(false);
67-
// formulation_.addRigidContact(*cp, settings_.w_constact_force_, settings_.w_contact_motion_, 0);
68-
// contactPoints_.push_back(cp);
69-
// }
72+
const size_t n_contacts = model_handler_.getFeetNames().size();
73+
const Eigen::Vector3d normal{0, 0, 1};
74+
const double weight = model_handler_.getMass() * 9.81;
75+
const double max_f = settings_.contact_weight_ratio_max * weight;
76+
const double min_f = settings_.contact_weight_ratio_min * weight;
77+
for (int i = 0; i < n_contacts; i++)
78+
{
79+
const std::string frame_name = model_handler.getFootName(i);
80+
// Create contact point
81+
tsid::contacts::ContactPoint & contact_point = tsid_contacts.emplace_back(
82+
frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
83+
// Set contact parameters
84+
contact_point.Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
85+
contact_point.Kd(2.0 * contact_point.Kp().cwiseSqrt());
86+
contact_point.useLocalFrame(false);
87+
// By default contact is not active (will be by setTarget)
88+
active_tsid_contacts_.push_back(false);
89+
}
7090

7191
// Add the posture task
7292
postureTask_ = std::make_shared<tsid::tasks::TaskJointPosture>("task-posture", robot_);
@@ -100,38 +120,47 @@ namespace simple_mpc
100120
const Eigen::VectorXd & q_target,
101121
const Eigen::VectorXd & v_target,
102122
const Eigen::VectorXd & a_target,
103-
const std::vector<bool> & contact_state,
123+
const std::vector<bool> & contact_state_target,
104124
const Eigen::VectorXd & f_target)
105125
{
126+
// Posture task
106127
samplePosture_.setValue(q_target.tail(robot_.nq_actuated()));
107128
samplePosture_.setDerivative(v_target.tail(robot_.na()));
108129
samplePosture_.setSecondDerivative(a_target.tail(robot_.na()));
109130
postureTask_->setReference(samplePosture_);
110131

132+
// Base task
111133
pose_base_.rotation() = pinocchio::SE3::Quaternion(q_target[3], q_target[4], q_target[5], q_target[6]).matrix();
112134
pose_base_.translation() = q_target.head(3);
113135
tsid::math::SE3ToVector(pose_base_, sampleBase_.pos);
114136
sampleBase_.setDerivative(v_target.head(6));
115137
sampleBase_.setSecondDerivative(a_target.head(6));
116138
baseTask_->setReference(sampleBase_);
117139

118-
// for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i ++) {
119-
// std::string name = model_handler_.getFeetNames()[i];
120-
// if (contact_state[i]) {
121-
// if(!check_contact(name)) {
122-
// // formulation_.addRigidContact(*contactPoints_[i], w_constact_force_, w_contact_motion_, 0);
123-
// }
124-
// else {
125-
// contactPoints_[i]->setForceReference(forces.segment(i * 3, 3));
126-
// contactPoints_[i]->setReference(robot_->framePosition(data, robot_->model().getFrameId(name)));
127-
// }
128-
// }
129-
// else {
130-
// if (check_contact(name)) {
131-
// // formulation_.removeRigidContact(name, 0);
132-
// }
133-
// }
134-
// }
140+
// Foot contacts
141+
data_handler_.updateInternalData(q_target, v_target, false);
142+
for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i++)
143+
{
144+
std::string name = model_handler_.getFeetNames()[i];
145+
if (contact_state_target[i])
146+
{
147+
if (!active_tsid_contacts_[i])
148+
{
149+
formulation_.addRigidContact(tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion, 1);
150+
}
151+
tsid_contacts[i].setForceReference(f_target.segment(i * 3, 3));
152+
tsid_contacts[i].setReference(data_handler_.getFootPose(i));
153+
active_tsid_contacts_[i] = true;
154+
}
155+
else
156+
{
157+
if (active_tsid_contacts_[i])
158+
{
159+
formulation_.removeRigidContact(name, 0);
160+
active_tsid_contacts_[i] = false;
161+
}
162+
}
163+
}
135164
}
136165

137166
bool check_contact(std::string & name)
@@ -167,7 +196,8 @@ namespace simple_mpc
167196
tsid::robots::RobotWrapper robot_;
168197
tsid::InverseDynamicsFormulationAccForce formulation_;
169198

170-
std::vector<std::shared_ptr<tsid::contacts::ContactPoint>> contactPoints_;
199+
std::vector<bool> active_tsid_contacts_;
200+
std::vector<tsid::contacts::ContactPoint> tsid_contacts;
171201
std::shared_ptr<tsid::tasks::TaskJointPosture> postureTask_;
172202
std::shared_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
173203
tsid::solvers::SolverHQPBase * solver_;

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

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -175,17 +175,26 @@ namespace simple_mpc
175175
RobotDataHandler(const RobotModelHandler & model_handler);
176176

177177
// Set new robot state
178+
void updateInternalData(const ConstVectorRef & q, const ConstVectorRef & v, const bool updateJacobians);
178179
void updateInternalData(const ConstVectorRef & x, const bool updateJacobians);
179180
void updateJacobiansMassMatrix(const ConstVectorRef & x);
180181

181182
// Const getters
182183
const SE3 & getRefFootPose(const std::string & foot_name) const
183184
{
184-
return data_.oMf[model_handler_.getRefFootId(foot_name)];
185+
return getRefFootPose(model_handler_.getRefFootId(foot_name));
186+
};
187+
const SE3 & getRefFootPose(size_t foot_id) const
188+
{
189+
return data_.oMf[foot_id];
185190
};
186191
const SE3 & getFootPose(const std::string & foot_name) const
187192
{
188-
return data_.oMf[model_handler_.getFootId(foot_name)];
193+
return getFootPose(model_handler_.getFootId(foot_name));
194+
};
195+
const SE3 & getFootPose(size_t foot_id) const
196+
{
197+
return data_.oMf[foot_id];
189198
};
190199
const SE3 & getBaseFramePose() const
191200
{

‎src/robot-handler.cpp‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,7 @@ namespace simple_mpc
7777
RobotDataHandler::RobotDataHandler(const RobotModelHandler & model_handler)
7878
: model_handler_(model_handler)
7979
, data_(model_handler.getModel())
80+
, x_(model_handler.getReferenceState().size())
8081
{
8182
updateInternalData(model_handler.getReferenceState(), true);
8283
}
@@ -85,15 +86,22 @@ namespace simple_mpc
8586
{
8687
const Eigen::Block q = x.head(model_handler_.getModel().nq);
8788
const Eigen::Block v = x.tail(model_handler_.getModel().nv);
88-
x_ = x;
89+
90+
updateInternalData(q, v, updateJacobians);
91+
}
92+
93+
void
94+
RobotDataHandler::updateInternalData(const ConstVectorRef & q, const ConstVectorRef & v, const bool updateJacobians)
95+
{
96+
x_ << q, v;
8997

9098
forwardKinematics(model_handler_.getModel(), data_, q);
9199
updateFramePlacements(model_handler_.getModel(), data_);
92100
computeCentroidalMomentum(model_handler_.getModel(), data_, q, v);
93101

94102
if (updateJacobians)
95103
{
96-
updateJacobiansMassMatrix(x);
104+
updateJacobiansMassMatrix(x_);
97105
}
98106
}
99107

0 commit comments

Comments
 (0)