Skip to content

Commit 5d3d789

Browse files
EtienneArEtienneAr
committed
remove easy initilization of ID Settings and make centroidal test compile
1 parent d4aec23 commit 5d3d789

File tree

5 files changed

+169
-152
lines changed

5 files changed

+169
-152
lines changed

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

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,12 @@ namespace simple_mpc
1717
struct Settings : public KinodynamicsID::Settings
1818
{
1919
// Tasks gains
20-
DEFINE_FIELD(double, kp_com, 0.)
21-
DEFINE_FIELD(double, kp_feet_tracking, 0.)
20+
double kp_com = 0.;
21+
double kp_feet_tracking = 0.;
2222

2323
// Tasks weights
24-
DEFINE_FIELD(double, w_com, -1.) // Disabled by default
25-
DEFINE_FIELD(double, w_feet_tracking, -1.) // Disabled by default
24+
double w_com = -1.; // Disabled by default
25+
double w_feet_tracking = -1.; // Disabled by default
2626
};
2727

2828
CentroidalID(const RobotModelHandler & model_handler, double control_dt, const Settings settings);

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

Lines changed: 15 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,6 @@
1111
#include <tsid/tasks/task-se3-equality.hpp>
1212
#include <tsid/trajectories/trajectory-base.hpp>
1313

14-
// Allow to define a field, it's default value and its convenient chainable setter in one keyword.
15-
#define DEFINE_FIELD(type, name, value) \
16-
type name = value; \
17-
Settings & set_##name(type v) \
18-
{ \
19-
name = v; \
20-
return *this; \
21-
}
22-
2314
namespace simple_mpc
2415
{
2516

@@ -33,29 +24,27 @@ namespace simple_mpc
3324
{
3425

3526
// Physical quantities
36-
DEFINE_FIELD(double, friction_coefficient, 0.6)
37-
DEFINE_FIELD(
38-
double,
39-
contact_weight_ratio_max,
40-
10.0) // Max force for one foot contact (express as a multiple of the robot weight)
41-
DEFINE_FIELD(
42-
double,
43-
contact_weight_ratio_min,
44-
0.01) // Min force for one foot contact (express as a multiple of the robot weight)
27+
double friction_coefficient = 0.6;
28+
29+
double contact_weight_ratio_max =
30+
10.0; // Max force for one foot contact (express as a multiple of the robot weight)
31+
32+
double contact_weight_ratio_min =
33+
0.01; // Min force for one foot contact (express as a multiple of the robot weight)
4534

4635
// Tasks gains
47-
DEFINE_FIELD(double, kp_base, 0.)
48-
DEFINE_FIELD(double, kp_posture, 0.)
49-
DEFINE_FIELD(double, kp_contact, 0.)
36+
double kp_base = 0.;
37+
double kp_posture = 0.;
38+
double kp_contact = 0.;
5039

5140
// Tasks weights
52-
DEFINE_FIELD(double, w_base, -1.) // Disabled by default
53-
DEFINE_FIELD(double, w_posture, -1.) // Disabled by default
54-
DEFINE_FIELD(double, w_contact_motion, -1.) // Disabled by default
55-
DEFINE_FIELD(double, w_contact_force, -1.) // Disabled by default
41+
double w_base = -1.; // Disabled by default
42+
double w_posture = -1.; // Disabled by default
43+
double w_contact_motion = -1.; // Disabled by default
44+
double w_contact_force = -1.; // Disabled by default
5645

5746
///< Are the contact motion = 0 handled as a hard contraint (true) or a cost (if false)
58-
DEFINE_FIELD(bool, contact_motion_equality, false)
47+
bool contact_motion_equality = false;
5948
};
6049

6150
KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings);

‎tests/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ set(
5555
robot_handler
5656
problem
5757
inverse-dynamics/kinodynamics
58+
inverse-dynamics/centroidal
5859
#mpc
5960
friction
6061
interpolator

‎tests/inverse-dynamics/centroidal.cpp‎

Lines changed: 87 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,11 @@
44
#include <pinocchio/algorithm/center-of-mass.hpp>
55
#include <pinocchio/algorithm/frames.hpp>
66

7-
#include "simple-mpc/inverse-dynamics/kinodynamics.hpp"
7+
#include "simple-mpc/inverse-dynamics/centroidal.hpp"
88
#include "simple-mpc/robot-handler.hpp"
99
#include "test_utils.cpp"
1010

11-
BOOST_AUTO_TEST_SUITE(inverse_dynamics)
11+
BOOST_AUTO_TEST_SUITE(inverse_dynamics_centroidal)
1212

1313
using namespace simple_mpc;
1414

@@ -28,10 +28,10 @@ Eigen::VectorXd solo_q_start(const RobotModelHandler & model_handler)
2828
}
2929

