Skip to content

Commit a5b88cc

Browse files
author
earlaud
committed
Split hpp and cpp thanks to eigen::ref
1 parent 38cd655 commit a5b88cc

File tree

2 files changed

+167
-149
lines changed

2 files changed

+167
-149
lines changed

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

Lines changed: 5 additions & 149 deletions
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,10 @@
66
#include <tsid/solvers/solver-HQP-factory.hxx>
77
#include <tsid/solvers/utils.hpp>
88
#include <tsid/tasks/task-actuation-bounds.hpp>
9-
#include <tsid/tasks/task-joint-bounds.hpp>
109
#include <tsid/tasks/task-joint-posVelAcc-bounds.hpp>
1110
#include <tsid/tasks/task-joint-posture.hpp>
1211
#include <tsid/tasks/task-se3-equality.hpp>
13-
#include <tsid/trajectories/trajectory-euclidian.hpp>
12+
#include <tsid/trajectories/trajectory-base.hpp>
1413

1514
// Allow to define a field, it's default value and its convenient chainable setter in one keyword.
1615
#define DEFINE_FIELD(type, name, value) \
@@ -53,165 +52,22 @@ namespace simple_mpc
5352
DEFINE_FIELD(double, w_contact_force, -1.) // Disabled by default
5453
};
5554

56-
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings)
57-
: settings_(settings)
58-
, model_handler_(model_handler)
59-
, data_handler_(model_handler_)
60-
, robot_(model_handler_.getModel())
61-
, formulation_("tsid", robot_)
62-
{
63-
const pinocchio::Model & model = model_handler.getModel();
64-
const size_t nq = model.nq;
65-
const size_t nq_actuated = robot_.nq_actuated();
66-
const size_t nv = model.nv;
67-
const size_t nu = nv - 6;
68-
69-
// Prepare contact point task
70-
const size_t n_contacts = model_handler_.getFeetNames().size();
71-
const Eigen::Vector3d normal{0, 0, 1};
72-
const double weight = model_handler_.getMass() * 9.81;
73-
const double max_f = settings_.contact_weight_ratio_max * weight;
74-
const double min_f = settings_.contact_weight_ratio_min * weight;
75-
for (int i = 0; i < n_contacts; i++)
76-
{
77-
const std::string frame_name = model_handler.getFootName(i);
78-
// Create contact point
79-
tsid::contacts::ContactPoint & contact_point = tsid_contacts.emplace_back(
80-
frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
81-
// Set contact parameters
82-
contact_point.Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
83-
contact_point.Kd(2.0 * contact_point.Kp().cwiseSqrt());
84-
contact_point.useLocalFrame(false);
85-
// By default contact is not active (will be by setTarget)
86-
active_tsid_contacts_.push_back(false);
87-
}
88-
89-
// Add the posture task
90-
postureTask_ = std::make_shared<tsid::tasks::TaskJointPosture>("task-posture", robot_);
91-
postureTask_->Kp(settings_.kp_posture * Eigen::VectorXd::Ones(nu));
92-
postureTask_->Kd(2.0 * postureTask_->Kp().cwiseSqrt());
93-
if (settings_.w_posture > 0.)
94-
formulation_.addMotionTask(*postureTask_, settings_.w_posture, 1);
95-
96-
samplePosture_ = tsid::trajectories::TrajectorySample(nq_actuated, nu);
97-
98-
// Add the base task
99-
baseTask_ =
100-
std::make_shared<tsid::tasks::TaskSE3Equality>("task-base", robot_, model_handler_.getBaseFrameName());
101-
baseTask_->Kp(settings_.kp_base * Eigen::VectorXd::Ones(6));
102-
baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt());
103-
if (settings_.w_base > 0.)
104-
formulation_.addMotionTask(*baseTask_, settings_.w_base, 1);
105-
106-
sampleBase_ = tsid::trajectories::TrajectorySample(12, 6);
107-
108-
// Add joint limit task
109-
boundsTask_ = std::make_shared<tsid::tasks::TaskJointPosVelAccBounds>("task-joint-limits", robot_, control_dt);
110-
boundsTask_->setPositionBounds(
111-
model_handler_.getModel().lowerPositionLimit.tail(nq_actuated),
112-
model_handler_.getModel().upperPositionLimit.tail(nq_actuated));
113-
boundsTask_->setVelocityBounds(model_handler_.getModel().upperVelocityLimit.tail(nu));
114-
boundsTask_->setImposeBounds(
115-
true, true, true, false); // For now do not impose acceleration bound as it is not provided in URDF
116-
formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint
117-
118-
// Add actuation limit task
119-
actuationTask_ = std::make_shared<tsid::tasks::TaskActuationBounds>("actuation-limits", robot_);
120-
actuationTask_->setBounds(
121-
model_handler_.getModel().lowerEffortLimit.tail(nu), model_handler_.getModel().upperEffortLimit.tail(nu));
122-
formulation_.addActuationTask(*actuationTask_, 1.0, 0); // No weight needed as it is set as constraint
123-
124-
// Create an HQP solver
125-
solver_ = tsid::solvers::SolverHQPFactory::createNewSolver(tsid::solvers::SOLVER_HQP_PROXQP, "solver-proxqp");
126-
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
127-
128-
// Dry run to initialize solver data & output
129-
const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq);
130-
const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv);
131-
std::vector<bool> c_ref(n_contacts);
132-
Eigen::VectorXd f_ref = Eigen::VectorXd::Zero(3 * n_contacts);
133-
for (int i = 0; i < n_contacts; i++)
134-
{
135-
c_ref[i] = true;
136-
f_ref[2 + 3 * i] = weight / n_contacts;
137-
}
138-
setTarget(q_ref, v_ref, v_ref, c_ref, f_ref);
139-
const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref);
140-
last_solution_ = solver_->solve(solver_data);
141-
}
55+
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings);
14256

