Python Static Optimization

Bindings/Python/examples/StaticOptimization/data/arm26_InverseKinematics.mot

@@ -0,0 +1,132 @@
+Coordinates
+version=1
+nRows=121
+nColumns=3
+inDegrees=yes
+
+Units are S.I. units (second, meters, Newtons, ...)
+Angles are in degrees.
+
+endheader
+time	r_shoulder_elev	r_elbow_flex
+      0.00000000	     -0.03394423	     -0.05440752
+      0.00833333	     -0.03657365	      0.01417949
+      0.01666667	     -0.03908443	      0.08815700
+      0.02500000	     -0.04126018	      0.17357577
+      0.03333333	     -0.04302262	      0.27576038
+      0.04166667	     -0.04431782	      0.39945264
+      0.05000000	     -0.04510954	      0.54869991
+      0.05833333	     -0.04537808	      0.72677247
+      0.06666667	     -0.04512001	      0.93612489
+      0.07500000	     -0.04434823	      1.17840384
+      0.08333333	     -0.04309148	      1.45449954
+      0.09166667	     -0.04139335	      1.76463436
+      0.10000000	     -0.03931062	      2.10847927
+      0.10833333	     -0.03691091	      2.48528740
+      0.11666667	     -0.03427009	      2.89403350
+      0.12500000	     -0.03146918	      3.33354847
+      0.13333333	     -0.02859119	      3.80263942
+      0.14166667	     -0.02571790	      4.30018788
+      0.15000000	     -0.02292668	      4.82522141
+      0.15833333	     -0.02028769	      5.37695629
+      0.16666667	     -0.01786127	      5.95481213
+      0.17500000	     -0.01569588	      6.55840109
+      0.18333333	     -0.01382651	      7.18749683
+      0.19166667	     -0.01227362	      7.84198911
+      0.20000000	     -0.01104271	      8.52183068
+      0.20833333	     -0.01063203	      9.22680499
+      0.21666667	     -0.01014548	      9.95712408
+      0.22500000	     -0.00994868	     10.71250561
+      0.23333333	     -0.00999836	     11.49266577
+      0.24166667	     -0.01024449	     12.29720264
+      0.25000000	     -0.01063082	     13.12558584
+      0.25833333	     -0.01109740	     13.97716626
+      0.26666667	     -0.01158331	     14.85119468
+      0.27500000	     -0.01202923	     15.74684640
+      0.28333333	     -0.01237995	     16.66324846
+      0.29166667	     -0.01258656	     17.59950631
+      0.30000000	     -0.01260837	     18.55472747
+      0.30833333	     -0.01241432	     19.52804021
+      0.31666667	     -0.01198399	     20.51860647
+      0.32500000	     -0.01058758	     21.52207195
+      0.33333333	     -0.00959073	     22.54440307
+      0.34166667	     -0.00836629	     23.58174648
+      0.35000000	     -0.00694125	     24.63344306
+      0.35833333	     -0.00534967	     25.69885458
+      0.36666667	     -0.00363212	     26.77735633
+      0.37500000	     -0.00183366	     27.86832623
+      0.38333333	     -0.00000136	     28.97113454
+      0.39166667	      0.00181800	     30.08513509
+      0.40000000	      0.00358032	     31.20965795
+      0.40833333	      0.00428682	     32.34819524
+      0.41666667	      0.00590309	     33.49117787
+      0.42500000	      0.00738472	     34.64241319
+      0.43333333	      0.00871270	     35.80113533
+      0.44166667	      0.00987599	     36.96654805
+      0.45000000	      0.01087179	     38.13783197
+      0.45833333	      0.01170564	     39.31414404
+      0.46666667	      0.01239170	     40.49461673
+      0.47500000	      0.01295331	     41.67835672
+      0.48333333	      0.01327011	     42.86586863
+      0.49166667	      0.01377298	     44.05247968
+      0.50000000	      0.01343804	     45.24176825
+      0.50833333	      0.01425886	     46.42911029
+      0.51666667	      0.01516679	     47.61407250
+      0.52500000	      0.01576730	     48.79835999
+      0.53333333	      0.01655188	     49.97933728
+      0.54166667	      0.01756672	     51.15600652
+      0.55000000	      0.01883929	     52.32746631
+      0.55833333	      0.02037991	     53.49290785
+      0.56666667	      0.02218621	     54.65157998
+      0.57500000	      0.02424569	     55.80276096
+      0.58333333	      0.02653710	     56.94574291
+      0.59166667	      0.02903146	     58.07982547
+      0.60000000	      0.03169288	     59.20431944
+      0.60833333	      0.03447969	     60.31855210
+      0.61666667	      0.03734550	     61.42186798
+      0.62500000	      0.04024030	     62.51362855
+      0.63333333	      0.04311165	     63.59321090
+      0.64166667	      0.04589217	     64.66132688
+      0.65000000	      0.04854040	     65.71428274
+      0.65833333	      0.05102086	     66.75336239
+      0.66666667	      0.05327580	     67.77793606
+      0.67500000	      0.05525461	     68.78735815
+      0.68333333	      0.05691445	     69.78095927
+      0.69166667	      0.05811323	     70.75864875
+      0.70000000	      0.05876137	     71.71835406
+      0.70833333	      0.05918762	     72.66006045
+      0.71666667	      0.05930941	     73.58300983
+      0.72500000	      0.05907709	     74.48642151
+      0.73333333	      0.05846323	     75.36950928
+      0.74166667	      0.05746128	     76.23150702
+      0.75000000	      0.05608208	     77.07169562
+      0.75833333	      0.05434973	     77.88942823
+      0.76666667	      0.05229773	     78.68415149
+      0.77500000	      0.04996565	     79.45542034
+      0.78333333	      0.04739652	     80.20290345
+      0.79166667	      0.04463464	     80.92637757
+      0.80000000	      0.04172377	     81.62570894
+      0.80833333	      0.03870563	     82.30082252
+      0.81666667	      0.03561853	     82.95165954
+      0.82500000	      0.03249633	     83.57812722
+      0.83333333	      0.02936748	     84.18004462
+      0.84166667	      0.02625434	     84.75709092
+      0.85000000	      0.02322870	     85.31019088
+      0.85833333	      0.01999313	     85.83566423
+      0.86666667	      0.01687593	     86.33409957
+      0.87500000	      0.01384316	     86.80436866
+      0.88333333	      0.01087758	     87.24519349
+      0.89166667	      0.00795763	     87.65522203
+      0.90000000	      0.00505936	     88.03313282
+      0.90833333	      0.00215732	     88.37776134
+      0.91666667	     -0.00077423	     88.68824068
+      0.92500000	     -0.00376022	     88.96414621
+      0.93333333	     -0.00682352	     89.20563380
+      0.94166667	     -0.00998396	     89.41355950
+      0.95000000	     -0.01325729	     89.58957023
+      0.95833333	     -0.01665443	     89.73615585
+      0.96666667	     -0.02018099	     89.85665620
+      0.97500000	     -0.02383697	     89.95521895
+      0.98333333	     -0.02761671	     90.03670904
+      0.99166667	     -0.03150905	     90.10657304
+      1.00000000	     -0.03549721	     90.17066638

