[RTR] Holonomic muscle model

bioptim/dynamics/state_space_dynamics/torque_dynamics_holonomic.py

@@ -55,8 +55,8 @@ def dynamics(
 
         defects = None
         if isinstance(nlp.dynamics_type.ode_solver, OdeSolver.COLLOCATION):
-            slope_q = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx)
-            slope_qdot = DynamicsFunctions.get(nlp.states_dot["qddot_u"], nlp.states_dot.scaled.cx)
+            slope_q = DynamicsFunctions.get(nlp.states_dot["q_u"], nlp.states_dot.scaled.cx)
+            slope_qdot = DynamicsFunctions.get(nlp.states_dot["qdot_u"], nlp.states_dot.scaled.cx)
             if nlp.dynamics_type.ode_solver.defects_type == DefectType.QDDOT_EQUALS_FORWARD_DYNAMICS:
                 qddot_u = nlp.model.partitioned_forward_dynamics()(q_u, qdot_u, q_v_init, tau)
                 derivative = vertcat(qdot_u, qddot_u)

bioptim/examples/models/3bar.bioMod

@@ -0,0 +1,98 @@
+version 4
+
+segment Seg0
+    rotations	x
+    ranges
+        -10*pi 10*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 0
+    marker marker_0
+        parent Seg0
+        position 0 0 0
+    endmarker
+
+    // Marker 1
+    marker marker_1
+        parent Seg0
+        position 0 0 -1
+    endmarker
+
+    // Marker 2
+    marker CoM0
+        parent Seg0
+        position -0.0005 0.0688 -0.9542
+    endmarker
+
+segment Seg1
+    translations    yz
+    rotations	x
+    ranges
+        -5 5
+        -5 5
+        -3*pi 3*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 3
+    marker marker_3
+        parent Seg1
+        position 0 0 0
+    endmarker
+
+    // Marker 4
+    marker marker_4
+        parent Seg1
+        position 0 0 -1
+    endmarker
+
+    // Marker CoM1
+    marker CoM1
+        parent Seg1
+        position -0.0005 0.0688 -0.9542
+    endmarker
+
+segment Seg2
+    RT 0 0 0 xyz 0 1 0
+    rotations	x
+    ranges
+        -10*pi 10*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 5
+    marker marker_5
+        parent Seg2
+        position 0 0 0
+    endmarker
+
+    // Marker 6
+    marker marker_6
+        parent Seg2
+        position 0 0 -1
+    endmarker
+
+    // Marker CoM3
+    marker CoM3
+        parent Seg2
+        position -0.0005 0.0688 -0.9542
+    endmarker

bioptim/examples/models/4bar.bioMod

@@ -0,0 +1,132 @@
+version 4
+
+segment Seg0
+    rotations	x
+    ranges
+        -10*pi 10*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 0
+    marker marker_0
+        parent Seg0
+        position 0 0 0
+    endmarker
+
+    // Marker 1
+    marker marker_1
+        parent Seg0
+        position 0 0 -1
+    endmarker
+
+    // Marker 2
+    marker CoM0
+        parent Seg0
+        position -0.0005 0.0688 -0.9542
+    endmarker
+
+segment Seg1
+    translations    yz
+    rotations	x
+    ranges
+        -5 5
+        -5 5
+        -3*pi 3*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 3
+    marker marker_3
+        parent Seg1
+        position 0 0 0
+    endmarker
+
+    // Marker 4
+    marker marker_4
+        parent Seg1
+        position 0 0 -1
+    endmarker
+
+    // Marker CoM1
+    marker CoM1
+        parent Seg1
+        position -0.0005 0.0688 -0.9542
+    endmarker
+
+segment Seg2
+    RT 0 0 0 xyz 0 1 0
+    rotations	x
+    ranges
+        -10*pi 10*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 5
+    marker marker_5
+        parent Seg2
+        position 0 0 0
+    endmarker
+
+    // Marker 6
+    marker marker_6
+        parent Seg2
+        position 0 0 -1
+    endmarker
+
+    // Marker CoM3
+    marker CoM3
+        parent Seg2
+        position -0.0005 0.0688 -0.9542
+    endmarker
+
+segment Seg3
+    translations    yz
+    rotations	x
+    ranges
+        -5 5
+        -5 5
+        -3*pi 3*pi
+    mass 0.25
+    inertia
+        0.00391  0.0000	0.0000
+	    0.0000	0.0335	-0.0032
+	    0.0000	-0.0032	0.0090
+    com  -0.0005 0.0688 -0.9542
+    meshfile mesh/pendulum.STL
+endsegment
+
+    // Marker 7
+    marker marker_7
+        parent Seg3
+        position 0 0 0
+    endmarker
+
+    // Marker 8
+    marker marker_8
+        parent Seg3
+        position 0 0 -1
+    endmarker
+
+    // Marker CoM4
+    marker CoM4
+        parent Seg3
+        position -0.0005 0.0688 -0.9542
+    endmarker