Biorbd Release_1.13.0
Loading...
Searching...
No Matches
StaticOptimizationIpopt.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/StaticOptimizationIpopt.h"
3
4#include <iomanip>
5#include <iostream>
6
7#include "BiorbdModel.h"
8#include "InternalForces/Muscles/StateDynamics.h"
9#include "RigidBody/GeneralizedCoordinates.h"
10#include "RigidBody/GeneralizedTorque.h"
11#include "RigidBody/GeneralizedVelocity.h"
12#include "Utils/Error.h"
13#include "Utils/Matrix.h"
14#include "Utils/Vector.h"
15
16using namespace BIORBD_NAMESPACE;
17using namespace internal_forces;
18
19const double internal_forces::muscles::StaticOptimizationIpopt::
20 m_activationLowerBound = 0.0001;
21const double internal_forces::muscles::StaticOptimizationIpopt::
22 m_activationUpperBound = 0.9999;
23
25 Model& model,
28 const rigidbody::GeneralizedTorque& torqueTarget,
29 const utils::Vector& activationInit,
30 bool useResidual,
31 unsigned int pNormFactor,
32 int verbose,
33 double eps)
34 : m_model(model),
35 m_nbQ(std::make_shared<unsigned int>(model.nbQ())),
36 m_nbQdot(std::make_shared<unsigned int>(model.nbQdot())),
37 m_nbMus(std::make_shared<unsigned int>(model.nbMuscleTotal())),
38 m_nbDof(std::make_shared<unsigned int>(model.nbDof())),
39 m_nbTorque(std::make_shared<unsigned int>(model.nbGeneralizedTorque())),
40 m_nbTorqueResidual(std::make_shared<unsigned int>(*m_nbQ)),
41 m_eps(std::make_shared<double>(eps)),
42 m_activations(std::make_shared<utils::Vector>(activationInit)),
43 m_Q(std::make_shared<rigidbody::GeneralizedCoordinates>(Q)),
44 m_Qdot(std::make_shared<rigidbody::GeneralizedVelocity>(Qdot)),
46 std::make_shared<rigidbody::GeneralizedTorque>(torqueTarget)),
48 std::make_shared<utils::Vector>(utils::Vector::Zero(*m_nbTorque))),
49 m_torquePonderation(std::make_shared<double>(1000)),
51 std::make_shared<
52 std::vector<std::shared_ptr<internal_forces::muscles::State>>>(
53 *m_nbMus)),
54 m_pNormFactor(std::make_shared<unsigned int>(pNormFactor)),
55 m_verbose(std::make_shared<int>(verbose)),
56 // Default to the initial guess in case Ipopt aborts before
57 // finalize_solution is called
58 m_finalSolution(std::make_shared<utils::Vector>(activationInit)),
60 std::make_shared<utils::Vector>(utils::Vector::Zero(*m_nbTorque))) {
61 if (*m_eps < 1e-12) {
63 "epsilon for partial derivates approximation is too small ! \nLimit "
64 "for epsilon is 1e-12");
65 }
66
67 for (auto& s : *m_states) {
68 s = std::make_shared<internal_forces::muscles::State>();
69 }
70
71 m_model.updateMuscles(*m_Q, *m_Qdot, true);
72 if (!useResidual) {
73 m_torqueResidual->setZero();
76 }
77}
78
80
82 Ipopt::Index& n,
83 Ipopt::Index& m,
84 Ipopt::Index& nnz_jac_g,
85 Ipopt::Index& nnz_h_lag,
86 IndexStyleEnum& index_style) {
87 index_style = TNLP::C_STYLE;
88
89 // variables
90 n = static_cast<int>(*m_nbMus + *m_nbTorqueResidual);
91
92 // Constraints
93 m = static_cast<int>(*m_nbTorque);
94 if (*m_nbTorqueResidual) {
95 nnz_jac_g =
96 (static_cast<int>(*m_nbMus) + 1) * static_cast<int>(*m_nbTorque);
97 } else {
98 nnz_jac_g = static_cast<int>(*m_nbMus) * static_cast<int>(*m_nbTorque);
99 }
100 nnz_h_lag = static_cast<int>(*m_nbTorque) * static_cast<int>(*m_nbTorque);
101
102 if (*m_verbose >= 2) {
103 std::cout << "n: " << n << std::endl;
104 std::cout << "m: " << m << std::endl;
105 }
106 return true;
107}
108
110 Ipopt::Index n,
111 Ipopt::Number* x_l,
112 Ipopt::Number* x_u,
113 Ipopt::Index m,
114 Ipopt::Number* g_l,
115 Ipopt::Number* g_u) {
116 // Should not be necessary?
117 assert(static_cast<unsigned int>(n) == *m_nbMus + *m_nbTorqueResidual);
118 assert(static_cast<unsigned int>(m) == *m_nbTorque);
119
120 // Variables
121 for (unsigned int i = 0; i < *m_nbMus; ++i) {
122 x_l[i] = m_activationLowerBound;
123 x_u[i] = m_activationUpperBound;
124 }
125 for (unsigned int i = *m_nbMus; i < *m_nbMus + *m_nbTorqueResidual; ++i) {
126 x_l[i] = -1000.0;
127 x_u[i] = 1000.0;
128 }
129
130 // Constraints
131 for (unsigned int i = 0; i < *m_nbTorque; ++i) {
132 g_l[i] = g_u[i] = 0;
133 }
134
135 return true;
136}
137
139 Ipopt::Index,
140 bool init_x,
141 Ipopt::Number* x,
142 bool init_z,
143 Ipopt::Number*,
144 Ipopt::Number*,
145 Ipopt::Index,
146 bool init_lambda,
147 Ipopt::Number*) {
148 assert(init_x == true);
149 assert(init_z == false);
150 assert(init_lambda == false);
151
152 for (unsigned int i = 0; i < *m_nbMus; i++) {
153 x[i] = (*m_activations)[i];
154 }
155
156 for (unsigned int i = 0; i < *m_nbTorqueResidual; i++) {
157 x[i + *m_nbMus] = (*m_torqueResidual)[i];
158 }
159
160 if (*m_verbose >= 2) {
161 std::cout << std::endl << "Initial guesses" << std::endl;
162 std::cout << "Activations = " << m_activations->transpose() << std::endl;
163 if (*m_nbTorqueResidual) {
164 std::cout << "Residuals = " << m_torqueResidual->transpose() << std::endl;
165 }
166 }
167
168 return true;
169}
170
172 Ipopt::Index n,
173 const Ipopt::Number* x,
174 bool new_x,
175 Ipopt::Number& obj_value) {
176 assert(static_cast<unsigned int>(n) == *m_nbMus + *m_nbTorqueResidual);
177
178 if (new_x) {
179 dispatch(x);
180 }
181
182 // Warning, residual norm HAS to be a non-sqrt, otherwise the problem is
183 // degenerated. The activation is just a speed matter.
184 obj_value = m_activations->norm(*m_pNormFactor, true) +
185 *m_torquePonderation * m_torqueResidual->norm(2, true);
186
187 return true;
188}
189
191 Ipopt::Index n,
192 const Ipopt::Number* x,
193 bool new_x,
194 Ipopt::Number* grad_f) {
195 assert(static_cast<unsigned int>(n) == *m_nbMus + *m_nbTorqueResidual);
196
197 if (new_x) {
198 dispatch(x);
199 }
200
201 utils::Vector grad_activ(m_activations->normGradient(*m_pNormFactor, true));
202 utils::Vector grad_residual(m_torqueResidual->normGradient(2, true));
203
204 for (unsigned i = 0; i < *m_nbMus; i++) {
205 grad_f[i] = grad_activ[i];
206 }
207 for (unsigned int i = 0; i < *m_nbTorqueResidual; i++) {
208 grad_f[i + *m_nbMus] = *m_torquePonderation * grad_residual[i];
209 }
210 return true;
211}
212
214 Ipopt::Index n,
215 const Ipopt::Number* x,
216 bool new_x,
217 Ipopt::Index m,
218 Ipopt::Number* g) {
219 assert(static_cast<unsigned int>(n) == *m_nbMus + *m_nbTorqueResidual);
220 assert(static_cast<unsigned int>(m) == *m_nbTorque);
221 if (new_x) {
222 dispatch(x);
223 }
224
225 const rigidbody::GeneralizedTorque& GeneralizedTorqueMusc(
226 m_model.muscularJointTorque(*m_states));
227
228 for (unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
229 g[i] = GeneralizedTorqueMusc[i] + (*m_torqueResidual)[i] -
230 (*m_torqueTarget)[i];
231 }
232
233 if (*m_verbose >= 2) {
234 std::cout << "GeneralizedTorque_musc = "
235 << GeneralizedTorqueMusc.transpose() << std::endl;
236 std::cout << "GeneralizedTorque_residual = "
237 << m_torqueResidual->transpose() << std::endl;
238 }
239 return true;
240}
241
243 Ipopt::Index n,
244 const Ipopt::Number* x,
245 bool new_x,
246 Ipopt::Index m,
247 Ipopt::Index,
248 Ipopt::Index* iRow,
249 Ipopt::Index* jCol,
250 Ipopt::Number* values) {
251 if (values == nullptr) {
252 // Setup non-zeros values
253 Ipopt::Index k(0);
254 for (Ipopt::Index j = 0; static_cast<unsigned int>(j) < *m_nbMus; ++j) {
255 for (Ipopt::Index i = 0; i < m; ++i) {
256 iRow[k] = i;
257 jCol[k++] = j;
258 }
259 }
260 for (Ipopt::Index j = 0; static_cast<unsigned int>(j) < *m_nbTorqueResidual;
261 ++j) {
262 iRow[k] = j;
263 jCol[k++] = j + static_cast<Ipopt::Index>(*m_nbMus);
264 }
265 } else {
266 if (new_x) {
267 dispatch(x);
268 }
269 const rigidbody::GeneralizedTorque& GeneralizedTorqueMusc(
270 m_model.muscularJointTorque(*m_states));
271 unsigned int k(0);
272 for (unsigned int j = 0; j < *m_nbMus; ++j) {
273 std::vector<std::shared_ptr<internal_forces::muscles::State>>
274 stateEpsilon;
275 for (unsigned int i = 0; i < *m_nbMus; ++i) {
276 unsigned int delta(0);
277 if (i == j) {
278 delta = 1;
279 }
280 stateEpsilon.push_back(
281 std::make_shared<internal_forces::muscles::State>(
283 0, (*m_activations)[i] + delta * *m_eps)));
284 }
285 const rigidbody::GeneralizedTorque& GeneralizedTorqueCalculEpsilon(
286 m_model.muscularJointTorque(stateEpsilon));
287 for (unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
288 values[k++] =
289 (GeneralizedTorqueCalculEpsilon[i] - GeneralizedTorqueMusc[i]) /
290 *m_eps;
291 if (*m_verbose >= 3) {
292 std::cout << std::setprecision(20) << std::endl;
293 std::cout << "values[" << k - 1 << "]: " << values[k - 1]
294 << std::endl;
295 std::cout << "GeneralizedTorqueCalculEpsilon[" << i
296 << "]: " << GeneralizedTorqueCalculEpsilon[i] << std::endl;
297 std::cout << "GeneralizedTorqueMusc[" << i
298 << "]: " << GeneralizedTorqueMusc[i] << std::endl;
299 }
300 }
301 }
302 for (unsigned int j = 0; j < *m_nbTorqueResidual; j++) {
303 values[k++] = 1;
304 }
305
306 if (*m_verbose >= 2) {
307 k = 0;
308 utils::Matrix jacobian(
309 utils::Matrix::Zero(*m_nbTorque, static_cast<unsigned int>(n)));
310 for (unsigned int j = 0; j < *m_nbMus; j++)
311 for (unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
312 jacobian(i, j) = values[k++];
313 }
314 for (unsigned int j = 0; j < *m_nbTorqueResidual; j++) {
315 jacobian(j, j + *m_nbMus) = values[k++];
316 }
317 std::cout << "jacobian = " << std::endl << jacobian << std::endl;
318 }
319 }
320 return true;
321}
322
324 Ipopt::SolverReturn,
325 Ipopt::Index,
326 const Ipopt::Number* x,
327 const Ipopt::Number*, // z_L,
328 const Ipopt::Number*, // z_U,
329 Ipopt::Index,
330 const Ipopt::Number*,
331 const Ipopt::Number*,
332 Ipopt::Number obj_value,
333 const Ipopt::IpoptData*,
334 Ipopt::IpoptCalculatedQuantities*) {
335 // Storing to solution
336 dispatch(x);
339
340 // Plot it, if it makes sense
341 if (*m_verbose >= 1) {
342 std::cout << std::endl << "Final results" << std::endl;
343 std::cout << "f(x*) = " << obj_value << std::endl;
344 std::cout << "Activations = " << m_activations->transpose() << std::endl;
345 std::cout << "Muscular torques = "
346 << m_model.muscularJointTorque(*m_states).transpose()
347 << std::endl;
348 std::cout << "GeneralizedTorque target = " << m_torqueTarget->transpose()
349 << std::endl;
350 if (*m_nbTorqueResidual) {
351 std::cout << "Residual torques= " << m_torqueResidual->transpose()
352 << std::endl;
353 }
354 // // Uncomment to show lagrange multipliers
355 // std::cout << "Solution of the bound multipliers, z_L and z_U" <<
356 // std::endl; for( Ipopt::Index i = 0; i < n; i++ )
357 // std::cout << "z_L[" << i << "] = " << z_L[i] << ", z_U[" << i
358 // << "] = " << z_U[i] << std::endl << std::endl;
359 }
360}
361
366
371
373 const Ipopt::Number* x) {
374 for (unsigned int i = 0; i < *m_nbMus; i++) {
375 (*m_activations)[i] = x[i];
376 (*m_states)[i]->setActivation((*m_activations)[i]);
377 }
378
379 for (unsigned int i = 0; i < *m_nbTorqueResidual; i++) {
380 (*m_torqueResidual)[i] = x[i + *m_nbMus];
381 }
382}
The actual musculoskeletal model that holds everything in biorbd.
EMG holder to interact with the muscle.
Definition State.h:17
virtual bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Compute the objective value from a set of variables.
std::shared_ptr< unsigned int > m_nbQdot
The number of generalized velocities.
std::shared_ptr< utils::Vector > m_torqueResidual
The torque residual.
std::shared_ptr< std::vector< std::shared_ptr< State > > > m_states
The muscle states.
void dispatch(const Ipopt::Number *x)
To dispatch the variables into biorbd format.
std::shared_ptr< unsigned int > m_nbTorque
The number of torques to match.
virtual bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
Method to return the constraint residuals.
std::shared_ptr< unsigned int > m_nbMus
The number of muscles.
std::shared_ptr< double > m_eps
Precision of the finite differentiate.
virtual bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Get info about the NLP.
std::shared_ptr< unsigned int > m_pNormFactor
The p-norm factor.
std::shared_ptr< unsigned int > m_nbTorqueResidual
The number of torque residual.
StaticOptimizationIpopt(Model &model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedTorque &torqueTarget, const utils::Vector &activationInit, bool useResidual=true, unsigned int pNormFactor=2, int verbose=0, double eps=1e-8)
Construct an Ipopt static optimization problem.
virtual bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x, bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U, Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
Return the starting point for the algorithm.
std::shared_ptr< rigidbody::GeneralizedVelocity > m_Qdot
The generalized velocities.
virtual void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number *x, const Ipopt::Number *z_L, const Ipopt::Number *z_U, Ipopt::Index m, const Ipopt::Number *g, const Ipopt::Number *lambda, Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq)
Finalize the optimization.
std::shared_ptr< unsigned int > m_nbQ
The number of generalized coordinates.
std::shared_ptr< rigidbody::GeneralizedCoordinates > m_Q
The generalized coordinates.
std::shared_ptr< utils::Vector > m_finalResidual
The final residual.
virtual bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
Return the bounds for my problem.
std::shared_ptr< rigidbody::GeneralizedTorque > m_torqueTarget
The torque to match.
virtual bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
Return the jacobian of contraints.
virtual bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Return the gradient of the objective.
std::shared_ptr< double > m_torquePonderation
The torque ponderation.
std::shared_ptr< unsigned int > m_nbDof
The number of degrees of freedom.
std::shared_ptr< utils::Vector > m_finalSolution
The final solution.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21