Skip to content

Commit 8a73a0d

Browse files
author
earlaud
committed
Add Pos Vel joint bounds
1 parent e409e99 commit 8a73a0d

File tree

2 files changed

+45
-29
lines changed

2 files changed

+45
-29
lines changed

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

Lines changed: 15 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include <tsid/solvers/solver-HQP-factory.hxx>
77
#include <tsid/solvers/utils.hpp>
88
#include <tsid/tasks/task-joint-bounds.hpp>
9+
#include <tsid/tasks/task-joint-posVelAcc-bounds.hpp>
910
#include <tsid/tasks/task-joint-posture.hpp>
1011
#include <tsid/tasks/task-se3-equality.hpp>
1112
#include <tsid/trajectories/trajectory-euclidian.hpp>
@@ -51,7 +52,7 @@ namespace simple_mpc
5152
DEFINE_FIELD(double, w_contact_force, -1.) // Disabled by default
5253
};
5354

54-
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, const Settings settings)
55+
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings)
5556
: settings_(settings)
5657
, model_handler_(model_handler)
5758
, data_handler_(model_handler_)
@@ -60,6 +61,7 @@ namespace simple_mpc
6061
{
6162
const pinocchio::Model & model = model_handler.getModel();
6263
const size_t nq = model.nq;
64+
const size_t nq_actuated = robot_.nq_actuated();
6365
const size_t nv = model.nv;
6466
const size_t nu = nv - 6;
6567

@@ -90,7 +92,7 @@ namespace simple_mpc
9092
if (settings_.w_posture > 0.)
9193
formulation_.addMotionTask(*postureTask_, settings_.w_posture, 1);
9294

93-
samplePosture_ = tsid::trajectories::TrajectorySample(robot_.nq_actuated(), robot_.na());
95+
samplePosture_ = tsid::trajectories::TrajectorySample(nq_actuated, nu);
9496

9597
// Add the base task
9698
baseTask_ =
@@ -102,6 +104,16 @@ namespace simple_mpc
102104

103105
sampleBase_ = tsid::trajectories::TrajectorySample(12, 6);
104106

107+
// Add joint limit task
108+
boundsTask_ = std::make_shared<tsid::tasks::TaskJointPosVelAccBounds>("task-joint-limits", robot_, control_dt);
109+
boundsTask_->setPositionBounds(
110+
model_handler_.getModel().lowerPositionLimit.tail(nq_actuated),
111+
model_handler_.getModel().upperPositionLimit.tail(nq_actuated));
112+
boundsTask_->setVelocityBounds(model_handler_.getModel().upperVelocityLimit.tail(nu));
113+
boundsTask_->setImposeBounds(
114+
true, true, true, false); // For now do not impose acceleration bound as it is not provided in URDF
115+
formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint
116+
105117
// Create an HQP solver
106118
solver_ = tsid::solvers::SolverHQPFactory::createNewSolver(tsid::solvers::SOLVER_HQP_PROXQP, "solver-proxqp");
107119
solver_->resize(formulation_.nVar(), formulation_.nEq(), formulation_.nIn());
@@ -203,6 +215,7 @@ namespace simple_mpc
203215
std::vector<tsid::contacts::ContactPoint> tsid_contacts;
204216
std::shared_ptr<tsid::tasks::TaskJointPosture> postureTask_;
205217
std::shared_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
218+
std::shared_ptr<tsid::tasks::TaskJointPosVelAccBounds> boundsTask_;
206219
tsid::solvers::SolverHQPBase * solver_;
207220
tsid::solvers::HQPOutput last_solution_;
208221
tsid::trajectories::TrajectorySample samplePosture_; // TODO: no need to store it

‎tests/inverse_dynamics.cpp‎

Lines changed: 30 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,10 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
2929
{
3030
RobotModelHandler model_handler = getSoloHandler();
3131
RobotDataHandler data_handler(model_handler);
32+
const double dt = 1e-3;
3233

3334
KinodynamicsID solver(
34-
model_handler, KinodynamicsID::Settings().set_kp_posture(10.).set_w_posture(1.)); // only a posture task
35+
model_handler, dt, KinodynamicsID::Settings().set_kp_posture(10.).set_w_posture(1.)); // only a posture task
3536

3637
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
3738

@@ -40,7 +41,6 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
4041
{false, false, false, false}, Eigen::VectorXd::Zero(4 * 3));
4142

