From c9faec2b8f35949f8fe2c37aa8740226d5fc6e00 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 16:20:27 -0400 Subject: [PATCH 1/4] hum --- .github/workflows/run_tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 38351cb6..8b6a58a7 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -44,7 +44,7 @@ jobs: - name: Run the actual tests on LINUX run: | sudo apt-get install xvfb - xvfb-run --server-args="-screen 0 1024x768x24" pytest -v --color=yes --cov-report term-missing --cov=bioviz tests + xvfb-run --server-args="-screen 0 1024x768x24" pytest -v --color=yes --cov-report term-missing --cov=bionc tests if: matrix.label == 'linux-64' - name: Run the actual tests From 2496407e02e7d88a8ca7b2fbe07dca465c10935b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Apr 2023 19:05:40 -0400 Subject: [PATCH 2/4] update --- bionc/bionc_casadi/biomechanical_model.py | 10 +++++++++- bionc/bionc_numpy/biomechanical_model.py | 2 ++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/bionc/bionc_casadi/biomechanical_model.py b/bionc/bionc_casadi/biomechanical_model.py index d84e4813..92b67b15 100644 --- a/bionc/bionc_casadi/biomechanical_model.py +++ b/bionc/bionc_casadi/biomechanical_model.py @@ -6,6 +6,7 @@ from .natural_velocities import NaturalVelocities from .natural_accelerations import NaturalAccelerations from ..protocols.biomechanical_model import GenericBiomechanicalModel +from .external_force import ExternalForceList class BiomechanicalModel(GenericBiomechanicalModel): @@ -601,6 +602,7 @@ def forward_dynamics( self, Q: NaturalCoordinates, Qdot: NaturalCoordinates, + external_forces: ExternalForceList = None, # external_forces: ExternalForces ): """ @@ -612,6 +614,8 @@ def forward_dynamics( The natural coordinates of the segment [12 * nb_segments, 1] Qdot : NaturalCoordinates The natural coordinates time derivative of the segment [12 * nb_segments, 1] + external_forces : ExternalForceList + The list of external forces applied on the system Returns ------- @@ -624,6 +628,10 @@ def forward_dynamics( K = self.holonomic_constraints_jacobian(Q) Kdot = self.holonomic_constraints_jacobian_derivative(Qdot) + external_forces = ( + ExternalForceList.empty_from_nb_segment(self.nb_segments) if external_forces is None else external_forces + ) + fext = external_forces.to_natural_external_forces(Q) # if stabilization is not None: # biais -= stabilization["alpha"] * self.rigid_body_constraint(Qi) + stabilization[ # "beta" @@ -636,7 +644,7 @@ def forward_dynamics( lower_KKT_matrix = horzcat(K, np.zeros((K.shape[0], K.shape[0]))) KKT_matrix = vertcat(upper_KKT_matrix, lower_KKT_matrix) - forces = self.weight() + forces = self.weight() + fext biais = -Kdot @ Qdot B = vertcat(forces, biais) diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index d748421b..dfcbf8c9 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -658,6 +658,8 @@ def forward_dynamics( ------- Qddot : NaturalAccelerations The natural accelerations [12 * nb_segments, 1] + lagrange_multipliers : np.ndarray + The lagrange multipliers [nb_holonomic_constraints, 1] """ G = self.mass_matrix K = self.holonomic_constraints_jacobian(Q) From 97754cdb48e442a36bfa4d502b374c28f60479f5 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Apr 2023 19:08:57 -0400 Subject: [PATCH 3/4] black --- bionc/bionc_casadi/external_force.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bionc/bionc_casadi/external_force.py b/bionc/bionc_casadi/external_force.py index 1ae61690..2b90f3b6 100644 --- a/bionc/bionc_casadi/external_force.py +++ b/bionc/bionc_casadi/external_force.py @@ -35,7 +35,9 @@ def __init__(self, application_point_in_local: np.ndarray | MX, external_forces: self.external_forces = MX(external_forces) @classmethod - def from_components(cls, application_point_in_local: np.ndarray | MX, force: np.ndarray | MX, torque: np.ndarray | MX): + def from_components( + cls, application_point_in_local: np.ndarray | MX, force: np.ndarray | MX, torque: np.ndarray | MX + ): """ This function creates an external force from its components. From 88ca152d5c2bdebdcbbc51e93574489bb095e4f6 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 19 Apr 2023 19:19:09 -0400 Subject: [PATCH 4/4] done --- examples/play_with_joints/two_constant_length.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/play_with_joints/two_constant_length.py b/examples/play_with_joints/two_constant_length.py index 05a4c023..67b00ed2 100644 --- a/examples/play_with_joints/two_constant_length.py +++ b/examples/play_with_joints/two_constant_length.py @@ -39,7 +39,7 @@ def build_two_link_segment(): NaturalMarker( name="point_A", parent_name="segment_0", - position=np.array([0, -1, 0]), + position=np.array([0, -1, 0.05]), is_technical=True, is_anatomical=False, ) @@ -49,7 +49,7 @@ def build_two_link_segment(): NaturalMarker( name="point_AA", parent_name="segment_0", - position=np.array([0, -1, 0.05]), + position=np.array([0, -1, -0.05]), is_technical=True, is_anatomical=False, ) @@ -59,7 +59,7 @@ def build_two_link_segment(): NaturalMarker( name="point_B", parent_name="segment_1", - position=np.array([0, 0, 0]), + position=np.array([0, 0, 0.05]), is_technical=True, is_anatomical=False, ) @@ -69,7 +69,7 @@ def build_two_link_segment(): NaturalMarker( name="point_BB", parent_name="segment_1", - position=np.array([0, 0, 0.05]), + position=np.array([0, 0, -0.05]), is_technical=True, is_anatomical=False, ) @@ -129,11 +129,11 @@ def main(initial_pose: str = "hanged"): 1, 0, 0, - 0.2 * np.cos(np.pi / 4), 0, - -(0.8 + 0.2 * np.sin(np.pi / 4)), 0.2 * np.cos(np.pi / 4), + -(0.8 + 0.2 * np.sin(np.pi / 4)), 0, + 0.2 * np.cos(np.pi / 4), -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8, 0, -1, @@ -203,5 +203,5 @@ def main(initial_pose: str = "hanged"): if __name__ == "__main__": - main("hanged") - # main("ready_to_swing") + # main("hanged") + main("ready_to_swing")