1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/StaticOptimizationIpopt.h"
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"
16using namespace BIORBD_NAMESPACE;
17using namespace internal_forces;
19const double internal_forces::muscles::StaticOptimizationIpopt::
20 m_activationLowerBound = 0.0001;
21const double internal_forces::muscles::StaticOptimizationIpopt::
22 m_activationUpperBound = 0.9999;
31 unsigned int pNormFactor,
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())),
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))),
52 std::vector<std::shared_ptr<internal_forces::muscles::
State>>>(
55 m_verbose(std::make_shared<int>(verbose)),
60 std::make_shared<utils::Vector>(utils::Vector::Zero(*
m_nbTorque))) {
63 "epsilon for partial derivates approximation is too small ! \nLimit "
64 "for epsilon is 1e-12");
68 s = std::make_shared<internal_forces::muscles::State>();
84 Ipopt::Index& nnz_jac_g,
85 Ipopt::Index& nnz_h_lag,
86 IndexStyleEnum& index_style) {
87 index_style = TNLP::C_STYLE;
93 m =
static_cast<int>(*m_nbTorque);
96 (
static_cast<int>(*m_nbMus) + 1) *
static_cast<int>(*
m_nbTorque);
98 nnz_jac_g =
static_cast<int>(*m_nbMus) *
static_cast<int>(*m_nbTorque);
100 nnz_h_lag =
static_cast<int>(*m_nbTorque) *
static_cast<int>(*m_nbTorque);
103 std::cout <<
"n: " << n << std::endl;
104 std::cout <<
"m: " << m << std::endl;
115 Ipopt::Number* g_u) {
118 assert(
static_cast<unsigned int>(m) == *
m_nbTorque);
121 for (
unsigned int i = 0; i < *
m_nbMus; ++i) {
131 for (
unsigned int i = 0; i < *
m_nbTorque; ++i) {
148 assert(init_x ==
true);
149 assert(init_z ==
false);
150 assert(init_lambda ==
false);
152 for (
unsigned int i = 0; i < *
m_nbMus; i++) {
153 x[i] = (*m_activations)[i];
157 x[i + *
m_nbMus] = (*m_torqueResidual)[i];
161 std::cout << std::endl <<
"Initial guesses" << std::endl;
162 std::cout <<
"Activations = " <<
m_activations->transpose() << std::endl;
173 const Ipopt::Number* x,
175 Ipopt::Number& obj_value) {
192 const Ipopt::Number* x,
194 Ipopt::Number* grad_f) {
204 for (
unsigned i = 0; i < *
m_nbMus; i++) {
205 grad_f[i] = grad_activ[i];
215 const Ipopt::Number* x,
220 assert(
static_cast<unsigned int>(m) == *
m_nbTorque);
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];
234 std::cout <<
"GeneralizedTorque_musc = "
235 << GeneralizedTorqueMusc.transpose() << std::endl;
236 std::cout <<
"GeneralizedTorque_residual = "
244 const Ipopt::Number* x,
250 Ipopt::Number* values) {
251 if (values ==
nullptr) {
254 for (Ipopt::Index j = 0;
static_cast<unsigned int>(j) < *
m_nbMus; ++j) {
255 for (Ipopt::Index i = 0; i < m; ++i) {
263 jCol[k++] = j +
static_cast<Ipopt::Index
>(*m_nbMus);
272 for (
unsigned int j = 0; j < *
m_nbMus; ++j) {
273 std::vector<std::shared_ptr<internal_forces::muscles::State>>
275 for (
unsigned int i = 0; i < *
m_nbMus; ++i) {
276 unsigned int delta(0);
280 stateEpsilon.push_back(
281 std::make_shared<internal_forces::muscles::State>(
286 m_model.muscularJointTorque(stateEpsilon));
287 for (
unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
289 (GeneralizedTorqueCalculEpsilon[i] - GeneralizedTorqueMusc[i]) /
292 std::cout << std::setprecision(20) << std::endl;
293 std::cout <<
"values[" << k - 1 <<
"]: " << values[k - 1]
295 std::cout <<
"GeneralizedTorqueCalculEpsilon[" << i
296 <<
"]: " << GeneralizedTorqueCalculEpsilon[i] << std::endl;
297 std::cout <<
"GeneralizedTorqueMusc[" << i
298 <<
"]: " << GeneralizedTorqueMusc[i] << std::endl;
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++];
315 jacobian(j, j + *
m_nbMus) = values[k++];
317 std::cout <<
"jacobian = " << std::endl << jacobian << std::endl;
326 const Ipopt::Number* x,
327 const Ipopt::Number*,
328 const Ipopt::Number*,
330 const Ipopt::Number*,
331 const Ipopt::Number*,
332 Ipopt::Number obj_value,
333 const Ipopt::IpoptData*,
334 Ipopt::IpoptCalculatedQuantities*) {
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()
348 std::cout <<
"GeneralizedTorque target = " <<
m_torqueTarget->transpose()
373 const Ipopt::Number* x) {
374 for (
unsigned int i = 0; i < *
m_nbMus; i++) {
375 (*m_activations)[i] = x[i];
380 (*m_torqueResidual)[i] = x[i + *
m_nbMus];
The actual musculoskeletal model that holds everything in biorbd.
EMG holder to interact with the muscle.
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.
virtual ~StaticOptimizationIpopt()
Destroy class properly.
Model & m_model
The model.
std::shared_ptr< utils::Vector > m_torqueResidual
The torque residual.
std::shared_ptr< std::vector< std::shared_ptr< State > > > m_states
The muscle states.
static const double m_activationUpperBound
Maximal muscle activation.
utils::Vector finalResidual() const
Return the final residual.
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< utils::Vector > m_activations
The activations.
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.
utils::Vector finalSolution() const
Return the final solution.
static const double m_activationLowerBound
Minimal muscle activation.
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.
std::shared_ptr< int > m_verbose
Verbose level of IPOPT.
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.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
static void raise(const String &message)
Throw an error message.
A wrapper for the Eigen::MatrixXd.
Wrapper of the Eigen VectorXd.