3030
// Helper class to create the problem and run it
31-
class TestKinoID
31+
class TestCentroidalID
3232
{
3333
public:
34-
TestKinoID(RobotModelHandler model_handler_, KinodynamicsID::Settings settings_)
34+
TestCentroidalID(RobotModelHandler model_handler_, CentroidalID::Settings settings_)
3535
: model_handler(model_handler_)
3636
, data_handler(model_handler)
3737
, settings(settings_)
@@ -93,9 +93,9 @@ class TestKinoID
9393
public:
9494
const RobotModelHandler model_handler;
9595
RobotDataHandler data_handler;
96-
KinodynamicsID::Settings settings;
96+
CentroidalID::Settings settings;
9797
double dt = 1e-3;
98-
KinodynamicsID solver;
98+
CentroidalID solver;
9999

100100
double t = 0.;
101101
int step_i = 0;
@@ -107,19 +107,31 @@ class TestKinoID
107107
std::map<std::string, double> errors;
108108
};
109109

110-
BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
110+
BOOST_AUTO_TEST_CASE(CentroidalID_postureTask)
111111
{
112-
TestKinoID test(getSoloHandler(), KinodynamicsID::Settings().set_kp_posture(20.).set_w_posture(1.));
112+
CentroidalID::Settings settings;
113+
settings.kp_posture = 20.;
114+
settings.w_posture = 1.;
115+
116+
TestCentroidalID test(getSoloHandler(), settings);
113117

114118
// Easy access
115119
const RobotModelHandler & model_handler = test.model_handler;
116120
const size_t nq = model_handler.getModel().nq;
117121
const size_t nv = model_handler.getModel().nv;
118122

119123
// Target state
120-
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
124+
const Eigen::VectorXd q_target =
125+
model_handler.getReferenceState().head(nq); // CentroidalID set posture target to reference configuration
126+
CentroidalID::FeetPoseVector feet_pose_vec;
127+
CentroidalID::FeetVelocityVector feet_vel_vec;
128+
for (int foot_nb; foot_nb < test.model_handler.getFeetNb(); foot_nb++)
129+
{
130+
feet_pose_vec.push_back(pinocchio::SE3::Identity());
131+
feet_vel_vec.push_back(pinocchio::Motion::Zero());
132+
}
121133
test.solver.setTarget(
122-
q_target, Eigen::VectorXd::Zero(nv), Eigen::VectorXd::Zero(nv), {false, false, false, false}, {});
134+
Eigen::VectorXd::Zero(3), Eigen::VectorXd::Zero(3), feet_pose_vec, feet_vel_vec, {false, false, false, false}, {});
123135

124136
// Change initial state
125137
test.q = solo_q_start(model_handler);
@@ -139,15 +151,15 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
139151
}
140152
}
141153

142-
void test_contact(TestKinoID test)
154+
void test_contact(TestCentroidalID test)
143155
{
144156
// Easy access
145157
const RobotModelHandler & model_handler = test.model_handler;
146158
const RobotDataHandler & data_handler = test.data_handler;
147159
const size_t nq = model_handler.getModel().nq;
148160
const size_t nv = model_handler.getModel().nv;
149161

150-
// No need to set target as KinodynamicsID sets it by default to reference state
162+
// No need to set target as CentroidalID sets it by default to reference state
151163
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
152164

153165
// Let the robot stabilize
@@ -172,77 +184,81 @@ void test_contact(TestKinoID test)
172184
}
173185
}
174186

175-
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_cost)
187+
BOOST_AUTO_TEST_CASE(CentroidalID_contactPoint_cost)
176188
{
177-
TestKinoID simu(
178-
getSoloHandler(), KinodynamicsID::Settings()
179-
.set_kp_base(1.0)
180-
.set_kp_contact(10.0)
181-
.set_w_base(1.)
182-
.set_w_contact_motion(10.0)
183-
.set_w_contact_force(1.0));
189+
CentroidalID::Settings settings;
190+
settings.kp_base = 1.0;
191+
settings.kp_contact = 10.0;
192+
settings.w_base = 1.;
193+
settings.w_contact_motion = 10.0;
194+
settings.w_contact_force = 1.0;
195+
196+
TestCentroidalID simu(getSoloHandler(), settings);
184197
simu.q = solo_q_start(simu.model_handler); // Set initial configuration
185198
test_contact(simu);
186199
}
187200

188-
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_cost)
201+
BOOST_AUTO_TEST_CASE(CentroidalID_contactQuad_cost)
189202
{
190-
TestKinoID simu(
191-
getTalosModelHandler(), KinodynamicsID::Settings()
192-
.set_kp_base(1.0)
193-
.set_kp_posture(1.)
194-
.set_kp_contact(10.0)
195-
.set_w_base(1.)
196-
.set_w_posture(0.05)
197-
.set_w_contact_motion(10.0)
198-
.set_w_contact_force(1.0));
203+
CentroidalID::Settings settings;
204+
settings.kp_base = 1.0;
205+
settings.kp_posture = 1.;
206+
settings.kp_contact = 10.0;
207+
settings.w_base = 1.;
208+
settings.w_posture = 0.05;
209+
settings.w_contact_motion = 10.0;
210+
settings.w_contact_force = 1.0;
211+
212+
TestCentroidalID simu(getTalosModelHandler(), settings);
199213
test_contact(simu);
200214
}
201215

