Biorbd Release_1.13.0
Loading...
Searching...
No Matches
StaticOptimization.cpp
1#define BIORBD_API_EXPORTS
2
3#include "InternalForces/Muscles/StaticOptimization.h"
4
5#include <IpIpoptApplication.hpp>
6
7#include "BiorbdModel.h"
8#include "InternalForces/Muscles/StateDynamics.h"
9#include "InternalForces/Muscles/StaticOptimizationIpoptLinearized.h"
10#include "RigidBody/GeneralizedCoordinates.h"
11#include "RigidBody/GeneralizedTorque.h"
12#include "RigidBody/GeneralizedVelocity.h"
13#include "Utils/Error.h"
14#include "Utils/Vector.h"
15
16using namespace BIORBD_NAMESPACE;
17using namespace internal_forces;
18
20 Model& model,
23 const rigidbody::GeneralizedTorque& torqueTarget,
24 double initialActivationGuess,
25 unsigned int pNormFactor,
26 bool useResidualTorque,
27 int verbose,
28 double objectiveScaling)
29 : m_model(model),
30 m_useResidualTorque(useResidualTorque),
32 std::make_shared<utils::Vector>(m_model.nbMuscles())),
33 m_pNormFactor(pNormFactor),
34 m_verbose(verbose),
35 m_objectiveScaling(objectiveScaling),
37 m_alreadyRun(false) {
39 m_objectiveScaling > 0, "The objective scaling must be positive");
40 m_allQ.push_back(Q);
41 m_allQdot.push_back(Qdot);
42 m_allTorqueTarget.push_back(torqueTarget);
43 for (unsigned int i = 0; i < m_model.nbMuscles(); ++i) {
44 (*m_initialActivationGuess)[i] = initialActivationGuess;
45 }
46}
47
49 Model& model,
52 const rigidbody::GeneralizedTorque& torqueTarget,
53 const utils::Vector& initialActivationGuess,
54 unsigned int pNormFactor,
55 bool useResidualTorque,
56 int verbose,
57 double objectiveScaling)
58 : m_model(model),
59 m_useResidualTorque(useResidualTorque),
61 std::make_shared<utils::Vector>(m_model.nbMuscles())),
62 m_pNormFactor(pNormFactor),
63 m_verbose(verbose),
64 m_objectiveScaling(objectiveScaling),
66 m_alreadyRun(false) {
68 m_objectiveScaling > 0, "The objective scaling must be positive");
69 m_allQ.push_back(Q);
70 m_allQdot.push_back(Qdot);
71 m_allTorqueTarget.push_back(torqueTarget);
72
73 if (initialActivationGuess.size() != m_model.nbMuscles()) {
75 "Initial guess must either be a single value or a vector "
76 "of dimension nbMuscles");
77 }
78 *m_initialActivationGuess = initialActivationGuess;
79}
80
82 Model& model,
85 const rigidbody::GeneralizedTorque& torqueTarget,
86 const std::vector<StateDynamics>& initialActivationGuess,
87 unsigned int pNormFactor,
88 bool useResidualTorque,
89 int verbose,
90 double objectiveScaling)
91 : m_model(model),
92 m_useResidualTorque(useResidualTorque),
94 std::make_shared<utils::Vector>(m_model.nbMuscles())),
95 m_pNormFactor(pNormFactor),
96 m_verbose(verbose),
97 m_objectiveScaling(objectiveScaling),
98 m_alreadyRun(false) {
100 m_objectiveScaling > 0, "The objective scaling must be positive");
101 m_allQ.push_back(Q);
102 m_allQdot.push_back(Qdot);
103 m_allTorqueTarget.push_back(torqueTarget);
104
105 if (initialActivationGuess.size() != m_model.nbMuscles()) {
107 "Initial guess must either be a single value or a vector "
108 "of dimension nbMuscles");
109 }
110
111 for (unsigned int i = 0; i < m_model.nbMuscles(); i++) {
112 (*m_initialActivationGuess)[i] = initialActivationGuess[i].activation();
113 }
114}
115
117 Model& model,
118 const std::vector<rigidbody::GeneralizedCoordinates>& allQ,
119 const std::vector<rigidbody::GeneralizedVelocity>& allQdot,
120 const std::vector<rigidbody::GeneralizedTorque>& allTorqueTarget,
121 double initialActivationGuess,
122 unsigned int pNormFactor,
123 bool useResidualTorque,
124 int verbose,
125 double objectiveScaling)
126 : m_model(model),
127 m_useResidualTorque(useResidualTorque),
128 m_allQ(allQ),
129 m_allQdot(allQdot),
130 m_allTorqueTarget(allTorqueTarget),
132 std::make_shared<utils::Vector>(m_model.nbMuscles())),
133 m_pNormFactor(pNormFactor),
134 m_verbose(verbose),
135 m_objectiveScaling(objectiveScaling),
136 m_alreadyRun(false) {
138 m_objectiveScaling > 0, "The objective scaling must be positive");
139 for (unsigned int i = 0; i < m_model.nbMuscles(); ++i) {
140 (*m_initialActivationGuess)[i] = initialActivationGuess;
141 }
142}
143
145 Model& model,
146 const std::vector<rigidbody::GeneralizedCoordinates>& allQ,
147 const std::vector<rigidbody::GeneralizedVelocity>& allQdot,
148 const std::vector<rigidbody::GeneralizedTorque>& allTorqueTarget,
149 const utils::Vector& initialActivationGuess,
150 unsigned int pNormFactor,
151 bool useResidualTorque,
152 int verbose,
153 double objectiveScaling)
154 : m_model(model),
155 m_useResidualTorque(useResidualTorque),
156 m_allQ(allQ),
157 m_allQdot(allQdot),
158 m_allTorqueTarget(allTorqueTarget),
160 std::make_shared<utils::Vector>(m_model.nbMuscles())),
161 m_pNormFactor(pNormFactor),
162 m_verbose(verbose),
163 m_objectiveScaling(objectiveScaling),
164 m_alreadyRun(false) {
166 m_objectiveScaling > 0, "The objective scaling must be positive");
167 if (initialActivationGuess.size() != m_model.nbMuscles()) {
169 "Initial guess must either be a single value or a vector "
170 "of dimension nbMuscles");
171 }
172 *m_initialActivationGuess = initialActivationGuess;
173}
174
176 Model& model,
177 const std::vector<rigidbody::GeneralizedCoordinates>& allQ,
178 const std::vector<rigidbody::GeneralizedVelocity>& allQdot,
179 const std::vector<rigidbody::GeneralizedTorque>& allTorqueTarget,
180 const std::vector<StateDynamics>& initialActivationGuess,
181 unsigned int pNormFactor,
182 bool useResidualTorque,
183 int verbose,
184 double objectiveScaling)
185 : m_model(model),
186 m_useResidualTorque(useResidualTorque),
187 m_allQ(allQ),
188 m_allQdot(allQdot),
189 m_allTorqueTarget(allTorqueTarget),
191 std::make_shared<utils::Vector>(m_model.nbMuscles())),
192 m_pNormFactor(pNormFactor),
193 m_verbose(verbose),
194 m_objectiveScaling(objectiveScaling),
195 m_alreadyRun(false) {
197 m_objectiveScaling > 0, "The objective scaling must be positive");
198 if (initialActivationGuess.size() != m_model.nbMuscles()) {
200 "Initial guess must either be a single value or a vector "
201 "of dimension nbMuscles");
202 }
203
204 for (unsigned int i = 0; i < m_model.nbMuscles(); i++) {
205 (*m_initialActivationGuess)[i] = initialActivationGuess[i].activation();
206 }
207}
208
210 bool useLinearizedState,
211 double tolerance,
212 int maxIter,
213 bool checkDerivative) {
214 // Setup the Ipopt problem
215 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
216 app->Options()->SetNumericValue("tol", tolerance);
217 app->Options()->SetStringValue("mu_strategy", "adaptive");
218 // app->Options()->SetStringValue("output_file", "ipopt.out");
219 app->Options()->SetStringValue("hessian_approximation", "limited-memory");
220 app->Options()->SetNumericValue("obj_scaling_factor", m_objectiveScaling);
221 app->Options()->SetStringValue(
222 "derivative_test", checkDerivative ? "first-order" : "none");
223 app->Options()->SetIntegerValue("max_iter", maxIter);
224 app->Options()->SetIntegerValue("print_level", m_verbose);
225
226 Ipopt::ApplicationReturnStatus status;
227 status = app->Initialize();
229 status == Ipopt::Solve_Succeeded, "Ipopt initialization failed");
230
231 // Start from scratch so running multiple times gives the same results
232 m_staticOptimProblem.clear();
233 m_alreadyRun = false;
234 utils::Vector activationGuess(*m_initialActivationGuess);
235 for (unsigned int i = 0; i < m_allQ.size(); ++i) {
236 if (useLinearizedState)
237 m_staticOptimProblem.push_back(
239 m_model,
240 m_allQ[i],
241 m_allQdot[i],
243 activationGuess,
246 m_verbose));
247 else
248 m_staticOptimProblem.push_back(
250 m_model,
251 m_allQ[i],
252 m_allQdot[i],
254 activationGuess,
257 m_verbose));
258 // Optimize!
259 status = app->OptimizeTNLP(m_staticOptimProblem[i]);
261 status == Ipopt::Solve_Succeeded ||
262 status == Ipopt::Solved_To_Acceptable_Level,
263 "Static optimization failed to converge for frame " +
264 std::to_string(i));
265
266 // Take the solution of the previous optimization as the solution for the
267 // next optimization
268 activationGuess =
270 Ipopt::GetRawPtr(m_staticOptimProblem[i]))
271 ->finalSolution();
272 }
273 m_alreadyRun = true;
274}
275
276std::vector<utils::Vector>
278 std::vector<utils::Vector> res;
279 if (!m_alreadyRun) {
281 "Problem has not been ran through the optimization process "
282 "yet, you should optimize it first to get "
283 "the optimized solution");
284 } else {
285 for (unsigned int i = 0; i < m_allQ.size(); ++i) {
286 res.push_back(
288 Ipopt::GetRawPtr(m_staticOptimProblem[i]))
289 ->finalSolution());
290 }
291 }
292
293 return res;
294}
295
297 unsigned int index) {
298 utils::Vector res;
299 if (!m_alreadyRun) {
301 "Problem has not been ran through the optimization process "
302 "yet, you should optimize it first to get "
303 "the optimized solution");
304 } else {
306 Ipopt::GetRawPtr(m_staticOptimProblem[index]))
307 ->finalSolution();
308 }
309 return res;
310}
311
312std::vector<utils::Vector>
314 std::vector<utils::Vector> res;
315 if (!m_alreadyRun) {
317 "Problem has not been run through the optimization process "
318 "yet, you should optimize it first to get "
319 "the residual solution");
320 } else {
321 for (unsigned int i = 0; i < m_allQ.size(); ++i) {
322 res.push_back(
324 Ipopt::GetRawPtr(m_staticOptimProblem[i]))
325 ->finalResidual());
326 }
327 }
328 return res;
329}
330
332 unsigned int index) {
333 utils::Vector res;
334 if (!m_alreadyRun) {
336 "Problem has not been run through the optimization process "
337 "yet, you should optimize it first to get "
338 "the residual solution");
339 } else {
341 Ipopt::GetRawPtr(m_staticOptimProblem[index]))
342 ->finalResidual();
343 }
344 return res;
345}
The actual musculoskeletal model that holds everything in biorbd.
The actual implementation of the Static Optimization problem using a linearized approach.
The actual implementation of the Static Optimization problem.
std::vector< utils::Vector > finalSolution()
Return the final solution.
void run(bool useLinearizedState=true, double tolerance=1e-7, int maxIter=1000, bool checkDerivatives=false)
Run the static optimization.
std::vector< Ipopt::SmartPtr< Ipopt::TNLP > > m_staticOptimProblem
The static optimization problem.
StaticOptimization(Model &model, const rigidbody::GeneralizedCoordinates &Q, const rigidbody::GeneralizedVelocity &Qdot, const rigidbody::GeneralizedTorque &torqueTarget, double initialActivationGuess=0.01, unsigned int pNormFactor=2, bool useResidualTorque=true, int verbose=0, double objectiveScaling=1)
Construct static optimization.
std::shared_ptr< utils::Vector > m_initialActivationGuess
Initial activation guess.
std::vector< utils::Vector > finalResidual()
Return the final residuals.
std::vector< rigidbody::GeneralizedCoordinates > m_allQ
All the generalized coordinates.
bool m_alreadyRun
If already ran the static optimization.
std::vector< rigidbody::GeneralizedVelocity > m_allQdot
All the generalized velocities.
std::vector< rigidbody::GeneralizedTorque > m_allTorqueTarget
All the torque targets.
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void warning(bool cond, const String &message)
Non-blocking assert that displays the error message if false.
Definition Error.cpp:16
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
Wrapper of the Eigen VectorXd.
Definition Vector.h:21