Skip to content

Commit d4aec23

Browse files
EtienneArEtienneAr
committed
copy paste kino test to centroidal
1 parent 092858d commit d4aec23

File tree

1 file changed

+304
-0
lines changed

1 file changed

+304
-0
lines changed

‎tests/inverse-dynamics/centroidal.cpp‎

Lines changed: 304 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,304 @@
1+
2+
#include <boost/test/tools/old/interface.hpp>
3+
#include <boost/test/unit_test.hpp>
4+
#include <pinocchio/algorithm/center-of-mass.hpp>
5+
#include <pinocchio/algorithm/frames.hpp>
6+
7+
#include "simple-mpc/inverse-dynamics/kinodynamics.hpp"
8+
#include "simple-mpc/robot-handler.hpp"
9+
#include "test_utils.cpp"
10+
11+
BOOST_AUTO_TEST_SUITE(inverse_dynamics)
12+
13+
using namespace simple_mpc;
14+
15+
Eigen::VectorXd solo_q_start(const RobotModelHandler & model_handler)
16+
{
17+
Eigen::VectorXd q_start = model_handler.getReferenceState().head(model_handler.getModel().nq);
18+
for (int l = 0; l < 4; l++)
19+
{
20+
q_start[7 + 3 * l + 1] = 0.9;
21+
q_start[7 + 3 * l + 2] = -1.8;
22+
}
23+
q_start[0] = 0.01;
24+
q_start[1] = 0.01;
25+
q_start[2] = 0.21;
26+
27+
return q_start;
28+
}
29+
30+
// Helper class to create the problem and run it
31+
class TestKinoID
32+
{
33+
public:
34+
TestKinoID(RobotModelHandler model_handler_, KinodynamicsID::Settings settings_)
35+
: model_handler(model_handler_)
36+
, data_handler(model_handler)
37+
, settings(settings_)
38+
, solver(model_handler, dt, settings)
39+
, q(model_handler.getReferenceState().head(model_handler.getModel().nq))
40+
, dq(Eigen::VectorXd::Zero(model_handler.getModel().nv))
41+
, ddq(Eigen::VectorXd::Zero(model_handler.getModel().nv))
42+
, tau(Eigen::VectorXd::Zero(model_handler.getModel().nv - 6))
43+
{
44+
}
45+
46+
void step()
47+
{
48+
// Solve
49+
solver.solve(t, q, dq, tau);
50+
solver.getAccelerations(ddq);
51+
52+
// Integrate
53+
step_i += 1;
54+
t += dt;
55+
q = pinocchio::integrate(model_handler.getModel(), q, (dq + ddq / 2. * dt) * dt);
56+
dq += ddq * dt;
57+
58+
// Update data handler
59+
data_handler.updateInternalData(q, dq, true);
60+
61+
// Check common to all tests
62+
check_joint_limits();
63+
}
64+
65+
bool is_error_decreasing(std::string name, double error)
66+
{
67+
if (errors.count(name) == 0)
68+
{
69+
errors.insert({name, error});
70+
return true; // no further check
71+
}
72+
const bool res{error <= errors.at(name)};
73+
errors.at(name) = error; // Update value
74+
return res;
75+
}
76+
77+
protected:
78+
void check_joint_limits()
79+
{
80+
const pinocchio::Model & model = model_handler.getModel();
81+
for (int i = 0; i < model.nv - 6; i++)
82+
{
83+
BOOST_CHECK_LE(q[7 + i], model.upperPositionLimit[7 + i]);
84+
BOOST_CHECK_GE(q[7 + i], model.lowerPositionLimit[7 + i]);
85+
BOOST_CHECK_LE(dq[6 + i], model.upperVelocityLimit[6 + i]);
86+
// Do not use lower velocity bound as TSID cannot handle it
87+
BOOST_CHECK_GE(dq[6 + i], -model.upperVelocityLimit[6 + i]);
88+
BOOST_CHECK_LE(tau[i], model.upperEffortLimit[6 + i]);
89+
BOOST_CHECK_GE(tau[i], model.lowerEffortLimit[6 + i]);
90+
}
91+
}
92+
93+
public:
94+
const RobotModelHandler model_handler;
95+
RobotDataHandler data_handler;
96+
KinodynamicsID::Settings settings;
97+
double dt = 1e-3;
98+
KinodynamicsID solver;
99+
100+
double t = 0.;
101+
int step_i = 0;
102+
Eigen::VectorXd q;
103+
Eigen::VectorXd dq;
104+
Eigen::VectorXd ddq;
105+
Eigen::VectorXd tau;
106+
107+
std::map<std::string, double> errors;
108+
};
109+
110+
BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
111+
{
112+
TestKinoID test(getSoloHandler(), KinodynamicsID::Settings().set_kp_posture(20.).set_w_posture(1.));
113+
114+
// Easy access
115+
const RobotModelHandler & model_handler = test.model_handler;
116+
const size_t nq = model_handler.getModel().nq;
117+
const size_t nv = model_handler.getModel().nv;
118+
119+
// Target state
120+
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
121+
test.solver.setTarget(
122+
q_target, Eigen::VectorXd::Zero(nv), Eigen::VectorXd::Zero(nv), {false, false, false, false}, {});
123+
124+
// Change initial state
125+
test.q = solo_q_start(model_handler);
126+
for (int i = 0; i < 1000; i++)
127+
{
128+
// Solve
129+
test.step();
130+
131+
// compensate for free fall as we did not set any contact (we only care about joint posture)
132+
test.q.head(7) = q_target.head(7);
133+
test.dq.head(6).setZero();
134+
135+
// Check error is decreasing
136+
Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target);
137+
const double error = delta_q.tail(nv - 6).norm(); // Consider only the posture not the free flyer
138+
BOOST_CHECK(test.is_error_decreasing("posture", error));
139+
}
140+
}
141+
142+
void test_contact(TestKinoID test)
143+
{
144+
// Easy access
145+
const RobotModelHandler & model_handler = test.model_handler;
146+
const RobotDataHandler & data_handler = test.data_handler;
147+
const size_t nq = model_handler.getModel().nq;
148+
const size_t nv = model_handler.getModel().nv;
149+
150+
// No need to set target as KinodynamicsID sets it by default to reference state
151+
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
152+
153+
// Let the robot stabilize
154+
const int N_STEP = 500;
155+
while (test.step_i < N_STEP)
156+
{
157+
// Solve
158+
test.step();
159+
160+
// Check that contact velocity is null
161+
for (int foot_nb = 0; foot_nb < model_handler.getFeetNb(); foot_nb++)
162+
{
163+
const pinocchio::Motion foot_vel = pinocchio::getFrameVelocity(
164+
model_handler.getModel(), data_handler.getData(), model_handler.getFootFrameId(foot_nb), pinocchio::WORLD);
165+
BOOST_CHECK_LE(foot_vel.linear().norm(), 1e-2);
166+
if (model_handler.getFootType(foot_nb) == RobotModelHandler::FootType::QUAD)
167+
{
168+
// Rotation should also be null for quadrilateral contacts
169+
BOOST_CHECK_LE(foot_vel.angular().norm(), 1e-1);
170+
}
171+
}
172+
}
173+
}
174+
175+
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_cost)
176+
{
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));
184+
simu.q = solo_q_start(simu.model_handler); // Set initial configuration
185+
test_contact(simu);
186+
}
187+
188+
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_cost)
189+
{
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));
199+
test_contact(simu);
200+
}
201+
202+
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactPoint_equality)
203+
{
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));
212+
simu.q = solo_q_start(simu.model_handler); // Set initial configuration
213+
test_contact(simu);
214+
}
215+
216+
BOOST_AUTO_TEST_CASE(KinodynamicsID_contactQuad_equality)
217+
{
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));
228+
test_contact(simu);
229+
}
230+
231+
BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
232+
{
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));
240+
241+
// Easy access
242+
const RobotModelHandler & model_handler = test.model_handler;
243+
const size_t nq = model_handler.getModel().nq;
244+
245+
// No need to set target as KinodynamicsID sets it by default to reference state
246+
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
247+
248+
// Change initial state
249+
test.q = solo_q_start(model_handler);
250+
251+
const int N_STEP = 10000;
252+
for (int i = 0; i < N_STEP; i++)
253+
{
254+
// Solve
255+
test.step();
256+
257+
// Compute error
258+
const Eigen::VectorXd delta_pose = pinocchio::difference(model_handler.getModel(), test.q, q_target).head<6>();
259+
const double error = delta_pose.norm();
260+
261+
// Checks
262+
if (error > 2e-2) // If haven't converged yet, should be strictly decreasing
263+
BOOST_CHECK(test.is_error_decreasing("base", error));
264+
if (i > 9 * N_STEP / 10) // Should have converged by now
265+
BOOST_CHECK(error < 2e-2);
266+
}
267+
}
268+
269+
BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
270+
{
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));
280+
281+
// Easy access
282+
const RobotModelHandler & model_handler = test.model_handler;
283+
const size_t nq = model_handler.getModel().nq;
284+
const size_t nv = model_handler.getModel().nv;
285+
286+
// No need to set target as KinodynamicsID sets it by default to reference state
287+
const Eigen::VectorXd q_target = model_handler.getReferenceState().head(nq);
288+
289+
test.q = solo_q_start(model_handler);
290+
const int N_STEP = 1000;
291+
for (int i = 0; i < N_STEP; i++)
292+
{
293+
// Solve
294+
test.step();
295+
296+
// Check error is decreasing
297+
const Eigen::VectorXd delta_q = pinocchio::difference(model_handler.getModel(), test.q, q_target);
298+
const double error = delta_q.norm();
299+
300+
BOOST_CHECK(test.is_error_decreasing("q", error));
301+
}
302+
}
303+
304+
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)