4243
double t = 0;
43-
double dt = 1e-3;
4444
Eigen::VectorXd q = solo_q_start(model_handler);
4545
Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv);
4646
Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv);
@@ -74,16 +74,18 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_contact)
7474
{
7575
RobotModelHandler model_handler = getSoloHandler();
7676
RobotDataHandler data_handler(model_handler);
77+
const double dt = 1e-3;
7778

7879
KinodynamicsID solver(
79-
model_handler, KinodynamicsID::Settings()
80-
.set_kp_base(10.)
81-
.set_kp_posture(10.0)
82-
.set_kp_contact(10.0)
83-
.set_w_base(10.)
84-
.set_w_posture(1.0)
85-
.set_w_contact_motion(1.0)
86-
.set_w_contact_force(1.0));
80+
model_handler, dt,
81+
KinodynamicsID::Settings()
82+
.set_kp_base(10.)
83+
.set_kp_posture(10.0)
84+
.set_kp_contact(10.0)
85+
.set_w_base(10.)
86+
.set_w_posture(1.0)
87+
.set_w_contact_motion(1.0)
88+
.set_w_contact_force(1.0));
8789

8890
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
8991
Eigen::VectorXd f_target = Eigen::VectorXd::Zero(4 * 3);
@@ -97,7 +99,6 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_contact)
9799
{true, true, true, true}, f_target);
98100

99101
double t = 0;
100-
double dt = 1e-3;
101102
Eigen::VectorXd q = solo_q_start(model_handler);
102103
Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv);
103104
Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv);
@@ -139,20 +140,21 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
139140
{
140141
RobotModelHandler model_handler = getSoloHandler();
141142
RobotDataHandler data_handler(model_handler);
143+
const double dt = 1e-3;
142144

143145
KinodynamicsID solver(
144-
model_handler, KinodynamicsID::Settings()
145-
.set_kp_base(7.)
146-
.set_kp_contact(10.0)
147-
.set_w_base(100.0)
148-
.set_w_contact_force(1.0)
149-
.set_w_contact_motion(1.0));
146+
model_handler, dt,
147+
KinodynamicsID::Settings()
148+
.set_kp_base(7.)
149+
.set_kp_contact(10.0)
150+
.set_w_base(100.0)
151+
.set_w_contact_force(1.0)
152+
.set_w_contact_motion(1.0));
150153

151154
// No need to set target as KinodynamicsID sets it by default to reference state
152155
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
153156

154157
double t = 0;
155-
double dt = 1e-3;
156158
Eigen::VectorXd q = solo_q_start(model_handler);
157159
Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv);
158160
Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv);
@@ -187,16 +189,18 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
187189
{
188190
RobotModelHandler model_handler = getSoloHandler();
189191
RobotDataHandler data_handler(model_handler);
192+
const double dt = 1e-3;
190193

191194
KinodynamicsID solver(
192-
model_handler, KinodynamicsID::Settings()
193-
.set_kp_base(7.)
194-
.set_kp_posture(10.)
195-
.set_kp_contact(10.0)
196-
.set_w_base(100.0)
197-
.set_w_posture(1.0)
198-
.set_w_contact_force(1.0)
199-
.set_w_contact_motion(1.0));
195+
model_handler, dt,
196+
KinodynamicsID::Settings()
197+
.set_kp_base(7.)
198+
.set_kp_posture(10.)
199+
.set_kp_contact(10.0)
200+
.set_w_base(100.0)
201+
.set_w_posture(1.0)
202+
.set_w_contact_force(1.0)
203+
.set_w_contact_motion(1.0));
200204

201205
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
202206
Eigen::VectorXd f_target = Eigen::VectorXd::Zero(4 * 3);
@@ -210,7 +214,6 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
210214
{true, true, true, true}, f_target);
211215

212216
double t = 0;
213-
double dt = 1e-3;
214217
Eigen::VectorXd q = solo_q_start(model_handler);
215218
Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv);
216219
Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv);

0 commit comments

Comments
 (0)