1#define BIORBD_API_EXPORTS
3#include "InternalForces/Muscles/StaticOptimization.h"
5#include <IpIpoptApplication.hpp>
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"
16using namespace BIORBD_NAMESPACE;
17using namespace internal_forces;
24 double initialActivationGuess,
25 unsigned int pNormFactor,
26 bool useResidualTorque,
28 double objectiveScaling)
32 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
43 for (
unsigned int i = 0; i <
m_model.nbMuscles(); ++i) {
44 (*m_initialActivationGuess)[i] = initialActivationGuess;
54 unsigned int pNormFactor,
55 bool useResidualTorque,
57 double objectiveScaling)
61 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
73 if (initialActivationGuess.size() !=
m_model.nbMuscles()) {
75 "Initial guess must either be a single value or a vector "
76 "of dimension nbMuscles");
86 const std::vector<StateDynamics>& initialActivationGuess,
87 unsigned int pNormFactor,
88 bool useResidualTorque,
90 double objectiveScaling)
94 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
105 if (initialActivationGuess.size() !=
m_model.nbMuscles()) {
107 "Initial guess must either be a single value or a vector "
108 "of dimension nbMuscles");
111 for (
unsigned int i = 0; i <
m_model.nbMuscles(); i++) {
112 (*m_initialActivationGuess)[i] = initialActivationGuess[i].activation();
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,
125 double objectiveScaling)
132 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
139 for (
unsigned int i = 0; i <
m_model.nbMuscles(); ++i) {
140 (*m_initialActivationGuess)[i] = initialActivationGuess;
146 const std::vector<rigidbody::GeneralizedCoordinates>& allQ,
147 const std::vector<rigidbody::GeneralizedVelocity>& allQdot,
148 const std::vector<rigidbody::GeneralizedTorque>& allTorqueTarget,
150 unsigned int pNormFactor,
151 bool useResidualTorque,
153 double objectiveScaling)
160 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
167 if (initialActivationGuess.size() !=
m_model.nbMuscles()) {
169 "Initial guess must either be a single value or a vector "
170 "of dimension nbMuscles");
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,
184 double objectiveScaling)
191 std::make_shared<utils::Vector>(
m_model.nbMuscles())),
198 if (initialActivationGuess.size() !=
m_model.nbMuscles()) {
200 "Initial guess must either be a single value or a vector "
201 "of dimension nbMuscles");
204 for (
unsigned int i = 0; i <
m_model.nbMuscles(); i++) {
205 (*m_initialActivationGuess)[i] = initialActivationGuess[i].activation();
210 bool useLinearizedState,
213 bool checkDerivative) {
215 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
216 app->Options()->SetNumericValue(
"tol", tolerance);
217 app->Options()->SetStringValue(
"mu_strategy",
"adaptive");
219 app->Options()->SetStringValue(
"hessian_approximation",
"limited-memory");
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);
226 Ipopt::ApplicationReturnStatus status;
227 status = app->Initialize();
229 status == Ipopt::Solve_Succeeded,
"Ipopt initialization failed");
235 for (
unsigned int i = 0; i <
m_allQ.size(); ++i) {
236 if (useLinearizedState)
261 status == Ipopt::Solve_Succeeded ||
262 status == Ipopt::Solved_To_Acceptable_Level,
263 "Static optimization failed to converge for frame " +
276std::vector<utils::Vector>
278 std::vector<utils::Vector> res;
281 "Problem has not been ran through the optimization process "
282 "yet, you should optimize it first to get "
283 "the optimized solution");
285 for (
unsigned int i = 0; i <
m_allQ.size(); ++i) {
297 unsigned int index) {
301 "Problem has not been ran through the optimization process "
302 "yet, you should optimize it first to get "
303 "the optimized solution");
312std::vector<utils::Vector>
314 std::vector<utils::Vector> res;
317 "Problem has not been run through the optimization process "
318 "yet, you should optimize it first to get "
319 "the residual solution");
321 for (
unsigned int i = 0; i <
m_allQ.size(); ++i) {
332 unsigned int index) {
336 "Problem has not been run through the optimization process "
337 "yet, you should optimize it first to get "
338 "the residual solution");
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.
unsigned int m_pNormFactor
The p-norm factor.
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.
int m_verbose
Verbose level.
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.
double m_objectiveScaling
Scaling factor of the objective.
Model & m_model
A reference to the model.
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.
bool m_useResidualTorque
To use residual torque.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
static void raise(const String &message)
Throw an error message.
static void warning(bool cond, const String &message)
Non-blocking assert that displays the error message if false.
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Wrapper of the Eigen VectorXd.