14357
void setTarget(
14458
const Eigen::Ref<const Eigen::VectorXd> & q_target,
14559
const Eigen::Ref<const Eigen::VectorXd> & v_target,
14660
const Eigen::Ref<const Eigen::VectorXd> & a_target,
14761
const std::vector<bool> & contact_state_target,
148-
const Eigen::Ref<const Eigen::VectorXd> & f_target)
149-
{
150-
data_handler_.updateInternalData(q_target, v_target, false);
151-
152-
// Posture task
153-
samplePosture_.setValue(q_target.tail(robot_.nq_actuated()));
154-
samplePosture_.setDerivative(v_target.tail(robot_.na()));
155-
samplePosture_.setSecondDerivative(a_target.tail(robot_.na()));
156-
postureTask_->setReference(samplePosture_);
157-
158-
// Base task
159-
tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos);
160-
sampleBase_.setDerivative(v_target.head<6>());
161-
sampleBase_.setSecondDerivative(a_target.head<6>());
162-
baseTask_->setReference(sampleBase_);
163-
164-
// Foot contacts
165-
for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i++)
166-
{
167-
const std::string name = model_handler_.getFeetNames()[i];
168-
if (contact_state_target[i])
169-
{
170-
if (!active_tsid_contacts_[i])
171-
{
172-
formulation_.addRigidContact(tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion, 1);
173-
}
174-
tsid_contacts[i].setForceReference(f_target.segment(i * 3, 3));
175-
active_tsid_contacts_[i] = true;
176-
}
177-
else
178-
{
179-
if (active_tsid_contacts_[i])
180-
{
181-
formulation_.removeRigidContact(name, 0);
182-
active_tsid_contacts_[i] = false;
183-
}
184-
}
185-
}
186-
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
187-
}
62+
const Eigen::Ref<const Eigen::VectorXd> & f_target);
18863

18964
void solve(
19065
const double t,
19166
const Eigen::Ref<const Eigen::VectorXd> & q_meas,
19267
const Eigen::Ref<const Eigen::VectorXd> & v_meas,
193-
Eigen::Ref<Eigen::VectorXd> tau_res)
194-
{
195-
// Update contact position based on the real robot foot placement
196-
data_handler_.updateInternalData(q_meas, v_meas, false);
197-
for (std::size_t i = 0; i < active_tsid_contacts_.size(); i++)
198-
{
199-
if (active_tsid_contacts_[i])
200-
{
201-
tsid_contacts[i].setReference(data_handler_.getFootPose(i));
202-
}
203-
}
68+
Eigen::Ref<Eigen::VectorXd> tau_res);
20469

