Skip to content

Commit 6d2edb3

Browse files
EtienneArEtienneAr
committed
Make tsid task unique_pointer
1 parent 2d18953 commit 6d2edb3

File tree

7 files changed

+27
-27
lines changed

7 files changed

+27
-27
lines changed

‎bindings/expose-inverse-dynamics.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ namespace simple_mpc
7272
.def_readwrite("w_contact_force", &KinodynamicsID::Settings::w_contact_force)
7373
.def_readwrite("contact_motion_equality", &KinodynamicsID::Settings::contact_motion_equality);
7474

75-
bp::class_<KinodynamicsID>(
75+
bp::class_<KinodynamicsID, boost::noncopyable>(
7676
"KinodynamicsID", bp::init<const simple_mpc::RobotModelHandler &, double, const KinodynamicsID::Settings>(
7777
bp::args("self", "model_handler", "control_dt", "settings")))
7878
.def("setTarget", &KinodynamicsID::setTarget)
@@ -86,7 +86,7 @@ namespace simple_mpc
8686
.def_readwrite("w_com", &CentroidalID::Settings::w_com)
8787
.def_readwrite("w_feet_tracking", &CentroidalID::Settings::w_feet_tracking);
8888

89-
bp::class_<CentroidalID>(
89+
bp::class_<CentroidalID, boost::noncopyable>(
9090
"CentroidalID", bp::init<const simple_mpc::RobotModelHandler &, double, const CentroidalID::Settings>(
9191
bp::args("self", "model_handler", "control_dt", "settings")))
9292
.def("setTarget", &setTarget_CentroidalID)

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ namespace simple_mpc
2626
};
2727

2828
CentroidalID(const RobotModelHandler & model_handler, double control_dt, const Settings settings);
29+
CentroidalID(const CentroidalID &) = delete;
2930

3031
void setTarget(
3132
const Eigen::Ref<const Eigen::Vector<double, 3>> & com_position,
@@ -47,7 +48,7 @@ namespace simple_mpc
4748
const Settings settings_;
4849

4950
private:
50-
std::shared_ptr<tsid::tasks::TaskComEquality> comTask_;
51+
std::unique_ptr<tsid::tasks::TaskComEquality> comTask_;
5152
std::vector<tsid::tasks::TaskSE3Equality> trackingTasks_;
5253
tsid::trajectories::TrajectorySample sampleCom_;
5354
std::vector<tsid::trajectories::TrajectorySample> trackingSamples_;

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

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ namespace simple_mpc
4747
};
4848

4949
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings);
50+
KinodynamicsID(const KinodynamicsID &) = delete;
5051

5152
void setTarget(
5253
const Eigen::Ref<const Eigen::VectorXd> & q_target,
@@ -75,11 +76,11 @@ namespace simple_mpc
7576
tsid::InverseDynamicsFormulationAccForce formulation_;
7677

7778
std::vector<bool> active_tsid_contacts_;
78-
std::vector<std::shared_ptr<tsid::contacts::ContactBase>> tsid_contacts;
79-
std::shared_ptr<tsid::tasks::TaskJointPosture> postureTask_;
80-
std::shared_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
81-
std::shared_ptr<tsid::tasks::TaskJointPosVelAccBounds> boundsTask_;
82-
std::shared_ptr<tsid::tasks::TaskActuationBounds> actuationTask_;
79+
std::vector<std::unique_ptr<tsid::contacts::ContactBase>> tsid_contacts;
80+
std::unique_ptr<tsid::tasks::TaskJointPosture> postureTask_;
81+
std::unique_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
82+
std::unique_ptr<tsid::tasks::TaskJointPosVelAccBounds> boundsTask_;
83+
std::unique_ptr<tsid::tasks::TaskActuationBounds> actuationTask_;
8384
tsid::solvers::SolverProxQP solver_;
8485
tsid::solvers::HQPOutput last_solution_;
8586
tsid::trajectories::TrajectorySample samplePosture_;

‎src/inverse-dynamics/centroidal.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ namespace simple_mpc
2020
}
2121

2222
// Add the center of mass task
23-
comTask_ = std::make_shared<tsid::tasks::TaskComEquality>("task-com", robot_);
23+
comTask_ = std::make_unique<tsid::tasks::TaskComEquality>("task-com", robot_);
2424
comTask_->Kp(settings_.kp_com * Eigen::VectorXd::Ones(3));
2525
comTask_->Kd(2.0 * comTask_->Kp().cwiseSqrt());
2626
if (settings_.w_com > 0.)

‎src/inverse-dynamics/kinodynamics.cpp‎

