Skip to content

Commit 397c7f1

Browse files
pariterrepariterre
authored
Merge pull request #23 from pariterre/PlayingOnString
Playing on string
2 parents 49ee617 + 888d499 commit 397c7f1

14 files changed

Lines changed: 335 additions & 73 deletions

‎analyses/show_optim_control.py‎

Lines changed: 44 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
import numpy as np
12
import matplotlib.pyplot as plt
23
import biorbd
34
from BiorbdViz import BiorbdViz
@@ -10,9 +11,30 @@
1011
output_files = "AvNPhases"
1112
fun_dyn = utils.dynamics_from_muscles_and_torques
1213
runge_kutta_algo = 'rk45'
13-
nb_nodes = 30
14+
nb_intervals = 30
1415
nb_phases = 2
1516
nb_frame_inter = 500
17+
muscle_plot_mapping = \
18+
[[14, 7, 0, 0, 0, 0], # Trapeze1
19+
[15, 7, 1, 0, 0, 0], # Trapeze2
20+
[16, 8, 0, 0, 0, 0], # Trapeze3
21+
[17, 8, 1, 0, 0, 0], # Trapeze4
22+
[10, 5, 2, 1, 0, 1], # SupraSpin
23+
[8, 5, 0, 1, 0, 1], # InfraSpin
24+
[11, 5, 3, 1, 0, 1], # SubScap
25+
[6, 4, 0, 0, 1, 2], # Pectoral1
26+
[0, 0, 0, 0, 1, 2], # Pectoral2
27+
[1, 0, 1, 0, 1, 2], # Pectoral3
28+
[7, 4, 1, 1, 1, 3], # Deltoid1
29+
[9, 5, 1, 1, 1, 3], # Deltoid2
30+
[2, 1, 0, 1, 1, 3], # Deltoid3
31+
[12, 6, 0, 2, 0, 4], # BicepsLong
32+
[13, 6, 1, 2, 0, 4], # BicepsShort
33+
[3, 2, 0, 2, 1, 5], # TricepsLong
34+
[5, 3, 1, 2, 1, 5], # TricepsMed
35+
[4, 3, 0, 2, 1, 5], # TricepsLat
36+
]
37+
muscle_plot_names = ["Trapèzes", "Coiffe des rotateurs", "Pectoraux", "Deltoïdes", "Biceps", "Triceps"]
1638

1739
# Load the model
1840
m = biorbd.Model(f"../models/{model_name}.bioMod")
@@ -29,11 +51,11 @@
2951
raise NotImplementedError("Dynamic not implemented yet")
3052