205-
const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(t, q_meas, v_meas);
206-
last_solution_ = solver_->solve(solver_data);
207-
assert(last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL);
208-
tau_res = formulation_.getActuatorForces(last_solution_);
209-
}
210-
211-
const Eigen::VectorXd & getAccelerations()
212-
{
213-
return formulation_.getAccelerations(last_solution_);
214-
}
70+
const Eigen::VectorXd & getAccelerations();
21571

21672
private:
21773
// Order matters to be instanciated in the right order

‎src/inverse-dynamics.cpp‎

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
#include <simple-mpc/inverse-dynamics.hpp>
2+
3+
using namespace simple_mpc;
4+
5+
KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double control_dt, const Settings settings)
6+
: settings_(settings)
7+
, model_handler_(model_handler)
8+
, data_handler_(model_handler_)
9+
, robot_(model_handler_.getModel())
10+
, formulation_("tsid", robot_)
11+
{
12+
const pinocchio::Model & model = model_handler.getModel();
13+
const size_t nq = model.nq;
14+
const size_t nq_actuated = robot_.nq_actuated();
15+
const size_t nv = model.nv;
16+
const size_t nu = nv - 6;
17+
18+
// Prepare contact point task
19+
const size_t n_contacts = model_handler_.getFeetNames().size();
20+
const Eigen::Vector3d normal{0, 0, 1};
21+
const double weight = model_handler_.getMass() * 9.81;
22+
const double max_f = settings_.contact_weight_ratio_max * weight;
23+
const double min_f = settings_.contact_weight_ratio_min * weight;
24+
for (int i = 0; i < n_contacts; i++)
25+
{
26+
const std::string frame_name = model_handler.getFootName(i);
27+
// Create contact point
28+
tsid::contacts::ContactPoint & contact_point =
29+
tsid_contacts.emplace_back(frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
30+
// Set contact parameters
31+
contact_point.Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
32+
contact_point.Kd(2.0 * contact_point.Kp().cwiseSqrt());
33+
contact_point.useLocalFrame(false);
34+
// By default contact is not active (will be by setTarget)
35+
active_tsid_contacts_.push_back(false);
36+
}
37+
38+
// Add the posture task
39+
postureTask_ = std::make_shared<tsid::tasks::TaskJointPosture>("task-posture", robot_);
40+
postureTask_->Kp(settings_.kp_posture * Eigen::VectorXd::Ones(nu));
41+
postureTask_->Kd(2.0 * postureTask_->Kp().cwiseSqrt());
42+
if (settings_.w_posture > 0.)
43+
formulation_.addMotionTask(*postureTask_, settings_.w_posture, 1);
44+
45+
samplePosture_ = tsid::trajectories::TrajectorySample(nq_actuated, nu);
46+
47+
// Add the base task
48+
baseTask_ = std::make_shared<tsid::tasks::TaskSE3Equality>("task-base", robot_, model_handler_.getBaseFrameName());
49+
baseTask_->Kp(settings_.kp_base * Eigen::VectorXd::Ones(6));
50+
baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt());
51+
if (settings_.w_base > 0.)
52+
formulation_.addMotionTask(*baseTask_, settings_.w_base, 1);
53+
54+
sampleBase_ = tsid::trajectories::TrajectorySample(12, 6);
55+
56+
// Add joint limit task
57+
boundsTask_ = std::make_shared<tsid::tasks::TaskJointPosVelAccBounds>("task-joint-limits", robot_, control_dt);
58+
boundsTask_->setPositionBounds(
59+
model_handler_.getModel().lowerPositionLimit.tail(nq_actuated),
60+
model_handler_.getModel().upperPositionLimit.tail(nq_actuated));
61+
boundsTask_->setVelocityBounds(model_handler_.getModel().upperVelocityLimit.tail(nu));
62+
boundsTask_->setImposeBounds(
63+
true, true, true, false); // For now do not impose acceleration bound as it is not provided in URDF
64+
formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint
65+
66+
// Add actuation limit task
67+
actuationTask_ = std::make_shared<tsid::tasks::TaskActuationBounds>("actuation-limits", robot_);
68+
actuationTask_->setBounds(
69+
model_handler_.getModel().lowerEffortLimit.tail(nu), model_handler_.getModel().upperEffortLimit.tail(nu));
70+
formulation_.addActuationTask(*actuationTask_, 1.0, 0); // No weight needed as it is set as constraint
71+
72+
// Create an HQP solver
73+
solver_ = tsid::solvers::SolverHQPFactory::createNewSolver(tsid::solvers::SOLVER_HQP_PROXQP, "solver-proxqp");
74+
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
75+
76+
// Dry run to initialize solver data & output
77+
const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq);
78+
const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv);
79+
std::vector<bool> c_ref(n_contacts);
80+
Eigen::VectorXd f_ref = Eigen::VectorXd::Zero(3 * n_contacts);
81+
for (int i = 0; i < n_contacts; i++)
82+
{
83+
c_ref[i] = true;
84+
f_ref[2 + 3 * i] = weight / n_contacts;
85+
}
86+
setTarget(q_ref, v_ref, v_ref, c_ref, f_ref);
87+
const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref);
88+
last_solution_ = solver_->solve(solver_data);
89+
}
90+
91+
void KinodynamicsID::setTarget(
92+
const Eigen::Ref<const Eigen::VectorXd> & q_target,
93+
const Eigen::Ref<const Eigen::VectorXd> & v_target,
94+
const Eigen::Ref<const Eigen::VectorXd> & a_target,
95+
const std::vector<bool> & contact_state_target,
96+
const Eigen::Ref<const Eigen::VectorXd> & f_target)
97+
{
98+
data_handler_.updateInternalData(q_target, v_target, false);
99+
100+
// Posture task
101+
samplePosture_.setValue(q_target.tail(robot_.nq_actuated()));
102+
samplePosture_.setDerivative(v_target.tail(robot_.na()));
103+
samplePosture_.setSecondDerivative(a_target.tail(robot_.na()));
104+
postureTask_->setReference(samplePosture_);
105+
106+
// Base task
107+
tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos);
108+
sampleBase_.setDerivative(v_target.head<6>());
109+
sampleBase_.setSecondDerivative(a_target.head<6>());
110+
baseTask_->setReference(sampleBase_);
111+
112+
// Foot contacts
113+
for (std::size_t i = 0; i < model_handler_.getFeetNames().size(); i++)
114+
{
115+
const std::string name = model_handler_.getFeetNames()[i];
116+
if (contact_state_target[i])
117+
{
118+
if (!active_tsid_contacts_[i])
119+
{
120+
formulation_.addRigidContact(tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion, 1);
121+
}
122+
tsid_contacts[i].setForceReference(f_target.segment(i * 3, 3));
123+
active_tsid_contacts_[i] = true;
124+
}
125+
else
126+
{
127+
if (active_tsid_contacts_[i])
128+
{
129+
formulation_.removeRigidContact(name, 0);
130+
active_tsid_contacts_[i] = false;
131+
}
132+
}
133+
}
134+
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
135+
}
136+
137+
void KinodynamicsID::solve(
138+
const double t,
139+
const Eigen::Ref<const Eigen::VectorXd> & q_meas,
140+
const Eigen::Ref<const Eigen::VectorXd> & v_meas,
141+
Eigen::Ref<Eigen::VectorXd> tau_res)
142+
{
143+
// Update contact position based on the real robot foot placement
144+
data_handler_.updateInternalData(q_meas, v_meas, false);
145+
for (std::size_t i = 0; i < active_tsid_contacts_.size(); i++)
146+
{
147+
if (active_tsid_contacts_[i])
148+
{
149+
tsid_contacts[i].setReference(data_handler_.getFootPose(i));
150+
}
151+
}
152+
153+
const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(t, q_meas, v_meas);
154+
last_solution_ = solver_->solve(solver_data);
155+
assert(last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL);
156+
tau_res = formulation_.getActuatorForces(last_solution_);
157+
}
158+
159+
const Eigen::VectorXd & KinodynamicsID::getAccelerations()
160+
{
161+
return formulation_.getAccelerations(last_solution_);
162+
}

0 commit comments

Comments
 (0)