Biorbd Release_1.13.0
Loading...
Searching...
No Matches
StaticOptimizationIpoptLinearized.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/Muscles/StaticOptimizationIpoptLinearized.h"
3
4#include <algorithm>
5
6#include "BiorbdModel.h"
7#include "InternalForces/Muscles/State.h"
8#include "RigidBody/GeneralizedTorque.h"
9#include "Utils/Matrix.h"
10
11using namespace BIORBD_NAMESPACE;
12using namespace internal_forces;
13
16 Model &model,
19 const rigidbody::GeneralizedTorque &torqueTarget,
20 const utils::Vector &activationInit,
21 bool useResidual,
22 unsigned int pNormFactor,
23 int verbose,
24 double eps)
25 : internal_forces::muscles::StaticOptimizationIpopt(
26 model,
27 Q,
28 Qdot,
29 torqueTarget,
30 activationInit,
31 useResidual,
32 pNormFactor,
33 verbose,
34 eps),
35 m_jacobian(std::make_shared<utils::Matrix>(*m_nbDof, *m_nbMus)),
36 m_torquePassive(std::make_shared<utils::Vector>(*m_nbDof)) {
38}
39
42 m_model.updateMuscles(*m_Q, *m_Qdot, true);
43 std::vector<std::shared_ptr<internal_forces::muscles::State>> state_zero;
44 for (unsigned int i = 0; i < *m_nbMus; ++i) {
45 state_zero.push_back(
46 std::make_shared<internal_forces::muscles::State>(
48 }
49 const rigidbody::GeneralizedTorque &GeneralizedTorque_zero(
50 m_model.muscularJointTorque(state_zero));
51 *m_torquePassive = GeneralizedTorque_zero;
52 // Linearize each muscle between no activation and its reference activation
53 // (the initial guess, e.g. the solution of the previous frame). The active
54 // torque is then rescaled to a full activation so it is exact at the
55 // reference activation
56 for (unsigned int i = 0; i < *m_nbMus; ++i) {
57 utils::Scalar activationReference(std::min(
58 std::max((*m_activations)[i], m_activationLowerBound), 1.0));
59 std::vector<std::shared_ptr<internal_forces::muscles::State>> state;
60 for (unsigned int j = 0; j < *m_nbMus; ++j) {
61 state.push_back(
62 std::make_shared<internal_forces::muscles::State>(
64 0, j == i ? activationReference : 0)));
65 }
66 const rigidbody::GeneralizedTorque &GeneralizedTorque(
67 m_model.muscularJointTorque(state, *m_Q.get(), *m_Qdot.get()));
68 for (unsigned int j = 0; j < *m_nbTorque; ++j) {
69 (*m_jacobian)(j, i) =
70 (GeneralizedTorque(j) - GeneralizedTorque_zero(j)) /
71 activationReference;
72 }
73 }
74}
75
78
80 Ipopt::Index n,
81 const Ipopt::Number *x,
82 bool new_x,
83 Ipopt::Index m,
84 Ipopt::Number *g) {
85 assert(static_cast<unsigned int>(n) == *m_nbMus + *m_nbTorqueResidual);
86 assert(static_cast<unsigned int>(m) == *m_nbTorque);
87 if (new_x) {
88 dispatch(x);
89 }
90
91 utils::Vector res(static_cast<unsigned int>(m));
92 res.setZero();
93 // TODO Optimization using Eigen?
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];
97 }
98 g[i] = (*m_torquePassive)[i] + res[i] + (*m_torqueResidual)[i] -
99 (*m_torqueTarget)[i];
100 }
101 if (*m_verbose >= 2) {
102 std::cout << "GeneralizedTorque_musc_approximated = " << res.transpose()
103 << std::endl;
104 std::cout << "GeneralizedTorque_residual = "
105 << m_torqueResidual->transpose() << std::endl;
106 }
107 return true;
108}
109
111 Ipopt::Index,
112 const Ipopt::Number *x,
113 bool new_x,
114 Ipopt::Index m,
115 Ipopt::Index,
116 Ipopt::Index *iRow,
117 Ipopt::Index *jCol,
118 Ipopt::Number *values) {
119 if (new_x) {
120 dispatch(x);
121 }
122
123 if (values == nullptr) {
124 // Setup non-zeros values
125 Ipopt::Index k(0);
126 for (Ipopt::Index j = 0; static_cast<unsigned int>(j) < *m_nbMus; ++j) {
127 for (Ipopt::Index i = 0; i < m; ++i) {
128 iRow[k] = i;
129 jCol[k++] = j;
130 }
131 }
132 for (Ipopt::Index j = 0; static_cast<unsigned int>(j) < *m_nbTorqueResidual;
133 ++j) {
134 iRow[k] = j;
135 jCol[k++] = j + static_cast<Ipopt::Index>(*m_nbMus);
136 }
137 } else {
138 unsigned int k(0);
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);
142 }
143 for (unsigned int j = 0; j < *m_nbTorqueResidual; j++) {
144 values[k++] = 1;
145 }
146 }
147 return true;
148}
The actual musculoskeletal model that holds everything in biorbd.
EMG holder to interact with the muscle.
Definition State.h:17
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.
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.
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< 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.
std::shared_ptr< rigidbody::GeneralizedCoordinates > m_Q
The generalized coordinates.
std::shared_ptr< unsigned int > m_nbDof
The number of degrees of freedom.
Wrapper of the Eigen VectorXd.
Definition Vector.h:21