1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/StaticOptimizationIpoptLinearized.h"
6#include "BiorbdModel.h"
7#include "InternalForces/Muscles/State.h"
8#include "RigidBody/GeneralizedTorque.h"
9#include "Utils/Matrix.h"
11using namespace BIORBD_NAMESPACE;
12using namespace internal_forces;
22 unsigned int pNormFactor,
43 std::vector<std::shared_ptr<internal_forces::muscles::State>> state_zero;
44 for (
unsigned int i = 0; i < *
m_nbMus; ++i) {
46 std::make_shared<internal_forces::muscles::State>(
50 m_model.muscularJointTorque(state_zero));
56 for (
unsigned int i = 0; i < *
m_nbMus; ++i) {
57 utils::Scalar activationReference(std::min(
59 std::vector<std::shared_ptr<internal_forces::muscles::State>> state;
60 for (
unsigned int j = 0; j < *
m_nbMus; ++j) {
62 std::make_shared<internal_forces::muscles::State>(
64 0, j == i ? activationReference : 0)));
68 for (
unsigned int j = 0; j < *
m_nbTorque; ++j) {
70 (GeneralizedTorque(j) - GeneralizedTorque_zero(j)) /
81 const Ipopt::Number *x,
86 assert(
static_cast<unsigned int>(m) == *
m_nbTorque);
94 for (
unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
95 for (
unsigned int j = 0; j < *
m_nbMus; j++) {
96 res[i] += (*m_jacobian)(i, j) * x[j];
98 g[i] = (*m_torquePassive)[i] + res[i] + (*m_torqueResidual)[i] -
102 std::cout <<
"GeneralizedTorque_musc_approximated = " << res.transpose()
104 std::cout <<
"GeneralizedTorque_residual = "
112 const Ipopt::Number *x,
118 Ipopt::Number *values) {
123 if (values ==
nullptr) {
126 for (Ipopt::Index j = 0;
static_cast<unsigned int>(j) < *
m_nbMus; ++j) {
127 for (Ipopt::Index i = 0; i < m; ++i) {
135 jCol[k++] = j +
static_cast<Ipopt::Index
>(*m_nbMus);
139 for (
unsigned int j = 0; j < *
m_nbMus; j++)
140 for (
unsigned int i = 0; i < static_cast<unsigned int>(m); i++) {
141 values[k++] = (*m_jacobian)(i, j);
The actual musculoskeletal model that holds everything in biorbd.
EMG holder to interact with the muscle.
virtual ~StaticOptimizationIpoptLinearized()
Destroy class properly.
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.
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.
std::shared_ptr< utils::Matrix > m_jacobian
The constraints jacobian.
void prepareJacobian()
Setup the constant constraints jacobian.
StaticOptimizationIpoptLinearized(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.
std::shared_ptr< utils::Vector > m_torquePassive
The muscle torque when not activated.
Model & m_model
The model.
std::shared_ptr< utils::Vector > m_torqueResidual
The torque 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.
std::shared_ptr< unsigned int > m_nbMus
The number of muscles.
std::shared_ptr< utils::Vector > m_activations
The activations.
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.
std::shared_ptr< rigidbody::GeneralizedVelocity > m_Qdot
The generalized velocities.
static const double m_activationLowerBound
Minimal muscle activation.
std::shared_ptr< rigidbody::GeneralizedCoordinates > m_Q
The generalized coordinates.
std::shared_ptr< int > m_verbose
Verbose level of IPOPT.
std::shared_ptr< unsigned int > m_nbDof
The number of degrees of freedom.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
Wrapper of the Eigen VectorXd.