Bindings/Python/examples/StaticOptimization/static_optim/constraints.py

@@ -0,0 +1,31 @@
+"""
+Constraints class.
+
+ConstraintAccelerationTarget create a strict constraint for the optimization such as the generalized accelerations
+computed from muscle activation equal the generalized accelerations from inverse kinematics.
+"""
+import numpy as np
+from scipy.optimize import approx_fprime
+
+
+class ConstraintAccelerationTarget:
+    def __init__(self):
+        pass
+
+    def constraints(self, x, idx=None):
+        """callback for calculating the constraints."""
+        qddot_from_muscles = self.forward_dynamics(x)
+        const = [
+            self.actual_qddot[idx_q] - qddot_from_muscles.get(idx_q)
+            for idx_q in range(qddot_from_muscles.size())
+        ]
+        if idx is not None:
+            return const[idx]
+        else:
+            return const
+
+    def jacobian(self, x):
+        jac = np.ndarray([self.n_dof, self.n_actuators])
+        for idof in range(self.n_dof):
+            jac[idof, :] = approx_fprime(x, self.constraints, 1e-10, idof)
+        return jac

Bindings/Python/examples/StaticOptimization/static_optim/dynamic_models.py

@@ -0,0 +1,208 @@
+"""
+Dynamic models.
+
+These are the actual classes to send to IPOPT.
+"""
+
+import numpy as np
+import opensim as osim
+
+from static_optim.constraints import ConstraintAccelerationTarget
+from static_optim.forces import ResidualForces, ExternalForces
+from static_optim.kinematic import KinematicModel
+from static_optim.objective import ObjMinimizeActivation
+
+
+class ClassicalStaticOptimization(
+    KinematicModel,
+    ObjMinimizeActivation,
+    ConstraintAccelerationTarget,
+    ResidualForces,
+    ExternalForces,
+):
+    """
+    Computes the muscle activations in order to minimize them while targeting the acceleration from inverse kinematics.
+    This is the most classical approach to Static Opimization.
+    """
+
+    # TODO: write tests
+    def __init__(
+        self,
+        model,
+        mot,
+        filter_param=None,
+        activation_exponent=2,
+        residual_actuator_xml=None,
+        external_load_xml=None,
+    ):
+        KinematicModel.__init__(self, model, mot, filter_param)
+        ObjMinimizeActivation.__init__(self, activation_exponent)
+        ResidualForces.__init__(self, residual_actuator_xml)
+        ExternalForces.__init__(self, external_load_xml)
+
+    def forward_dynamics(self, x):
+        # set residual forces
+        fs = self.model.getForceSet()
+        for i in range(self.n_muscles, fs.getSize()):
+            act = osim.ScalarActuator.safeDownCast(fs.get(i))
+            if act:
+                act.setOverrideActuation(self.state, x[i])
+
+        # update muscles
+        muscle_activation = x[: self.n_muscles]
+        for m in range(self.n_muscles):
+            self.muscle_actuators.get(m).setActivation(self.state, muscle_activation[m])
+        self.model.equilibrateMuscles(self.state)
+        self.model.realizeAcceleration(self.state)
+        return self.state.getUDot()
+
+
+class ClassicalOptimizationLinearConstraints(
+    KinematicModel,
+    ObjMinimizeActivation,
+    ConstraintAccelerationTarget,
+    ResidualForces,
+    ExternalForces,
+):
+    """
+    Intends to mimic the classical approach but with the constraints linearized.
+    It makes the assumption that muscle length is constant at a particular position and velocity, whatever the muscle
+    activation.
+    """
+
+    # TODO: write tests
+    def __init__(
+        self,
+        model,
+        mot,
+        filter_param=None,
+        activation_exponent=2,
+        residual_actuator_xml=None,
+        external_load_xml=None,
+        muscle_physiology=True,
+    ):
+        self.muscle_physiology = muscle_physiology
+        self.previous_activation = np.array([])
+
+        KinematicModel.__init__(self, model, mot, filter_param)
+        ObjMinimizeActivation.__init__(self, activation_exponent)
+        ResidualForces.__init__(self, residual_actuator_xml)
+        ExternalForces.__init__(self, external_load_xml)
+
+        # prepare linear constraints variables
+        self.previous_activation = np.ones(self.n_muscles)
+        self.qddot_from_nl = []
+        self.constraint_vector = []
+        self.optimal_forces = []
+        self.passive_forces = []
+        self.constraint_matrix = []
+        self.jacobian_matrix = []  # Precomputed jacobian
+        self._prepare_constraints()
+
+    def forward_dynamics(self, x):
+        fs = self.model.getForceSet()
+        for i in range(fs.getSize()):
+            act = osim.ScalarActuator.safeDownCast(fs.get(i))
+            if act:
+                act.setOverrideActuation(self.state, x[i] * self.optimal_forces[i])
+
+        self.model.realizeAcceleration(self.state)
+        return self.state.getUDot()
+
+    def passive_forward_dynamics(self):
+        fs = self.model.getForceSet()
+        for i in range(fs.getSize()):
+            act = osim.ScalarActuator.safeDownCast(fs.get(i))
+            if act:
+                act.setOverrideActuation(self.state, self.passive_forces[i])
+
+        self.model.realizeAcceleration(self.state)
+        qddot = self.state.getUDot()
+        return [qddot.get(idx_q) for idx_q in range(qddot.size())]
+
+    def upd_model_kinematics(self, frame):
+        super().upd_model_kinematics(frame)
+        if self.previous_activation.size > 0:
+            self._prepare_constraints()
+
+    def _prepare_constraints(self):
+        fs = self.model.getForceSet()
+        for i in range(fs.getSize()):
+            act = osim.ScalarActuator.safeDownCast(fs.get(i))
+            if act:
+                act.overrideActuation(self.state, False)
+
+        self.passive_forces = []
+        for m in range(self.n_muscles):
+            self.muscle_actuators.get(m).setActivation(self.state, self.previous_activation[m])
+        self.model.equilibrateMuscles(self.state)
+        self.model.realizeVelocity(self.state)
+        for m in range(self.n_muscles):
+            self.passive_forces.append(self.muscle_actuators.get(m).getPassiveFiberForceAlongTendon(self.state))
+
+        forces = self.model.getForceSet()
+        self.optimal_forces = []
+        imus = 0
+        for i in range(forces.getSize()):
+            muscle = osim.Muscle.safeDownCast(forces.get(i))
+            if muscle:
+                if self.muscle_physiology:
+                    self.model.setAllControllersEnabled(True)
+                    self.optimal_forces.append(muscle.getActiveFiberForceAlongTendon(self.state) /
+                                               self.previous_activation[i])
+                    self.model.setAllControllersEnabled(False)
+                else:
+                    self.optimal_forces.append(muscle.getMaxIsometricForce())
+                imus += 1
+            coordinate = osim.CoordinateActuator.safeDownCast(forces.get(i))
+            if coordinate:
+                self.optimal_forces.append(coordinate.getOptimalForce())
+
+        self.linear_constraints()
+
+    def constraints(self, x, idx=None):
+        # x = self.previous_activation
+        x_tp = x.reshape((x.shape[0], 1))
+        x_mul = np.ndarray((self.constraint_matrix.shape[0], x_tp.shape[1]))
+        np.matmul(self.constraint_matrix, x_tp, x_mul)
+        x_constraint = x_mul.ravel()
+
+        # That is what really happening
+        # const = self.actual_qddot - (x_constraint + self.passive_forward_dynamics())
+        const = x_constraint - self.constraint_vector
+
+        if idx is not None:
+            return const[idx]
+        else:
+            return const
+
+    def linear_constraints(self):
+        fs = self.model.getForceSet()
+        for i in range(fs.getSize()):
+            act = osim.ScalarActuator.safeDownCast(fs.get(i))
+            if act:
+                act.overrideActuation(self.state, True)
+        p_vector = np.zeros(self.n_actuators)
+
+        qddot_from_nl_tp = self.forward_dynamics(p_vector)
+        self.qddot_from_nl = np.array(
+            [qddot_from_nl_tp.get(idx_q) for idx_q in range(qddot_from_nl_tp.size())])
+
+        self.constraint_matrix = np.zeros((self.n_dof, self.n_actuators))
+
+        for p in range(self.n_actuators):
+            p_vector[p] = 1
+            qddot_from_muscles_tp = self.forward_dynamics(p_vector)
+            qddot_from_muscles = np.array([qddot_from_muscles_tp.get(idx_q) for idx_q in range(qddot_from_muscles_tp.size())])
+            self.constraint_matrix[:, p] = qddot_from_muscles - self.qddot_from_nl
+            p_vector[p] = 0
+
+        self.constraint_vector = np.array(self.actual_qddot) - np.array(self.passive_forward_dynamics())
+
+    def jacobian(self, x):
+        return self.constraint_matrix
+
+    def set_previous_activation(self, x):
+        x[x <= 0] = 0.01
+        x[x > 1] = 1
+        self.previous_activation = np.array(x)