3153
# Read values
32-
t, all_q, all_qdot = utils.read_acado_output_states(f"../optimal_control/Results/States{output_files}.txt", m, nb_nodes,
54+
t, all_q, all_qdot = utils.read_acado_output_states(f"../optimal_control/Results/States{output_files}.txt", m, nb_intervals,
3355
nb_phases)
34-
all_u = utils.read_acado_output_controls(f"../optimal_control/Results/Controls{output_files}.txt", nb_nodes, nb_phases,
56+
all_u = utils.read_acado_output_controls(f"../optimal_control/Results/Controls{output_files}.txt", nb_intervals, nb_phases,
3557
nb_controls)
36-
t_final = utils.organize_time(f"../optimal_control/Results/Parameters{output_files}.txt", t, nb_phases, nb_nodes, parameter=False)
58+
t_final = utils.organize_time(f"../optimal_control/Results/Parameters{output_files}.txt", t, nb_phases, nb_intervals, parameter=False)
3759

3860

3961
# Integrate
@@ -62,30 +84,32 @@
6284
# plt.plot(t_interp, utils.derive(q_interp, t_interp), '--')
6385
plt.title("Qdot %i" % i)
6486

65-
# for i in range(nb_controls):
66-
# plt.subplot(nb_controls, 3, 3 + (3 * i))
67-
# utils.plot_piecewise_constant(t_final, all_u[i, :])
68-
# plt.title("Acceleration %i" % i)
69-
7087
for i in range(m.nbGeneralizedTorque()):
7188
plt.subplot(m.nbGeneralizedTorque(), 3, 3 + (3 * i))
7289
utils.plot_piecewise_constant(t_final, all_u[m.nbMuscleTotal()+i, :])
7390
plt.title("Torques %i" % i)
91+
plt.tight_layout(w_pad=-1.0, h_pad=-1.0)
7492

75-
# L = []
76-
# for i in range(m.nbMuscleGroups()):
77-
# L.append(m.muscleGroup(i).nbMuscles())
78-
# nb_muscles_max = max(L)
7993
plt.figure("Activations")
8094
cmp = 0
81-
for i in range(m.nbMuscleGroups()):
82-
for j in range(m.muscleGroup(i).nbMuscles()):
83-
#plt.subplot(nb_muscles_max, m.nbMuscleGroups(), i+1+(m.nbMuscleGroups()*j))
84-
plt.subplot(3, 6, cmp+1)
85-
utils.plot_piecewise_constant(t_final, all_u[cmp, :])
86-
plt.title(biorbd.HillType.getRef(m.muscleGroup(i).muscle(j)).name().getString())
95+
if muscle_plot_mapping is None:
96+
for i in range(m.nbMuscleGroups()):
97+
for j in range(m.muscleGroup(i).nbMuscles()):
98+
plt.subplot(3, 6, cmp + 1)
99+
utils.plot_piecewise_constant(t_final, all_u[cmp, :])
100+
plt.title(m.muscleGroup(i).muscle(j).name().getString())
101+
plt.ylim((0, 1))
102+
cmp += 1
103+
else:
104+
nb_row = np.max(muscle_plot_mapping, axis=0)[3] + 1
105+
nb_col = np.max(muscle_plot_mapping, axis=0)[4] + 1
106+
for muscle_map in muscle_plot_mapping:
107+
plt.subplot(nb_row, nb_col, muscle_map[3] * nb_col + muscle_map[4] + 1)
108+
utils.plot_piecewise_constant(t_final, all_u[muscle_map[0], :])
109+
# plt.title(m.muscleGroup(map[1]).muscle(map[2]).name().getString())
110+
plt.title(muscle_plot_names[muscle_map[5]])
87111
plt.ylim((0, 1))
88-
cmp += 1
112+
plt.tight_layout(w_pad=-1.0, h_pad=-1.0)
89113

90114
# plt.ion() # Non blocking plt.show
91115
plt.show()

‎analyses/utils.py‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,7 @@ def dynamics_from_muscles_and_torques(t_int, states, biorbd_model, u):
127127
for i in range(len(states_dynamics)):
128128
states_dynamics[i] = biorbd.StateDynamics(0, u[i])
129129

130-
biorbd_model.updateMuscles(biorbd_model, states[:nb_q], states[nb_q:], True)
130+
biorbd_model.updateMuscles(states[:nb_q], states[nb_q:], True)
131131
tau = biorbd.Model.muscularJointTorque(biorbd_model, states_dynamics, states[:nb_q], states[nb_q:])
132132

133133
tau_final = tau.get_array() + u[nb_muscle:nb_muscle+nb_tau]
@@ -178,6 +178,19 @@ def dynamics_from_joint_torque(t_int, states, biorbd_model, u):
178178
return rsh
179179

180180

181+
def dynamics_from_joint_torque_and_contact(t_int, states, biorbd_model, u):
182+
nb_q = biorbd_model.nbQ()
183+
nb_qdot = biorbd_model.nbQdot()
184+
cs = biorbd_model.getConstraints()
185+
qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(biorbd_model, states[:nb_q], states[nb_q:], u, cs).get_array()
186+
rsh = np.ndarray(nb_q + nb_qdot)
187+
for i in range(nb_q):
188+
rsh[i] = states[nb_q+i]
189+
rsh[i + nb_q] = qddot[i]
190+
191+
return rsh
192+
193+
181194
def dynamics_from_accelerations(t_int, states, biorbd_model, u):
182195
nb_q = biorbd_model.nbQ()
183196
nb_qdot = biorbd_model.nbQdot()
@@ -231,8 +244,13 @@ def integrate_states_from_controls(biorbd_model, t, all_q, all_qdot, all_u, dyn_
231244
q_init = integrated_tp.y[:, -1]
232245
else:
233246
q_init = np.concatenate((all_q[:, interval+1], all_qdot[:, interval+1]))
234-
all_t = np.concatenate((all_t, integrated_tp.t[:-1]))
235-
integrated_state = np.concatenate((integrated_state, integrated_tp.y[:, :-1]), axis=1)
247+
248+
if interval < t.shape[0] - 2:
249+
all_t = np.concatenate((all_t, integrated_tp.t[:-1]))
250+
integrated_state = np.concatenate((integrated_state, integrated_tp.y[:, :-1]), axis=1)
251+
else:
252+
all_t = np.concatenate((all_t, integrated_tp.t[:]))
253+
integrated_state = np.concatenate((integrated_state, integrated_tp.y[:, :]), axis=1)
236254

237255
if verbose:
238256
print(f"Time: {t[interval]}")
@@ -274,13 +292,11 @@ def plot_piecewise_constant(t, data):
274292
def plot_piecewise_linear(t, data):
275293
plt.plot(t, data)
276294

295+
277296
def derive(q, t):
278297
der = np.ndarray(q.shape)
279298
for i in range(q.shape[1]):
280299
for j in range(q.shape[0]-1):
281300
der[j][i] = (q[j+1][i]-q[j][i])/(t[j+1]-t[j])
282301

283302
return der
284-
285-
286-

‎models/BrasViolon.bioMod‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -482,14 +482,17 @@ external_forces 0
482482
position 0 0 0.2425
483483
endmarker
484484

485-
loopconstraint
486-
predecessor Archet
487-
successor Violon
488-
RTpredecessor 0 0 0 xyz 0 0 0.370
489-
RTsuccessor -1.57 0 -2 xyz 0 0 0.2425
490-
axis 1 1 1 1 1 0
491-
stabilizationParameter 0.6
492-
endloopconstraint
485+
486+
487+
488+
// loopconstraint
489+
// predecessor Archet
490+
// successor Violon
491+
// RTpredecessor 0 0 0 xyz 0 0 0.370
492+
// RTsuccessor -1.57 0 -2 xyz 0 0 0.2425
493+
// axis 1 1 1 1 1 0
494+
// stabilizationParameter 0.6
495+
// endloopconstraint
493496

494497
// DEFINITION DES MUSCLES
495498

‎models/ModelTest.bioMod‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
version 3
2+
3+
// Informations générales
4+
root_actuated 1
5+
external_forces 0
6+
7+
// DEFINITION DES SEGMENTS
8+
9+
10+
segment Cube1
11+
translations x //z
12+
// rotations xyz
13+
14+
mass 3.00000
15+
16+
inertia
17+
1.0000000000 0.0000000000 0.0000000000
18+
0.0000000000 1.0000000000 0.0000000000
19+
0.0000000000 0.0000000000 1.0000000000
20+
21+
com 0.0000000000 0.50000000000 0
22+
mesh -1 -1 -1
23+
mesh -1 1 -1
24+
mesh -1 1 1
25+
mesh -1 -1 1
26+
mesh -1 -1 -1
27+
mesh 1 -1 -1
28+
mesh 1 1 -1
29+
mesh -1 1 -1
30+
mesh 1 1 -1
31+
mesh 1 1 1
32+
mesh -1 1 1
33+
mesh 1 1 1
34+
mesh 1 -1 1
35+
mesh -1 -1 1
36+
mesh 1 -1 1
37+
mesh 1 -1 -1
38+
endsegment
39+
40+

‎optimal_control/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ target_include_directories(utils PUBLIC
4040
# Write down all files to include here
4141
set(source_files
4242
RepeatedUpAndDownBow.cpp
43+
EocarNphases.cpp
4344
)
4445

4546
# Setup each project

‎optimal_control/EocarNphases.cpp‎

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
#include <acado_optimal_control.hpp>
2+
#include <bindings/acado_gnuplot/gnuplot_window.hpp>
3+
#include "BiorbdModel.h"
4+
#include "includes/dynamics.h"
5+
#include "includes/objectives.h"
6+
#include "includes/constraints.h"
7+
#include <vector>
8+
9+
using namespace std;
10+
USING_NAMESPACE_ACADO
11+
12+
/* ---------- Model ---------- */
13+
biorbd::Model m("../../models/ModelTest.bioMod");
14+
unsigned int nQ(m.nbQ()); // states number
15+
unsigned int nQdot(m.nbQdot()); // derived states number
16+
unsigned int nTau(m.nbGeneralizedTorque()); // controls number
17+
unsigned int nMarkers(m.nMarkers()); // markers number
18+
unsigned int nMus(m.nbMuscleTotal()); // muscles number
19+
unsigned int nPhases(2); // phases number
20+
GeneralizedCoordinates Q(nQ), Qdot(nQdot), Qddot(nQdot);
21+
GeneralizedTorque Tau(nTau);
22+
std::vector<std::shared_ptr<biorbd::muscles::StateDynamics>> musclesStates(nMus);
23+
24+
const double t_Start = 0.0;
25+
const double t_End = 4.0;
26+
const int nPoints(31);
27+
28+
29+
int main ()
30+
{
31+
for (unsigned int i=0; i<nMus; ++i)
32+
musclesStates[i] = std::make_shared<biorbd::muscles::StateDynamics>(biorbd::muscles::StateDynamics());
33+
// printf( "nQdot vaut : %d \n", nQdot);
34+
// printf( "nQ vaut : %d \n", nQ);
35+
// printf( "nTau vaut : %d \n", nTau);
36+
// printf( "nMarkers vaut : %d \n", nMarkers);
37+
// printf( "nMus vaut : %d \n", nMus);
38+
39+
/* ---------- INITIALIZATION ---------- */
40+
41+
// DifferentialState x1("", nQ+nQdot, 1);
42+
// Control u1("", nMus+nTau, 1);
43+
// DifferentialEquation f;
44+
// IntermediateState is1("", nQ+nQdot+nMus+nTau, 1);
45+
46+
/* ---------- INITIALIZATION ---------- */
47+
std::vector<DifferentialState> x1;
48+
std::vector<Control> u1;
49+
std::vector<IntermediateState> is1;
50+
51+
52+
for (unsigned int p=0; p<nPhases; ++p){
53+
x1.push_back(DifferentialState("",nQ+nQdot,1));
54+
u1.push_back(Control("", nMus+nTau, 1));
55+
is1.push_back(IntermediateState(nQ + nQdot + nMus + nTau));
56+
57+
58+
59+
60+
for (unsigned int i = 0; i < nQ + nQdot; ++i) // On remplit le IntermediateState avec les X
61+
is1[p](i) = x1[p](i);
62+
63+
64+
65+
for (unsigned int i = 0; i < nMus + nTau; ++i) // Puis avec les controles
66+
is1[p](i+nQ+nQdot) = u1[p](i);
67+
68+
}
69+
70+
71+
// /* ----------- DEFINE OCP ------------- */
72+
// OCP ocp( t_Start , t_End , nPoints-1);
73+
// CFunction mayer(1, mayerVelocity); // Dans une sortie, on fais la somme des vitesses au carré
74+
// CFunction lagrange(1, lagrangeResidualTorques);
75+
// ocp.minimizeLagrangeTerm(lagrange(u1)); // On minimise le terme de Lagrange
76+
// ocp.minimizeMayerTerm(mayer(is1)); // On minimise le terme de Mayer
77+
78+
/* ----------- DEFINE OCP ------------- */
79+
OCP ocp(t_Start, t_End, nPoints);
80+
CFunction lagrangeRT(1, lagrangeResidualTorques);
81+
CFunction lagrangeA(1, lagrangeActivations);
82+
CFunction F(nQ+nQdot, forwardDynamicsFromJointTorque);
83+
DifferentialEquation f ;
84+
85+
86+
/* ------------ CONSTRAINTS ----------- */
87+
88+
for (unsigned int p=0; p<nPhases; ++p){
89+
90+
(f << dot( x1[p] )) == F(is1[p]);
91+
92+
93+
if (p == 0) {
94+
95+
ocp.subjectTo( AT_START, x1[p](0) == 0 );
96+
ocp.subjectTo( AT_END, x1[p](0) == 10 );
97+
98+
ocp.subjectTo( AT_START, x1[p](1) == 0);
99+
ocp.subjectTo( AT_END, x1[p](1) == 0);
100+
}
101+
else {
102+
ocp.subjectTo( 0.0, x1[p], -x1[p-1], 0.0 );
103+
ocp.subjectTo( 0.0, x1[p-1], -x1[p], 0.0 );
104+
}
105+
// ocp.subjectTo( AT_START, x1(2) == 0 );
106+
// ocp.subjectTo( AT_END, x1(2) == 0 );
107+
108+
// ocp.subjectTo( AT_START, x1(3) == 0 );
109+
// ocp.subjectTo( AT_END, x1(3) == 0 );
110+
111+
112+
for (unsigned int i=0; i<nMus; ++i)
113+
ocp.subjectTo(0.01 <= u1[p](i) <= 1);
114+
115+
116+
117+
for (unsigned int i=nMus; i<nMus+nTau; ++i)
118+
ocp.subjectTo(-100 <= u1[p](i) <= 100);
119+
}
120+
ocp.subjectTo( f ) ;
121+
/* ------------ OBJECTIVE ----------- */
122+
Expression sumLagrange = lagrangeRT(u1[0])+ lagrangeA(u1[0]);
123+
for(unsigned int p=1; p<nPhases; ++p)
124+
sumLagrange += lagrangeRT(u1[p]) + lagrangeA(u1[p]);
125+
126+
ocp.minimizeLagrangeTerm( sumLagrange ); // WARNING
127+
128+
129+
130+
131+
/* ---------- VISUALIZATION ------------ */
132+
133+
GnuplotWindow window; // visualize the results in a Gnuplot window
134+
for (unsigned int p=0; p<nPhases; ++p){
135+
136+
window.addSubplot( x1[p](0), "Etat 0 " );
137+
window.addSubplot( x1[p](1), "Etat 1 " );
138+
window.addSubplot( u1[p](0), "CONTROL 1" ) ;
139+
}
140+
141+
/* ---------- OPTIMIZATION ------------ */
142+
143+
OptimizationAlgorithm algorithm( ocp ) ; // construct optimization algorithm ,
144+
algorithm.set(MAX_NUM_ITERATIONS, 500);
145+
146+
algorithm << window;
147+
algorithm.solve();
148+
149+
algorithm.getDifferentialStates("../Results/StatesEocar.txt");
150+
algorithm.getControls("../Results/ControlsEocar.txt");
151+
152+
return 0;
153+
}

0 commit comments

Comments
 (0)