202-
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_equality)
216+
BOOST_AUTO_TEST_CASE(CentroidalID_contactPoint_equality)
203217
{
204-
TestKinoID simu(
205-
getSoloHandler(), KinodynamicsID::Settings()
206-
.set_kp_base(1.0)
207-
.set_kp_contact(10.0)
208-
.set_w_base(1.)
209-
.set_w_contact_motion(10.0)
210-
.set_w_contact_force(1.0)
211-
.set_contact_motion_equality(true));
218+
CentroidalID::Settings settings;
219+
settings.kp_base = 1.0;
220+
settings.kp_contact = 10.0;
221+
settings.w_base = 1.;
222+
settings.w_contact_motion = 10.0;
223+
settings.w_contact_force = 1.0;
224+
settings.contact_motion_equality = true;
225+
TestCentroidalID simu(getSoloHandler(), settings);
212226
simu.q = solo_q_start(simu.model_handler); // Set initial configuration
213227
test_contact(simu);
214228
}
215229

216-
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_equality)
230+
BOOST_AUTO_TEST_CASE(CentroidalID_contactQuad_equality)
217231
{
218-
TestKinoID simu(
219-
getTalosModelHandler(), KinodynamicsID::Settings()
220-
.set_kp_base(1.0)
221-
.set_kp_posture(1.)
222-
.set_kp_contact(10.0)
223-
.set_w_base(1.)
224-
.set_w_posture(0.05)
225-
.set_w_contact_motion(10.0)
226-
.set_w_contact_force(1.0)
227-
.set_contact_motion_equality(true));
232+
CentroidalID::Settings settings;
233+
settings.kp_base = 1.0;
234+
settings.kp_posture = 1.;
235+
settings.kp_contact = 10.0;
236+
settings.w_base = 1.;
237+
settings.w_posture = 0.05;
238+
settings.w_contact_motion = 10.0;
239+
settings.w_contact_force = 1.0;
240+
settings.contact_motion_equality = true;
241+
242+
TestCentroidalID simu(getTalosModelHandler(), settings);
228243
test_contact(simu);
229244
}
230245

231-
BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
246+
BOOST_AUTO_TEST_CASE(CentroidalID_baseTask)
232247
{
233-
TestKinoID test(
234-
getSoloHandler(), KinodynamicsID::Settings()
235-
.set_kp_base(7.)
236-
.set_kp_contact(.1)
237-
.set_w_base(100.0)
238-
.set_w_contact_force(1.0)
239-
.set_w_contact_motion(1.0));
248+
CentroidalID::Settings settings;
249+
settings.kp_base = 7.;
250+
settings.kp_contact = .1;
251+
settings.w_base = 100.0;
252+
settings.w_contact_force = 1.0;
253+
settings.w_contact_motion = 1.0;
254+
255+
TestCentroidalID test(getSoloHandler(), settings);
240256

241257
// Easy access
242258
const RobotModelHandler & model_handler = test.model_handler;
243259
const size_t nq = model_handler.getModel().nq;
244260

245-
// No need to set target as KinodynamicsID sets it by default to reference state
261+
// No need to set target as CentroidalID sets it by default to reference state
246262
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
247263

248264
// Change initial state
@@ -266,24 +282,25 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
266282
}
267283
}
268284

269-
BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
285+
BOOST_AUTO_TEST_CASE(CentroidalID_allTasks)
270286
{
271-
TestKinoID test(
272-
getSoloHandler(), KinodynamicsID::Settings()
273-
.set_kp_base(10.)
274-
.set_kp_posture(1.)
275-
.set_kp_contact(10.)
276-
.set_w_base(10.0)
277-
.set_w_posture(0.1)
278-
.set_w_contact_force(1.0)
279-
.set_w_contact_motion(1.0));
287+
CentroidalID::Settings settings;
288+
settings.kp_base = 10.;
289+
settings.kp_posture = 1.;
290+
settings.kp_contact = 10.;
291+
settings.w_base = 10.0;
292+
settings.w_posture = 0.1;
293+
settings.w_contact_force = 1.0;
294+
settings.w_contact_motion = 1.0;
295+
296+
TestCentroidalID test(getSoloHandler(), settings);
280297

281298
// Easy access
282299
const RobotModelHandler & model_handler = test.model_handler;
283300
const size_t nq = model_handler.getModel().nq;
284301
const size_t nv = model_handler.getModel().nv;
285302

286-
// No need to set target as KinodynamicsID sets it by default to reference state
303+
// No need to set target as CentroidalID sets it by default to reference state
287304
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
288305

289306
test.q = solo_q_start(model_handler);

0 commit comments

Comments
 (0)