Lines changed: 14 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -30,21 +30,21 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
3030
switch (model_handler.getFootType(i))
3131
{
3232
case RobotModelHandler::FootType::POINT: {
33-
auto contact_point = std::make_shared<tsid::contacts::ContactPoint>(
33+
auto contact_point = new tsid::contacts::ContactPoint(
3434
frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
3535
contact_point->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
3636
contact_point->Kd(2.0 * contact_point->Kp().cwiseSqrt());
3737
contact_point->useLocalFrame(false);
38-
tsid_contacts.push_back(contact_point);
38+
tsid_contacts.emplace_back(contact_point);
3939
break;
4040
}
4141
case RobotModelHandler::FootType::QUAD: {
42-
auto contact_6D = std::make_shared<tsid::contacts::Contact6d>(
42+
auto contact_6D = new tsid::contacts::Contact6d(
4343
frame_name, robot_, frame_name, model_handler_.getQuadFootContactPoints(i).transpose(), normal,
4444
settings_.friction_coefficient, min_f, max_f);
4545
contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(6));
4646
contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt());
47-
tsid_contacts.push_back(contact_6D);
47+
tsid_contacts.emplace_back(contact_6D);
4848
break;
4949
}
5050
default: {
@@ -56,7 +56,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
5656
}
5757

5858
// Add the posture task
59-
postureTask_ = std::make_shared<tsid::tasks::TaskJointPosture>("task-posture", robot_);
59+
postureTask_ = std::make_unique<tsid::tasks::TaskJointPosture>("task-posture", robot_);
6060
postureTask_->Kp(settings_.kp_posture * Eigen::VectorXd::Ones(nu));
6161
postureTask_->Kd(2.0 * postureTask_->Kp().cwiseSqrt());
6262
if (settings_.w_posture > 0.)
@@ -65,7 +65,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
6565
samplePosture_.resize(nq_actuated, nu);
6666

6767
// Add the base task
68-
baseTask_ = std::make_shared<tsid::tasks::TaskSE3Equality>("task-base", robot_, model_handler_.getBaseFrameName());
68+
baseTask_ = std::make_unique<tsid::tasks::TaskSE3Equality>("task-base", robot_, model_handler_.getBaseFrameName());
6969
baseTask_->Kp(settings_.kp_base * Eigen::VectorXd::Ones(6));
7070
baseTask_->Kd(2.0 * baseTask_->Kp().cwiseSqrt());
7171
if (settings_.w_base > 0.)
@@ -74,7 +74,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
7474
sampleBase_.resize(12, 6);
7575

7676
// Add joint limit task
77-
boundsTask_ = std::make_shared<tsid::tasks::TaskJointPosVelAccBounds>("task-joint-limits", robot_, control_dt);
77+
boundsTask_ = std::make_unique<tsid::tasks::TaskJointPosVelAccBounds>("task-joint-limits", robot_, control_dt);
7878
boundsTask_->setPositionBounds(
7979
model_handler_.getModel().lowerPositionLimit.tail(nq_actuated),
8080
model_handler_.getModel().upperPositionLimit.tail(nq_actuated));
@@ -84,7 +84,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
8484
formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint
8585

8686
// Add actuation limit task
87-
actuationTask_ = std::make_shared<tsid::tasks::TaskActuationBounds>("actuation-limits", robot_);
87+
actuationTask_ = std::make_unique<tsid::tasks::TaskActuationBounds>("actuation-limits", robot_);
8888
actuationTask_->setBounds(
8989
-model_handler_.getModel().effortLimit.tail(nu), model_handler_.getModel().effortLimit.tail(nu));
9090
formulation_.addActuationTask(*actuationTask_, 1.0, 0); // No weight needed as it is set as constraint
@@ -157,14 +157,12 @@ void KinodynamicsID::setTarget(
157157
{
158158
case RobotModelHandler::FootType::POINT: {
159159
assert(f_target.at(foot_nb).size() == 3);
160-
std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[foot_nb])
161-
->setForceReference(f_target.at(foot_nb));
160+
static_cast<tsid::contacts::ContactPoint &>(*tsid_contacts[foot_nb]).setForceReference(f_target.at(foot_nb));
162161
break;
163162
}
164163
case RobotModelHandler::FootType::QUAD: {
165164
assert(f_target.at(foot_nb).size() == 6);
166-
std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[foot_nb])
167-
->setForceReference(f_target.at(foot_nb));
165+
static_cast<tsid::contacts::Contact6d &>(*tsid_contacts[foot_nb]).setForceReference(f_target.at(foot_nb));
168166
break;
169167
}
170168
default: {
@@ -200,13 +198,13 @@ void KinodynamicsID::solve(
200198
switch (model_handler_.getFootType(foot_nb))
201199
{
202200
case RobotModelHandler::FootType::POINT: {
203-
std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[foot_nb])
204-
->setReference(data_handler_.getFootPose(foot_nb));
201+
static_cast<tsid::contacts::ContactPoint &>(*tsid_contacts[foot_nb])
202+
.setReference(data_handler_.getFootPose(foot_nb));
205203
break;
206204
}
207205
case RobotModelHandler::FootType::QUAD: {
208-
std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[foot_nb])
209-
->setReference(data_handler_.getFootPose(foot_nb));
206+
static_cast<tsid::contacts::Contact6d &>(*tsid_contacts[foot_nb])
207+
.setReference(data_handler_.getFootPose(foot_nb));
210208
break;
211209
}
212210
default: {

‎tests/inverse-dynamics/centroidal.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ BOOST_AUTO_TEST_CASE(CentroidalID_postureTask)
151151
}
152152
}
153153

154-
void test_contact(TestCentroidalID test)
154+
void test_contact(TestCentroidalID & test)
155155
{
156156
// Easy access
157157
const RobotModelHandler & model_handler = test.model_handler;

‎tests/inverse-dynamics/kinodynamics.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -143,7 +143,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
143143
}
144144
}
145145

146-
void test_contact(TestKinoID test)
146+
void test_contact(TestKinoID & test)
147147
{
148148
// Easy access
149149
const RobotModelHandler & model_handler = test.model_handler;

0 commit comments

Comments
 (0)