From cb2754323ccc5876705664761c411215041004fb Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 10 Mar 2023 22:46:15 -0500 Subject: [PATCH 01/40] docs --- bionc/bionc_numpy/joint.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index 4262b8fa..432761ad 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -870,7 +870,7 @@ def constraint_jacobian_derivative( Returns ------- - MX + np.ndarray joint constraints jacobian of the child segment [5, 12] """ From c2f4c516e89fc08b6bafe8d6de429c84d77d2298 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Wed, 22 Mar 2023 14:49:38 -0400 Subject: [PATCH 02/40] push with raphael --- bionc/bionc_casadi/joint.py | 15 +- bionc/bionc_numpy/joint.py | 16 +- .../knee_parallel_mechanism/knee_feikes.py | 252 ++++++++++++++++++ .../{knee.py => knee_sancisi.py} | 132 +++++---- .../knee_parallel_mechanism/simulation.py | 8 +- .../simulation_feikes.py | 66 +++++ examples/models/lower_limb.nc | Bin 16716 -> 16815 bytes 7 files changed, 405 insertions(+), 84 deletions(-) create mode 100644 examples/knee_parallel_mechanism/knee_feikes.py rename examples/knee_parallel_mechanism/{knee.py => knee_sancisi.py} (77%) create mode 100644 examples/knee_parallel_mechanism/simulation_feikes.py diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index 0952735f..89e42daa 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -1,4 +1,4 @@ -from casadi import MX, dot, cos, transpose, sumsqr +from casadi import MX, dot, cos, transpose, sumsqr, vertcat import numpy as np from .natural_segment import NaturalSegment @@ -871,14 +871,16 @@ def __init__( self, name: str, child: NaturalSegment, - Q_child_ref: SegmentNaturalCoordinates, + rp_child_ref: SegmentNaturalCoordinates = None, + rd_child_ref: SegmentNaturalCoordinates = None, index: int = None, ): super(GroundJoint.Weld, self).__init__(name, None, child, index) # check size and type of parent axis - self.Q_child_ref = Q_child_ref - self.nb_constraints = 12 + self.rp_child_ref = rp_child_ref + self.rd_child_ref = rd_child_ref + self.nb_constraints = 6 def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> MX: """ @@ -891,7 +893,8 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return self.Q_child_ref - Q_child + return vertcat(self.rp_child_ref - Q_child.rp, + self.rd_child_ref - Q_child.rd) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates @@ -901,7 +904,7 @@ def parent_constraint_jacobian( def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: - K_k_child = -MX.eye(self.nb_constraints) + K_k_child = -MX.eye(12)[3:9, :] return K_k_child diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index 432761ad..7aeb102c 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -1010,14 +1010,16 @@ def __init__( self, name: str, child: NaturalSegment, - Q_child_ref: SegmentNaturalCoordinates | np.ndarray = None, + rp_child_ref: SegmentNaturalCoordinates | np.ndarray = None, + rd_child_ref: SegmentNaturalCoordinates | np.ndarray = None, index: int = None, ): super(GroundJoint.Weld, self).__init__(name, None, child, index) + self.rp_child_ref = rp_child_ref + self.rd_child_ref = rd_child_ref # check size and type of parent axis - self.Q_child_ref = SegmentNaturalCoordinates(Q_child_ref) - self.nb_constraints = 12 + self.nb_constraints = 6 def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates) -> np.ndarray: """ @@ -1030,7 +1032,8 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return self.Q_child_ref.vector - Q_child.vector + return np.concatenate((self.rp_child_ref - Q_child.rp, + self.rd_child_ref - Q_child.rd), axis=0) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates @@ -1040,7 +1043,7 @@ def parent_constraint_jacobian( def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: - K_k_child = -np.eye(self.nb_constraints) + K_k_child = -np.eye(12)[3:9,:] return K_k_child @@ -1101,5 +1104,6 @@ def to_mx(self): name=self.name, child=self.child.to_mx(), index=self.index, - Q_child_ref=self.Q_child_ref, + rp_child_ref=self.rp_child_ref, + rd_child_ref=self.rd_child_ref, ) diff --git a/examples/knee_parallel_mechanism/knee_feikes.py b/examples/knee_parallel_mechanism/knee_feikes.py new file mode 100644 index 00000000..48ac2efe --- /dev/null +++ b/examples/knee_parallel_mechanism/knee_feikes.py @@ -0,0 +1,252 @@ +import numpy as np + +from bionc import JointType + +from bionc.bionc_numpy import ( + BiomechanicalModel, + NaturalSegment, + NaturalMarker, +) + + +def create_knee_model() -> BiomechanicalModel: + """ + This function creates a biomechanical model of a knee with a parallel mechanism. + + Data from + ---------- + Generic model of the right knee + J.D. Feikes et al. J Biomech 36 (2003) 125-129 + In tibia SCS = femur SCS at full extension + Origin at midpoint between medial and lateral centers + X = -X ISB, Y = Z ISB, Z = Y ISB + + """ + model = BiomechanicalModel() + # fill the biomechanical model with the segment + model["THIGH"] = NaturalSegment( + name="THIGH", + length=0.4, + mass=12, + center_of_mass=np.array([0, -0.1, 0]), # in segment coordinates system + inertia=np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]]), # in segment coordinates system + ) + + # added markers + model["THIGH"].add_natural_marker( + NaturalMarker( + name="hip_center", + parent_name="THIGH", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["THIGH"].add_natural_marker( + NaturalMarker( + name="condyle_center", + parent_name="THIGH", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="medial_centre_femur", + location=np.array([-5.0, 0.6, -22.3])/1000, + is_distal_location=True, + is_technical=True, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="lateral_centre_femur", + location=np.array([5.0, -0.6, 22.3])/1000, + is_distal_location=True, + is_technical=True, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="ACL_femur", + location=np.array([-27.8, 12.7, 5.0])/1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="PCL_femur", + location=np.array([-20.6, -4.3, -15.7])/1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + model["THIGH"].add_natural_marker_from_segment_coordinates( + name="MCL_femur", + location=np.array([-9.7, 10.2, -42.3])/1000, + is_distal_location=True, + is_technical=False, + is_anatomical=True, + ) + + model["SHANK"] = NaturalSegment( + name="SHANK", + length=0.4, + mass=8, + center_of_mass=np.array([0, -0.1, 0]), # in segment coordinates system + inertia=np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]]), # in segment coordinates system + ) + + model["SHANK"].add_natural_marker( + NaturalMarker( + name="condyle_center", + parent_name="SHANK", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["SHANK"].add_natural_marker( + NaturalMarker( + name="ankle_center", + parent_name="SHANK", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["SHANK"].add_natural_marker( + NaturalMarker( + name="ankle_center", + parent_name="SHANK", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="medial_contact_shank", + location=np.array([-0.005, -0.0202, -0.0223]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="lateral_contact_shank", + location=np.array([0.005, -0.0202, 0.0223]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="ACL_shank", + location=np.array([-0.0, -0.0152, 0.0]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="PCL_shank", + location=np.array([-0.0303, -0.0237, -0.0024]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="MCL_shank", + location=np.array([0.0012, -0.0672, -0.0036]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_marker_from_segment_coordinates( + name="LCL_shank", + location=np.array([0.0242639, -0.0479992, 0.0371213]), + is_technical=False, + is_anatomical=True, + ) + model["SHANK"].add_natural_vector_from_segment_coordinates( + name="medial_normal_shank", + direction=np.array([0, 1, 0]), + normalize=True, + ) + model["SHANK"].add_natural_vector_from_segment_coordinates( + name="lateral_normal_shank", + direction=np.array([0, 1, 0]), + normalize=True, + ) + + model._add_joint( + dict( + name="HIP", + joint_type=JointType.GROUND_SPHERICAL, + parent="GROUND", + child="THIGH", + ground_application_point=np.array([0, 0, 0]), + ) + ) + + # model._add_joint( + # dict( + # name="SPHERICAL_KNEE", + # joint_type=JointType.SPHERICAL, + # parent="THIGH", + # child="SHANK", + # ) + # ) + + # Parallel Mechanism joint definition + model._add_joint( + dict( + name="medial_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=np.linalg.norm(np.array([-5.0, -20.2, -22.3])/1000 - np.array([-5.0, 0.6, -22.3])/1000), + sphere_center="medial_centre_femur", + plane_normal="medial_normal_shank", + plane_point="medial_contact_shank", + ) + ) + model._add_joint( + dict( + name="lateral_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=np.linalg.norm(np.array([5.0, -20.2, 22.3])/1000 - np.array([5.0, -0.6, 22.3])/1000), + sphere_center="lateral_centre_femur", + plane_normal="lateral_normal_shank", + plane_point="lateral_contact_shank", + ) + ) + model._add_joint( + dict( + name="ACL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-0.0, -15.2, 0.0])/1000 - np.array([-27.8, 12.7, 5.0])/1000), + parent_point="ACL_femur", + child_point="ACL_shank", + ) + ) + model._add_joint( + dict( + name="PCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-20.6, -4.3, -15.7])/1000 - np.array([-30.3, -23.7, -2.4])/1000), + parent_point="PCL_femur", + child_point="PCL_shank", + ) + ) + model._add_joint( + dict( + name="MCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + length=np.linalg.norm(np.array([-9.7, 10.2, -42.3])/1000 - np.array([1.2, -67.2, -3.6])/1000), + parent_point="MCL_femur", + child_point="MCL_shank", + ) + ) + + return model diff --git a/examples/knee_parallel_mechanism/knee.py b/examples/knee_parallel_mechanism/knee_sancisi.py similarity index 77% rename from examples/knee_parallel_mechanism/knee.py rename to examples/knee_parallel_mechanism/knee_sancisi.py index f796c32f..fd5550ae 100644 --- a/examples/knee_parallel_mechanism/knee.py +++ b/examples/knee_parallel_mechanism/knee_sancisi.py @@ -2,10 +2,6 @@ from bionc import JointType -# from bionc.bionc_casadi import ( -# BiomechanicalModel, -# NaturalSegment, -# ) from bionc.bionc_numpy import ( BiomechanicalModel, NaturalSegment, @@ -197,75 +193,75 @@ def create_knee_model() -> BiomechanicalModel: ) ) - model._add_joint( - dict( - name="SPHERICAL_KNEE", - joint_type=JointType.SPHERICAL, - parent="THIGH", - child="SHANK", - ) - ) - - # Parallel Mechanism joint definition - # model._add_joint( - # dict( - # name="medial_knee", - # joint_type=JointType.SPHERE_ON_PLANE, - # parent="THIGH", - # child="SHANK", - # sphere_radius=0.03232, - # sphere_center="medial_centre_femur", - # plane_normal="medial_normal_shank", - # plane_point="medial_contact_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="lateral_knee", - # joint_type=JointType.SPHERE_ON_PLANE, - # parent="THIGH", - # child="SHANK", - # sphere_radius=0.02834, - # sphere_center="lateral_centre_femur", - # plane_normal="lateral_normal_shank", - # plane_point="lateral_contact_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="ACL", - # joint_type=JointType.CONSTANT_LENGTH, - # parent="THIGH", - # child="SHANK", - # # length=0.04053, # from article - # length=0.04356820117764876, # from data - # parent_point="ACL_femur", - # child_point="ACL_shank", - # ) - # ) - # model._add_joint( - # dict( - # name="PCL", - # joint_type=JointType.CONSTANT_LENGTH, - # parent="THIGH", - # child="SHANK", - # # length=0.04326, # from article - # length=0.042833461279363376, # from data - # parent_point="PCL_femur", - # child_point="PCL_shank", - # ) - # ) # model._add_joint( # dict( - # name="MCL", - # joint_type=JointType.CONSTANT_LENGTH, + # name="SPHERICAL_KNEE", + # joint_type=JointType.SPHERICAL, # parent="THIGH", # child="SHANK", - # # length=0.12970, # from article - # length=0.10828123262317323, - # parent_point="MCL_femur", - # child_point="MCL_shank", # ) # ) + # Parallel Mechanism joint definition + model._add_joint( + dict( + name="medial_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=0.03232, + sphere_center="medial_centre_femur", + plane_normal="medial_normal_shank", + plane_point="medial_contact_shank", + ) + ) + model._add_joint( + dict( + name="lateral_knee", + joint_type=JointType.SPHERE_ON_PLANE, + parent="THIGH", + child="SHANK", + sphere_radius=0.02834, + sphere_center="lateral_centre_femur", + plane_normal="lateral_normal_shank", + plane_point="lateral_contact_shank", + ) + ) + model._add_joint( + dict( + name="ACL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.04053, # from article + length=0.04356820117764876, # from data + parent_point="ACL_femur", + child_point="ACL_shank", + ) + ) + model._add_joint( + dict( + name="PCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.04326, # from article + length=0.042833461279363376, # from data + parent_point="PCL_femur", + child_point="PCL_shank", + ) + ) + model._add_joint( + dict( + name="MCL", + joint_type=JointType.CONSTANT_LENGTH, + parent="THIGH", + child="SHANK", + # length=0.12970, # from article + length=0.10828123262317323, + parent_point="MCL_femur", + child_point="MCL_shank", + ) + ) + return model diff --git a/examples/knee_parallel_mechanism/simulation.py b/examples/knee_parallel_mechanism/simulation.py index 8af72fe4..29837745 100644 --- a/examples/knee_parallel_mechanism/simulation.py +++ b/examples/knee_parallel_mechanism/simulation.py @@ -2,7 +2,7 @@ from bionc import NaturalCoordinates, SegmentNaturalCoordinates, Viz, SegmentNaturalVelocities, NaturalVelocities from bionc.bionc_casadi.utils import to_numeric_MX -from knee import create_knee_model +from knee_sancisi import create_knee_model from utils import forward_integration, post_computations model = create_knee_model() @@ -18,8 +18,8 @@ print(model.rigid_body_constraints(NaturalCoordinates(Q))) print(model.joint_constraints(NaturalCoordinates(Q))) -# viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=False, size_xp_marker=0.005) -# viz.animate(Q) +viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=False, size_xp_marker=0.005) +viz.animate(Q) marker_names = model.marker_names @@ -64,7 +64,7 @@ Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) # actual simulation -t_final = 1 # seconds +t_final = 2 # seconds time_steps, all_states, dynamics = forward_integration( model=model, Q_init=NaturalCoordinates(Q_opt), diff --git a/examples/knee_parallel_mechanism/simulation_feikes.py b/examples/knee_parallel_mechanism/simulation_feikes.py new file mode 100644 index 00000000..cc5b3cef --- /dev/null +++ b/examples/knee_parallel_mechanism/simulation_feikes.py @@ -0,0 +1,66 @@ +import numpy as np +from bionc import NaturalCoordinates, SegmentNaturalCoordinates, Viz, SegmentNaturalVelocities, NaturalVelocities + +from knee_feikes import create_knee_model +from utils import forward_integration, post_computations + +model = create_knee_model() + +Q0 = SegmentNaturalCoordinates( + np.array([1, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 1]) +) +Q1 = SegmentNaturalCoordinates( + np.array([1, 0, 0, 0, -0.4, 0, 0, -0.8, 0, 0, 0, 1]) +) + +Q = NaturalCoordinates.from_qi((Q0, Q1)) +print(model.rigid_body_constraints(NaturalCoordinates(Q))) +print(model.joint_constraints(NaturalCoordinates(Q))) + +viz = Viz(model, size_model_marker=0.004, show_frames=True, show_ground_frame=True, size_xp_marker=0.005) +viz.animate(Q) + +# simulation in forward dynamics +tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) +] +Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + +# actual simulation +t_final = 5 # seconds +time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=100, +) + +defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, +) + +# plot results +import matplotlib.pyplot as plt + +plt.figure() +for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") +plt.title("Rigid body constraints") +plt.legend() + +plt.figure() +for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") +plt.title("Joint constraints") +plt.legend() +plt.show() + +# animation +viz = Viz(model) +viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) + diff --git a/examples/models/lower_limb.nc b/examples/models/lower_limb.nc index b1612220fe6676027b5cf18fda21cf6ad6140275..8bb6546c91b333f95b057736a218496123190279 100644 GIT binary patch delta 4537 zcmcInZERCj7~bu=b>9rSjj^$@0UKQh>oz{-V1v!s(XHbmWda(pwp-!!*tMm-!{LH} zWQ^&J63;az=pPIrBqRbZiZL;u1dWlP2z~)^7^hLgh*6@%kHq)fdsnvJ(~W8Pb9;O5 zInVRH=Y5~&oO}Km?)Y9#zgz#(;gkAat|Z4Pgk1jcmO!Y>C3pjVC;WuGJ(8Hx>Gk<4 zob)P@($vx%WE2hlPFE=8+946i9@i%|ac+sMDKrgUN}Iz~ih94R%Oi<;NAt#ZNlXp8 zLLR^1lpi94m=p|zy_7*hi%iHO77;!*kCbKdqvuyboD(CzP(2FTM~z7Ca>BU~?cp3K#~r9(C{r0Y#WJCN8fp{v9f|1LL74kQ$C zBgCe+q~Gk`C%I88kLV(hWBTH(YokTx(GdF9guXMOVRBvHFax!qEr9VO`U(BKPeM1) z2zhTo4GAaARuCN{JTFKN=n&c?zweaLZLH(s;>I!zpg+-H-R^F;m=O#Gdc0jOpEDTn z`UMHa;9Zr0e$a64PApJ3c?RcnHm8`@=?x2$@Ff~5lgFpG-H;1ZFBmFe!Oj>;Qk1-k zfrH#I)KueYxDyRwkKhu75Gb`DRsfsn!}Jc1&*$t2fWrJjSc>L)S(>c zkA!v`Xi!2aYUt@WNV;+#OOuVON;Q@>g|6o1JX*n{Iv&;YXeEzU@u-1Ejhmqb@7Pre zRdSP+Z?`(Yv~Z9H66IU(tYk?qYjD@XGOj|aX1&ACTVr7zDq>+72X7ad0gK^*cD^Mx zR>`c>k^q{b+G02>xm~oD+?soJi555oVzfpaEJoV1$(;E{hRbG=T?-4yFWI@|!2Daq zqvnIoZkEuw0rUa-7+pZ0s>Qj4E|V*{7Q?7rWX{@DieH6Q#>e48jYmxO(>>4U6ao^M z$M^>SXqF{$7|(;D=)=jR%2jed;rrm3 zHt_@)KoiGxFb1c>KU`1ml-1P;KT3FzG)~r$Omp?tDKgEBFwtT%ET-Fy-S9@z${}yngB48HQAPZv>Zq34cT1j@RjDrB$#I=@(ShgG482R|ydAwCgJrl(uVx3r-Wca_`C>p*d5m zS9UuWW2ZTyBGXo6y1+3Q5UtLMUEoDJBgzGmPUGKb!176{jSGc|{!FBcZ+(J!IxR>U|Mw#dU zQa6dHWA8ox(fIlJ(U$v%-3Zz(Ci0<>i!^nKhOa<6zJ)ne3cB&8~wpVD3QEnL`~{E*{=?^E6p`--o2Bp*7xykCFoo zGr4W#XoLBwV?d)GF>Uw_%K19IYy)q`$LU=;bSFLmcXke+q(AZW)4}C`!sn6jxY=*g Y;qq*Gjy(JO$ZTC?b~k)Jbh|751$D@qEdT%j delta 4332 zcmcInZERCj7~WmGb#&{-zTEft+*Y@_b!;#vbWoxD=5CZtNQf)#R(X5YwR7ttIEM-p zhq+PAxr9Fuqfz6g;Ufzq28}T!nh>Kg7(=2mLWsW*zd}qTMBj67Th`lqM-%l=PuJb^ zJn#FS_j%rP&mQK!JjCf=(x0%M*H3ZuimBKn7>B(+U!9YEiu5Pb6xTrAnHLR$-tQXq zh_pCsDo4;ep4`*%E+}Wi_PnW+(P6zk!!*FeV$ukkP~{1-py^U zg*OGmgeT|<2FFE;MRAC5QZNm9d_Lz;z#qf{Pl(Yn4(0{TBjAG`*xsEK6opK1jgNc% z`+@=gI*X-Q42g=0Hsr69Gu)|?;W?HeGFlW>6;hvby7qgKcVqZ0Th*(I;S@x|;jr{) z6zr!S3A3x6=Vg`p!oFCd3yR3_yty4B)X;ah%4DH}-bpT|dkgCHTRFE#j~6V^gIrM& zd}D^IkSiGc!&n{H0W52G5?1Eg{0*+1D}-$@0&Xw}X=8y2Z_pd?i_#8yJGqoTt1IWO z(c`-IR0-rB{ZUuOU7NqB+r(ALjW_9rKO-HECxJllm&|xyW}ydD>hnn|my{&^Ow#aM znE&GxD_1GCZwyM08p>LO7K+=Tin3h|3jw;?VTA*_8 zq8jRyQ494Wqm^4fbPR@P7H24A_SoAV23nJ$1?tVPa%o{u4-FLNbE7m=IGAODMpz#{ zi^rjbHm`2*w3D}n(LeJ56><*Vvby5M+7q;F)G z?i%3XWJpw)5P203llAR{8xAbpsxTA#|43lygxTaM>kg&+?OCnuM&wkOSRn|w^e~Oy zT5Z;0o-}LCNnID%>Fk>3!7~WP!wEiwZWEv5*$CAvxxnI_BOzQ=d_t+#b~BmBC+WF0 zmfZgGbHOWbs(?E9uTNWq9Iut(*rk#Z@WFVEG0F0N{zed&cXP?ke2 zD{huYw1{?`va`d+cZ*;Q>(E3h>D7wCwK3WxFJghD2go5-s)!a)q$^OJHd&-LNu*?k zJrS$SmbwO;u+ZknMqK_crBBHe zPpDJGE4^l&y3#$BEv*SCo#ZnqUHzg0L3S_3O&)>8Oov#P>bjWw|9s40Ic@yory$PHDT~>-Y zw15t~o!xzvH_`YB6Mda1=YzMcm>R9&$!$~N$y4W9(kS$}M`;vvdFvAQgVh==|E)_ju(;+(GA;?7R z5AhH3KKt Date: Wed, 22 Mar 2023 14:50:27 -0400 Subject: [PATCH 03/40] black --- bionc/bionc_casadi/joint.py | 3 +-- bionc/bionc_numpy/joint.py | 5 ++--- .../knee_parallel_mechanism/knee_feikes.py | 20 +++++++++---------- .../simulation_feikes.py | 9 ++------- 4 files changed, 15 insertions(+), 22 deletions(-) diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index 89e42daa..7eb2463f 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -893,8 +893,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return vertcat(self.rp_child_ref - Q_child.rp, - self.rd_child_ref - Q_child.rd) + return vertcat(self.rp_child_ref - Q_child.rp, self.rd_child_ref - Q_child.rd) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index 7aeb102c..fd9e4c8e 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -1032,8 +1032,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura Kinematic constraints of the joint [12, 1] """ - return np.concatenate((self.rp_child_ref - Q_child.rp, - self.rd_child_ref - Q_child.rd), axis=0) + return np.concatenate((self.rp_child_ref - Q_child.rp, self.rd_child_ref - Q_child.rd), axis=0) def parent_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates @@ -1043,7 +1042,7 @@ def parent_constraint_jacobian( def child_constraint_jacobian( self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: - K_k_child = -np.eye(12)[3:9,:] + K_k_child = -np.eye(12)[3:9, :] return K_k_child diff --git a/examples/knee_parallel_mechanism/knee_feikes.py b/examples/knee_parallel_mechanism/knee_feikes.py index 48ac2efe..37ab7151 100644 --- a/examples/knee_parallel_mechanism/knee_feikes.py +++ b/examples/knee_parallel_mechanism/knee_feikes.py @@ -54,35 +54,35 @@ def create_knee_model() -> BiomechanicalModel: model["THIGH"].add_natural_marker_from_segment_coordinates( name="medial_centre_femur", - location=np.array([-5.0, 0.6, -22.3])/1000, + location=np.array([-5.0, 0.6, -22.3]) / 1000, is_distal_location=True, is_technical=True, is_anatomical=True, ) model["THIGH"].add_natural_marker_from_segment_coordinates( name="lateral_centre_femur", - location=np.array([5.0, -0.6, 22.3])/1000, + location=np.array([5.0, -0.6, 22.3]) / 1000, is_distal_location=True, is_technical=True, is_anatomical=True, ) model["THIGH"].add_natural_marker_from_segment_coordinates( name="ACL_femur", - location=np.array([-27.8, 12.7, 5.0])/1000, + location=np.array([-27.8, 12.7, 5.0]) / 1000, is_distal_location=True, is_technical=False, is_anatomical=True, ) model["THIGH"].add_natural_marker_from_segment_coordinates( name="PCL_femur", - location=np.array([-20.6, -4.3, -15.7])/1000, + location=np.array([-20.6, -4.3, -15.7]) / 1000, is_distal_location=True, is_technical=False, is_anatomical=True, ) model["THIGH"].add_natural_marker_from_segment_coordinates( name="MCL_femur", - location=np.array([-9.7, 10.2, -42.3])/1000, + location=np.array([-9.7, 10.2, -42.3]) / 1000, is_distal_location=True, is_technical=False, is_anatomical=True, @@ -197,7 +197,7 @@ def create_knee_model() -> BiomechanicalModel: joint_type=JointType.SPHERE_ON_PLANE, parent="THIGH", child="SHANK", - sphere_radius=np.linalg.norm(np.array([-5.0, -20.2, -22.3])/1000 - np.array([-5.0, 0.6, -22.3])/1000), + sphere_radius=np.linalg.norm(np.array([-5.0, -20.2, -22.3]) / 1000 - np.array([-5.0, 0.6, -22.3]) / 1000), sphere_center="medial_centre_femur", plane_normal="medial_normal_shank", plane_point="medial_contact_shank", @@ -209,7 +209,7 @@ def create_knee_model() -> BiomechanicalModel: joint_type=JointType.SPHERE_ON_PLANE, parent="THIGH", child="SHANK", - sphere_radius=np.linalg.norm(np.array([5.0, -20.2, 22.3])/1000 - np.array([5.0, -0.6, 22.3])/1000), + sphere_radius=np.linalg.norm(np.array([5.0, -20.2, 22.3]) / 1000 - np.array([5.0, -0.6, 22.3]) / 1000), sphere_center="lateral_centre_femur", plane_normal="lateral_normal_shank", plane_point="lateral_contact_shank", @@ -221,7 +221,7 @@ def create_knee_model() -> BiomechanicalModel: joint_type=JointType.CONSTANT_LENGTH, parent="THIGH", child="SHANK", - length=np.linalg.norm(np.array([-0.0, -15.2, 0.0])/1000 - np.array([-27.8, 12.7, 5.0])/1000), + length=np.linalg.norm(np.array([-0.0, -15.2, 0.0]) / 1000 - np.array([-27.8, 12.7, 5.0]) / 1000), parent_point="ACL_femur", child_point="ACL_shank", ) @@ -232,7 +232,7 @@ def create_knee_model() -> BiomechanicalModel: joint_type=JointType.CONSTANT_LENGTH, parent="THIGH", child="SHANK", - length=np.linalg.norm(np.array([-20.6, -4.3, -15.7])/1000 - np.array([-30.3, -23.7, -2.4])/1000), + length=np.linalg.norm(np.array([-20.6, -4.3, -15.7]) / 1000 - np.array([-30.3, -23.7, -2.4]) / 1000), parent_point="PCL_femur", child_point="PCL_shank", ) @@ -243,7 +243,7 @@ def create_knee_model() -> BiomechanicalModel: joint_type=JointType.CONSTANT_LENGTH, parent="THIGH", child="SHANK", - length=np.linalg.norm(np.array([-9.7, 10.2, -42.3])/1000 - np.array([1.2, -67.2, -3.6])/1000), + length=np.linalg.norm(np.array([-9.7, 10.2, -42.3]) / 1000 - np.array([1.2, -67.2, -3.6]) / 1000), parent_point="MCL_femur", child_point="MCL_shank", ) diff --git a/examples/knee_parallel_mechanism/simulation_feikes.py b/examples/knee_parallel_mechanism/simulation_feikes.py index cc5b3cef..bb9fe650 100644 --- a/examples/knee_parallel_mechanism/simulation_feikes.py +++ b/examples/knee_parallel_mechanism/simulation_feikes.py @@ -6,12 +6,8 @@ model = create_knee_model() -Q0 = SegmentNaturalCoordinates( - np.array([1, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 1]) -) -Q1 = SegmentNaturalCoordinates( - np.array([1, 0, 0, 0, -0.4, 0, 0, -0.8, 0, 0, 0, 1]) -) +Q0 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 1])) +Q1 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, -0.4, 0, 0, -0.8, 0, 0, 0, 1])) Q = NaturalCoordinates.from_qi((Q0, Q1)) print(model.rigid_body_constraints(NaturalCoordinates(Q))) @@ -63,4 +59,3 @@ # animation viz = Viz(model) viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) - From e73ef754c9c43278ed4e55fd0f5d9d6c4bcb5d23 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 23 Mar 2023 15:29:11 -0400 Subject: [PATCH 04/40] fixe test --- .../{simulation.py => simulation_sancisi.py} | 0 tests/test_joints.py | 14 +++++--------- 2 files changed, 5 insertions(+), 9 deletions(-) rename examples/knee_parallel_mechanism/{simulation.py => simulation_sancisi.py} (100%) diff --git a/examples/knee_parallel_mechanism/simulation.py b/examples/knee_parallel_mechanism/simulation_sancisi.py similarity index 100% rename from examples/knee_parallel_mechanism/simulation.py rename to examples/knee_parallel_mechanism/simulation_sancisi.py diff --git a/tests/test_joints.py b/tests/test_joints.py index b53f44d3..6336c088 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -147,12 +147,8 @@ def test_joints(bionc_type, joint_type: JointType): joint = GroundJoint.Weld( name="Weld", child=bbox, - Q_child_ref=SegmentNaturalCoordinates.from_components( - u=[0.1, 0.2, 0.3], - rp=[0.21, 0.04, 0.053], - rd=[0.22, 0.041, 0.052], - w=[0.23, 0.042, 0.051], - ), + rp_child_ref=np.array([0.1, 0.2, 0.3]), + rd_child_ref=np.array([0.2, 0.04, 0.05]), index=0, ) else: @@ -545,20 +541,20 @@ def test_joints(bionc_type, joint_type: JointType): elif joint_type == JointType.GROUND_WELD: TestUtils.assert_equal( joint.constraint(Q1, Q2), - np.array([-1.3, -1.9, -2.9, -1.29, -1.06, -3.147, -1.38, -2.159, -4.148, -1.47, -1.958, -5.249]), + np.array([-1.4 , -0.9 , -2.9 , -1.4 , -2.16, -4.15]), decimal=6, ) child_jacobian = joint.constraint_jacobian(Q1, Q2) TestUtils.assert_equal( child_jacobian, - -np.eye(12), + -np.eye(12)[3:9, :], decimal=6, ) child_jacobian_dot = joint.constraint_jacobian_derivative(Q1, Q2) TestUtils.assert_equal( child_jacobian_dot, - np.zeros((12, 12)), + np.zeros((6, 12)), decimal=6, ) From 88d89c3c5a87bf28dd0b8c3342eb03b85e84f0ec Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 23 Mar 2023 15:29:20 -0400 Subject: [PATCH 05/40] better initial q --- examples/knee_parallel_mechanism/simulation_feikes.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/knee_parallel_mechanism/simulation_feikes.py b/examples/knee_parallel_mechanism/simulation_feikes.py index bb9fe650..32e6d5ef 100644 --- a/examples/knee_parallel_mechanism/simulation_feikes.py +++ b/examples/knee_parallel_mechanism/simulation_feikes.py @@ -6,8 +6,8 @@ model = create_knee_model() -Q0 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0, 0, 0, -0.4, 0, 0, 0, 1])) -Q1 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, -0.4, 0, 0, -0.8, 0, 0, 0, 1])) +Q0 = SegmentNaturalCoordinates(np.array([0, 0, 1, 0, 0, 0, 0, -0.4, 0, -1, 0, 0])) +Q1 = SegmentNaturalCoordinates(np.array([0, 0, 1, 0, -0.4, 0, 0, -0.8, 0, -1, 0, 0])) Q = NaturalCoordinates.from_qi((Q0, Q1)) print(model.rigid_body_constraints(NaturalCoordinates(Q))) From 97f4675656a2f09a12bc6ae95d0aa799b39bca6d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Thu, 23 Mar 2023 15:29:34 -0400 Subject: [PATCH 06/40] black --- tests/test_joints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_joints.py b/tests/test_joints.py index 6336c088..284bb980 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -541,7 +541,7 @@ def test_joints(bionc_type, joint_type: JointType): elif joint_type == JointType.GROUND_WELD: TestUtils.assert_equal( joint.constraint(Q1, Q2), - np.array([-1.4 , -0.9 , -2.9 , -1.4 , -2.16, -4.15]), + np.array([-1.4, -0.9, -2.9, -1.4, -2.16, -4.15]), decimal=6, ) child_jacobian = joint.constraint_jacobian(Q1, Q2) From 42f1f97c545e1681d66f34be73f8721ab53f4b03 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 26 Mar 2023 22:11:04 -0400 Subject: [PATCH 07/40] bug catched on length ! --- bionc/bionc_casadi/joint.py | 4 +- bionc/bionc_numpy/joint.py | 4 +- bionc/protocols/biomechanical_model.py | 11 ++ examples/play_with_joints/constant_length.py | 178 +++++++++++++++++++ examples/play_with_joints/utils.py | 175 ++++++++++++++++++ 5 files changed, 368 insertions(+), 4 deletions(-) create mode 100644 examples/play_with_joints/constant_length.py create mode 100644 examples/play_with_joints/utils.py diff --git a/bionc/bionc_casadi/joint.py b/bionc/bionc_casadi/joint.py index 7eb2463f..243835e5 100644 --- a/bionc/bionc_casadi/joint.py +++ b/bionc/bionc_casadi/joint.py @@ -568,7 +568,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura return constraint def parent_constraint_jacobian( - self, Q_child: SegmentNaturalCoordinates, Q_parent: SegmentNaturalCoordinates + self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> MX: parent_point_location = self.parent_point.position_in_global(Q_parent) child_point_location = self.child_point.position_in_global(Q_child) @@ -588,7 +588,7 @@ def child_constraint_jacobian( return K_k_child def parent_constraint_jacobian_derivative( - self, Qdot_child: SegmentNaturalVelocities, Qdot_parent: SegmentNaturalVelocities + self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities ) -> MX: parent_point_location = self.parent_point.position_in_global(Qdot_parent) child_point_location = self.child_point.position_in_global(Qdot_child) diff --git a/bionc/bionc_numpy/joint.py b/bionc/bionc_numpy/joint.py index fd9e4c8e..33fc3fb2 100644 --- a/bionc/bionc_numpy/joint.py +++ b/bionc/bionc_numpy/joint.py @@ -643,7 +643,7 @@ def constraint(self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNatura return constraint def parent_constraint_jacobian( - self, Q_child: SegmentNaturalCoordinates, Q_parent: SegmentNaturalCoordinates + self, Q_parent: SegmentNaturalCoordinates, Q_child: SegmentNaturalCoordinates ) -> np.ndarray: parent_point_location = self.parent_point.position_in_global(Q_parent) child_point_location = self.child_point.position_in_global(Q_child) @@ -663,7 +663,7 @@ def child_constraint_jacobian( return np.array(K_k_child) def parent_constraint_jacobian_derivative( - self, Qdot_child: SegmentNaturalVelocities, Qdot_parent: SegmentNaturalVelocities + self, Qdot_parent: SegmentNaturalVelocities, Qdot_child: SegmentNaturalVelocities ) -> np.ndarray: parent_point_location = self.parent_point.position_in_global(Qdot_parent) child_point_location = self.child_point.position_in_global(Qdot_child) diff --git a/bionc/protocols/biomechanical_model.py b/bionc/protocols/biomechanical_model.py index 5fbdaa4f..70be2be4 100644 --- a/bionc/protocols/biomechanical_model.py +++ b/bionc/protocols/biomechanical_model.py @@ -468,6 +468,17 @@ def segment_from_index(self, index: int): return segment raise ValueError(f"The segment index does not exist, the model has only {self.nb_segments} segments") + @property + def normalized_coordinates(self) -> tuple[tuple[int, ...]]: + idx = [] + for i in range(self.nb_segments): + # create list from i* 12 to (i+1) * 12 + segment_idx = [i for i in range(i * 12, (i + 1) * 12)] + idx.append(segment_idx[0:3]) + idx.append(segment_idx[9:12]) + + return idx + @property def mass_matrix(self): """ diff --git a/examples/play_with_joints/constant_length.py b/examples/play_with_joints/constant_length.py new file mode 100644 index 00000000..186c941a --- /dev/null +++ b/examples/play_with_joints/constant_length.py @@ -0,0 +1,178 @@ +from bionc import ( + BiomechanicalModel, + SegmentNaturalCoordinates, + SegmentNaturalVelocities, + NaturalCoordinates, + NaturalVelocities, + NaturalSegment, + JointType, + CartesianAxis, + NaturalAxis, + NaturalMarker, + Viz, +) + +import numpy as np + +from utils import forward_integration, post_computations + + +def build_two_link_segment(): + # Let's create a model + model = BiomechanicalModel() + # number of segments + # fill the biomechanical model with the segment + for i in range(2): + name = f"segment_{i}" + model[name] = NaturalSegment( + name=name, + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates an orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=0.8, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_A", + parent_name="segment_0", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_B", + parent_name="segment_1", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge_0", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="segment_0", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + model._add_joint( + dict( + name=f"constant_length", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_A", + child_point="point_B", + ) + ) + return model + + +def main(initial_pose: str = "hanged"): + model = build_two_link_segment() + + if initial_pose == "hanged": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, -1, 0, 0, -1.8, 0, -1, 0]) + elif initial_pose == "ready_to_swing": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates([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, - (0.8 + 0.2 * np.sin(np.pi/4)) - 0.8, + 0, -1, 0]) + Q = NaturalCoordinates.from_qi((Q0, Q1)) + + # # numpy version + # Q_test = NaturalCoordinates(np.arange(24)) + # jacobian_numpy = model.joint_constraints_jacobian(Q_test) + # + # model_mx = model.to_mx() + # sym = NaturalCoordinatesMX.sym(2) + # j_constraints_sym = model_mx.joint_constraints(sym) + # # jacobian + # j_jacobian_sym = jacobian(j_constraints_sym, sym) + # j_jacobian_func = Function("j_jacobian_func", [sym], [j_jacobian_sym]) + # + # jacobian_mx = j_jacobian_func(np.arange(24)).toarray() + + + + print("--------------------") + print("INITIAL CONSTRAINTS") + print(model.rigid_body_constraints(Q)) + print(model.joint_constraints(Q)) + print("--------------------") + + # uncomment to see the initial position + # viz = Viz(model) + # viz.animate(Q) + + tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) + ] + Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + + # actual simulation + t_final = 5 # seconds + time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=100, + ) + + defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, + ) + + # plot results + import matplotlib.pyplot as plt + + plt.figure() + for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") + plt.title("Rigid body constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") + plt.title("Joint constraints") + plt.legend() + + + plt.figure() + for i in range(0, model.nb_holonomic_constraints): + plt.plot(time_steps, all_lambdas[i, :], marker="o", label=f"lambda {i}") + plt.title("Lagrange multipliers") + plt.legend() + plt.show() + + # animation + viz = Viz(model) + viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) + + +if __name__ == "__main__": + # main("hanged") + main("ready_to_swing") \ No newline at end of file diff --git a/examples/play_with_joints/utils.py b/examples/play_with_joints/utils.py new file mode 100644 index 00000000..85905c6b --- /dev/null +++ b/examples/play_with_joints/utils.py @@ -0,0 +1,175 @@ +from bionc import ( + BiomechanicalModel, + NaturalCoordinates, + NaturalVelocities, +) +import numpy as np + + +def forward_integration( + model: BiomechanicalModel, + Q_init: NaturalCoordinates, + Qdot_init: NaturalVelocities, + t_final: float = 2, + steps_per_second: int = 50, +): + """ + This function simulates the dynamics of a natural segment falling from 0m during 2s + + Parameters + ---------- + model : BiomechanicalModel + The model to be simulated + Q_init : SegmentNaturalCoordinates + The initial natural coordinates of the segment + Qdot_init : SegmentNaturalVelocities + The initial natural velocities of the segment + t_final : float, optional + The final time of the simulation, by default 2 + steps_per_second : int, optional + The number of steps per second, by default 50 + + Returns + ------- + tuple: + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + """ + + print("Evaluate Rigid Body Constraints:") + print(model.rigid_body_constraints(Q_init)) + print("Evaluate Rigid Body Constraints Jacobian Derivative:") + print(model.rigid_body_constraint_jacobian_derivative(Qdot_init)) + + if (model.rigid_body_constraints(Q_init) > 1e-4).any(): + print(model.rigid_body_constraints(Q_init)) + raise ValueError( + "The segment natural coordinates don't satisfy the rigid body constraint, at initial conditions." + ) + + t_final = t_final # [s] + steps_per_second = steps_per_second + time_steps = np.linspace(0, t_final, int(steps_per_second * t_final + 1)) + + # initial conditions, x0 = [Qi, Qidot] + states_0 = np.concatenate((Q_init.to_array(), Qdot_init.to_array()), axis=0) + + # Create the forward dynamics function Callable (f(t, x) -> xdot) + def dynamics(t, states): + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + qddot, lambdas = model.forward_dynamics( + NaturalCoordinates(states[idx_coordinates]), + NaturalVelocities(states[idx_velocities]), + # stabilization=dict(alpha=0.5, beta=0.5), + ) + return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas + + # Solve the Initial Value Problem (IVP) for each time step + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) + + return time_steps, all_states, dynamics + + +def RK4(t: np.ndarray, f, y0: np.ndarray, normalize_idx: tuple[tuple[int, ...]] = None, args=(), ) -> np.ndarray: + """ + Runge-Kutta 4th order method + + Parameters + ---------- + t : array_like + time steps + f : Callable + function to be integrated in the form f(t, y, *args) + y0 : np.ndarray + initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together + + Returns + ------- + y : array_like + states for each time step + + """ + n = len(t) + y = np.zeros((len(y0), n)) + y[:, 0] = y0 + for i in range(n - 1): + h = t[i + 1] - t[i] + yi = np.squeeze(y[:, i]) + k1 = f(t[i], yi, *args) + k2 = f(t[i] + h / 2.0, yi + k1 * h / 2.0, *args) + k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) + k4 = f(t[i] + h, yi + k3 * h, *args) + y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) + return y + + +def post_computations(model: BiomechanicalModel, time_steps: np.ndarray, all_states: np.ndarray, dynamics): + """ + This function computes: + - the rigid body constraint error + - the rigid body constraint jacobian derivative error + - the joint constraint error + - the lagrange multipliers of the rigid body constraint + + Parameters + ---------- + model : NaturalSegment + The segment to be simulated + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + + Returns + ------- + tuple: + rigid_body_constraint_error : np.ndarray + The rigid body constraint error at each time step + rigid_body_constraint_jacobian_derivative_error : np.ndarray + The rigid body constraint jacobian derivative error at each time step + joint_constraints: np.ndarray + The joint constraints at each time step + lambdas : np.ndarray + The lagrange multipliers of the rigid body constraint at each time step + """ + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + # compute the quantities of interest after the integration + all_lambdas = np.zeros((model.nb_holonomic_constraints, len(time_steps))) + defects = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + defects_dot = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + joint_defects = np.zeros((model.nb_joint_constraints, len(time_steps))) + joint_defects_dot = np.zeros((model.nb_joint_constraints, len(time_steps))) + + for i in range(len(time_steps)): + defects[:, i] = model.rigid_body_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + defects_dot[:, i] = model.rigid_body_constraints_derivative( + NaturalCoordinates(all_states[idx_coordinates, i]), NaturalVelocities(all_states[idx_velocities, i]) + ) + + joint_defects[:, i] = model.joint_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + # todo : to be implemented + # joint_defects_dot = model.joint_constraints_derivative( + # NaturalCoordinates(all_states[idx_coordinates, i]), + # NaturalVelocities(all_states[idx_velocities, i])) + # ) + + all_lambdas[:, i : i + 1] = dynamics(time_steps[i], all_states[:, i])[1] + + return defects, defects_dot, joint_defects, all_lambdas From 0ce9065a9a374bd2ad9770b96d14cdbe5fe61fc2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 26 Mar 2023 22:20:52 -0400 Subject: [PATCH 08/40] tests according --- tests/test_joints.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tests/test_joints.py b/tests/test_joints.py index 284bb980..fce0b056 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -346,18 +346,18 @@ def test_joints(bionc_type, joint_type: JointType): parent_jacobian_res = np.array( [ [ - 0.163589, - 0.056279, - 0.231968, - 1.963063, - 0.675351, - 2.783621, - -0.327177, - -0.112558, - -0.463937, - 0.490766, - 0.168838, - 0.695905, + -0.045226, + 0.014514, + 0.170561, + -0.542708, + 0.174171, + 2.046735, + 0.090451, + -0.029029, + -0.341123, + -0.135677, + 0.043543, + 0.511684, ] ] ) From e5b9a7bf407254c7a69bcedcbd1aeefa273f603a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 26 Mar 2023 22:21:27 -0400 Subject: [PATCH 09/40] normalized vectors within rk4 --- examples/knee_parallel_mechanism/utils.py | 18 +++++++++++++++--- examples/play_with_joints/utils.py | 8 +++++++- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/examples/knee_parallel_mechanism/utils.py b/examples/knee_parallel_mechanism/utils.py index 498e7c62..586d142c 100644 --- a/examples/knee_parallel_mechanism/utils.py +++ b/examples/knee_parallel_mechanism/utils.py @@ -71,12 +71,19 @@ def dynamics(t, states): return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas # Solve the Initial Value Problem (IVP) for each time step - all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0) + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) return time_steps, all_states, dynamics -def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: """ Runge-Kutta 4th order method @@ -88,6 +95,8 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: function to be integrated in the form f(t, y, *args) y0 : np.ndarray initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together Returns ------- @@ -99,7 +108,6 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: y = np.zeros((len(y0), n)) y[:, 0] = y0 for i in range(n - 1): - print(f"frame {i}") h = t[i + 1] - t[i] yi = np.squeeze(y[:, i]) k1 = f(t[i], yi, *args) @@ -107,6 +115,10 @@ def RK4(t: np.ndarray, f, y0: np.ndarray, args=()) -> np.ndarray: k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) k4 = f(t[i] + h, yi + k3 * h, *args) y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) return y diff --git a/examples/play_with_joints/utils.py b/examples/play_with_joints/utils.py index 85905c6b..586d142c 100644 --- a/examples/play_with_joints/utils.py +++ b/examples/play_with_joints/utils.py @@ -77,7 +77,13 @@ def dynamics(t, states): return time_steps, all_states, dynamics -def RK4(t: np.ndarray, f, y0: np.ndarray, normalize_idx: tuple[tuple[int, ...]] = None, args=(), ) -> np.ndarray: +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: """ Runge-Kutta 4th order method From 5ea2a30aa377247a8000748b213bc4a8df46698b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 26 Mar 2023 22:38:33 -0400 Subject: [PATCH 10/40] test joint todo --- tests/test_joints.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/tests/test_joints.py b/tests/test_joints.py index fce0b056..1dd235c6 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -560,3 +560,19 @@ def test_joints(bionc_type, joint_type: JointType): assert joint.parent_constraint_jacobian(Q1, Q2) is None assert joint.parent_constraint_jacobian_derivative(Q1, Q2) is None + + +# def test_numpy_jacobian_from_casadi_derivatives() +# todo: +# # numpy version +# Q_test = NaturalCoordinates(np.arange(24)) +# jacobian_numpy = model.joint_constraints_jacobian(Q_test) +# +# model_mx = model.to_mx() +# sym = NaturalCoordinatesMX.sym(2) +# j_constraints_sym = model_mx.joint_constraints(sym) +# # jacobian +# j_jacobian_sym = jacobian(j_constraints_sym, sym) +# j_jacobian_func = Function("j_jacobian_func", [sym], [j_jacobian_sym]) +# +# jacobian_mx = j_jacobian_func(np.arange(24)).toarray() \ No newline at end of file From 1f5168b065cb7e2c4937e238951084c0488ea350 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sun, 26 Mar 2023 22:39:16 -0400 Subject: [PATCH 11/40] value this content --- .../simulation_feikes.py | 8 +- examples/play_with_joints/constant_length.py | 25 ++- .../play_with_joints/two_constant_length.py | 207 ++++++++++++++++++ 3 files changed, 228 insertions(+), 12 deletions(-) create mode 100644 examples/play_with_joints/two_constant_length.py diff --git a/examples/knee_parallel_mechanism/simulation_feikes.py b/examples/knee_parallel_mechanism/simulation_feikes.py index 32e6d5ef..d3946534 100644 --- a/examples/knee_parallel_mechanism/simulation_feikes.py +++ b/examples/knee_parallel_mechanism/simulation_feikes.py @@ -6,8 +6,8 @@ model = create_knee_model() -Q0 = SegmentNaturalCoordinates(np.array([0, 0, 1, 0, 0, 0, 0, -0.4, 0, -1, 0, 0])) -Q1 = SegmentNaturalCoordinates(np.array([0, 0, 1, 0, -0.4, 0, 0, -0.8, 0, -1, 0, 0])) +Q0 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0.0, 0, 0, 0.4, 0, 0, 0, 1])) +Q1 = SegmentNaturalCoordinates(np.array([1, 0, 0, 0, 0.4, 0, 0, 0.8, 0, 0, 0, 1])) Q = NaturalCoordinates.from_qi((Q0, Q1)) print(model.rigid_body_constraints(NaturalCoordinates(Q))) @@ -24,13 +24,13 @@ Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) # actual simulation -t_final = 5 # seconds +t_final = 0.5 # seconds time_steps, all_states, dynamics = forward_integration( model=model, Q_init=Q, Qdot_init=Qdot, t_final=t_final, - steps_per_second=100, + steps_per_second=1000, ) defects, defects_dot, joint_defects, all_lambdas = post_computations( diff --git a/examples/play_with_joints/constant_length.py b/examples/play_with_joints/constant_length.py index 186c941a..cc4e27a8 100644 --- a/examples/play_with_joints/constant_length.py +++ b/examples/play_with_joints/constant_length.py @@ -91,10 +91,22 @@ def main(initial_pose: str = "hanged"): Q1 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, -1, 0, 0, -1.8, 0, -1, 0]) elif initial_pose == "ready_to_swing": Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) - Q1 = SegmentNaturalCoordinates([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, - (0.8 + 0.2 * np.sin(np.pi/4)) - 0.8, - 0, -1, 0]) + Q1 = SegmentNaturalCoordinates( + [ + 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, + -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8, + 0, + -1, + 0, + ] + ) Q = NaturalCoordinates.from_qi((Q0, Q1)) # # numpy version @@ -110,8 +122,6 @@ def main(initial_pose: str = "hanged"): # # jacobian_mx = j_jacobian_func(np.arange(24)).toarray() - - print("--------------------") print("INITIAL CONSTRAINTS") print(model.rigid_body_constraints(Q)) @@ -160,7 +170,6 @@ def main(initial_pose: str = "hanged"): plt.title("Joint constraints") plt.legend() - plt.figure() for i in range(0, model.nb_holonomic_constraints): plt.plot(time_steps, all_lambdas[i, :], marker="o", label=f"lambda {i}") @@ -175,4 +184,4 @@ def main(initial_pose: str = "hanged"): if __name__ == "__main__": # main("hanged") - main("ready_to_swing") \ No newline at end of file + main("ready_to_swing") diff --git a/examples/play_with_joints/two_constant_length.py b/examples/play_with_joints/two_constant_length.py new file mode 100644 index 00000000..05a4c023 --- /dev/null +++ b/examples/play_with_joints/two_constant_length.py @@ -0,0 +1,207 @@ +from bionc import ( + BiomechanicalModel, + SegmentNaturalCoordinates, + SegmentNaturalVelocities, + NaturalCoordinates, + NaturalVelocities, + NaturalSegment, + JointType, + CartesianAxis, + NaturalAxis, + NaturalMarker, + Viz, +) + +import numpy as np + +from utils import forward_integration, post_computations + + +def build_two_link_segment(): + # Let's create a model + model = BiomechanicalModel() + # number of segments + # fill the biomechanical model with the segment + for i in range(2): + name = f"segment_{i}" + model[name] = NaturalSegment( + name=name, + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates an orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=0.8, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_A", + parent_name="segment_0", + position=np.array([0, -1, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_0"].add_natural_marker( + NaturalMarker( + name="point_AA", + parent_name="segment_0", + position=np.array([0, -1, 0.05]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_B", + parent_name="segment_1", + position=np.array([0, 0, 0]), + is_technical=True, + is_anatomical=False, + ) + ) + + model["segment_1"].add_natural_marker( + NaturalMarker( + name="point_BB", + parent_name="segment_1", + position=np.array([0, 0, 0.05]), + is_technical=True, + is_anatomical=False, + ) + ) + + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge_0", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="segment_0", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + model._add_joint( + dict( + name=f"constant_length", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_A", + child_point="point_B", + ) + ) + + model._add_joint( + dict( + name=f"constant_length_2", + joint_type=JointType.CONSTANT_LENGTH, + parent=f"segment_0", + child=f"segment_1", + length=0.2, + parent_point="point_AA", + child_point="point_BB", + ) + ) + + return model + + +def main(initial_pose: str = "hanged"): + model = build_two_link_segment() + + if initial_pose == "hanged": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, -1, 0, 0, -1.8, 0, -1, 0]) + elif initial_pose == "ready_to_swing": + Q0 = SegmentNaturalCoordinates([1, 0, 0, 0, 0, 0, 0, 0, -0.8, 0, -1, 0]) + Q1 = SegmentNaturalCoordinates( + [ + 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, + -(0.8 + 0.2 * np.sin(np.pi / 4)) - 0.8, + 0, + -1, + 0, + ] + ) + Q = NaturalCoordinates.from_qi((Q0, Q1)) + + print("--------------------") + print("INITIAL CONSTRAINTS") + print(model.rigid_body_constraints(Q)) + print(model.joint_constraints(Q)) + print("--------------------") + + # uncomment to see the initial position + # viz = Viz(model) + # viz.animate(Q) + + tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, model.nb_segments) + ] + Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + + # actual simulation + t_final = 5 # seconds + time_steps, all_states, dynamics = forward_integration( + model=model, + Q_init=Q, + Qdot_init=Qdot, + t_final=t_final, + steps_per_second=100, + ) + + defects, defects_dot, joint_defects, all_lambdas = post_computations( + model=model, + time_steps=time_steps, + all_states=all_states, + dynamics=dynamics, + ) + + # plot results + import matplotlib.pyplot as plt + + plt.figure() + for i in range(0, model.nb_rigid_body_constraints): + plt.plot(time_steps, defects[i, :], marker="o", label=f"defects {i}") + plt.title("Rigid body constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_joint_constraints): + plt.plot(time_steps, joint_defects[i, :], marker="o", label=f"joint_defects {i}") + plt.title("Joint constraints") + plt.legend() + + plt.figure() + for i in range(0, model.nb_holonomic_constraints): + plt.plot(time_steps, all_lambdas[i, :], marker="o", label=f"lambda {i}") + plt.title("Lagrange multipliers") + plt.legend() + plt.show() + + # animation + viz = Viz(model) + viz.animate(NaturalCoordinates(all_states[: (12 * model.nb_segments), :]), None) + + +if __name__ == "__main__": + main("hanged") + # main("ready_to_swing") From 9897b3b643d55359e4bac3abb1cb065d0e158e4d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 27 Mar 2023 22:55:12 -0400 Subject: [PATCH 12/40] segment names needed --- bionc/protocols/biomechanical_model.py | 7 +++ .../inverse_kinematics_study.py | 55 +++++++++++++++++++ tests/test_biomech_model.py | 8 +++ 3 files changed, 70 insertions(+) create mode 100644 examples/inverse_kinematics/inverse_kinematics_study.py diff --git a/bionc/protocols/biomechanical_model.py b/bionc/protocols/biomechanical_model.py index 70be2be4..c0bedddf 100644 --- a/bionc/protocols/biomechanical_model.py +++ b/bionc/protocols/biomechanical_model.py @@ -314,6 +314,13 @@ def nb_markers_technical(self) -> int: nb_markers += self.segments[key].nb_markers_technical return nb_markers + @property + def segment_names(self) -> list[str]: + """ + This function returns the names of the segments in the model + """ + return list(self.segments.keys()) + @property def marker_names(self) -> list[str]: """ diff --git a/examples/inverse_kinematics/inverse_kinematics_study.py b/examples/inverse_kinematics/inverse_kinematics_study.py new file mode 100644 index 00000000..57d05e38 --- /dev/null +++ b/examples/inverse_kinematics/inverse_kinematics_study.py @@ -0,0 +1,55 @@ +""" +This example shows how to use the InverseKinematics class to solve an inverse kinematics problem. +""" +from bionc import InverseKinematics, Viz +import numpy as np +from pyomeca import Markers +from tests.utils import TestUtils +import time + + +def main(): + # build the model from the lower limb example + bionc = TestUtils.bionc_folder() + module = TestUtils.load_module(bionc + "/examples/model_creation/main.py") + + # Generate c3d file + filename = module.generate_c3d_file() + # Generate model + model = module.model_creation_from_measured_data(filename) + + # getting noisy markers for 200 frames + markers = Markers.from_c3d(filename).to_numpy()[:3, :, :] # 2 frames + markers = np.repeat(markers, 100, axis=2) # 2 x 100 frames + np.random.seed(42) + markers = markers + np.random.normal(0, 0.01, markers.shape) # add noise + + # you can import the class from bionc + ik_solver = InverseKinematics(model, markers, solve_frame_per_frame=True) + # or you can use the model method + ik_solver = model.inverse_kinematics(markers, solve_frame_per_frame=True) + + tic0 = time.time() + Qopt_sqp = ik_solver.solve(method="sqpmethod") # tend to be faster (with limited-memory hessian approximation) + toc0 = time.time() + + tic1 = time.time() + Qopt_ipopt = ik_solver.solve(method="ipopt") # tend to find lower cost functions but may flip axis. + toc1 = time.time() + + print(f"Time to solve 200 frames with sqpmethod: {toc0 - tic0}") + print(f"time to solve 200 frames with ipopt: {toc1 - tic1}") + + return ik_solver, Qopt_sqp, Qopt_ipopt, model, markers + + +if __name__ == "__main__": + ik_solver, Qopt, _, model, markers = main() + + viz = Viz( + model, + show_center_of_mass=False, # no center of mass in this example + show_xp_markers=True, + show_model_markers=True, + ) + viz.animate(Qopt, markers_xp=markers) diff --git a/tests/test_biomech_model.py b/tests/test_biomech_model.py index 16a68920..14861ea6 100644 --- a/tests/test_biomech_model.py +++ b/tests/test_biomech_model.py @@ -6960,6 +6960,14 @@ def test_biomech_model(bionc_type): "RTAR", ] assert natural_model.marker_names_technical == marker_names_technical + segment_names = [ + "PELVIS", + "THIGH", + "SHANK", + "FOOT", + ] + + assert natural_model.segment_names == segment_names filename = "natural_model.nc" if bionc_type == "numpy": From 4fc07c17b93fde11e1ef4461f6d26937ed2e853a Mon Sep 17 00:00:00 2001 From: Ipuch Date: Mon, 27 Mar 2023 22:56:04 -0400 Subject: [PATCH 13/40] sarrus --- bionc/bionc_numpy/inverse_kinematics.py | 62 +++++++++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index 1ccc9135..2553ac76 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/bionc/bionc_numpy/inverse_kinematics.py @@ -47,6 +47,29 @@ def _solve_nlp(method: str, nlp: dict, Q_init: np.ndarray, lbg: np.ndarray, ubg: return r +def sarrus(matrix: MX): + """ + Computes the determinant of a 3x3 matrix using the Sarrus rule + + Parameters + ---------- + matrix : MX + The matrix to compute the determinant of + + Returns + ------- + The determinant of the matrix + """ + return ( + matrix[0, 0] * matrix[1, 1] * matrix[2, 2] + + matrix[0, 1] * matrix[1, 2] * matrix[2, 0] + + matrix[0, 2] * matrix[1, 0] * matrix[2, 1] + - matrix[0, 0] * matrix[1, 2] * matrix[2, 1] + - matrix[0, 1] * matrix[1, 0] * matrix[2, 2] + - matrix[0, 2] * matrix[1, 1] * matrix[2, 0] + ) + + class InverseKinematics: """ Inverse kinematics solver also known as Multibody Kinematics Optimization (MKO) @@ -98,8 +121,25 @@ def __init__( experimental_markers: np.ndarray | str, Q_init: np.ndarray | NaturalCoordinates = None, solve_frame_per_frame: bool = True, + active_direct_frame_constraints: bool = False, ): + """ + Parameters + ---------- + model : BiomechanicalModel + The model considered (bionc.numpy) + experimental_markers : np.ndarray | str + The experimental markers (3xNxM numpy array), or a path to a c3d file + Q_init : np.ndarray | NaturalCoordinates + The initial guess for the inverse kinematics computed from the experimental markers + solve_frame_per_frame : bool + If True, the inverse kinematics is solved frame per frame, otherwise it is solved for the whole motion + active_direct_frame_constraints : bool + If True, the direct frame constraints are active, otherwise they are not + """ + self._frame_per_frame = solve_frame_per_frame + self._active_direct_frame_constraints = active_direct_frame_constraints if not isinstance(model, BiomechanicalModel): raise ValueError("model must be a BiomechanicalModel") @@ -192,6 +232,11 @@ def solve( lbg = np.zeros(self.model.nb_holonomic_constraints) ubg = np.zeros(self.model.nb_holonomic_constraints) constraints = self._constraints(self._Q_sym) + if self._active_direct_frame_constraints: + constraints = vertcat(constraints, self._direct_frame_constraints(self._Q_sym)) + lbg = np.concatenate((lbg, np.zeros(self.model.nb_segments))) + # upper bounds infinity + ubg = np.concatenate((ubg, np.full(self.model.nb_segments, np.inf))) nlp = dict( x=self._vert_Q_sym, g=constraints, @@ -203,6 +248,8 @@ def solve( Qopt[:, f : f + 1] = r["x"].toarray() else: constraints = self._constraints(self._Q_sym) + if self._active_direct_frame_constraints: + constraints = vertcat(constraints, self._direct_frame_constraints(self._Q_sym)) objective = self._objective(self._Q_sym, self.experimental_markers) nlp = dict( x=self._vert_Q_sym, @@ -212,6 +259,9 @@ def solve( Q_init = self.Q_init.reshape((12 * self.model.nb_segments * self.nb_frames, 1)) lbg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) ubg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) + if self._active_direct_frame_constraints: + lbg = np.concatenate((lbg, np.zeros(self.model.nb_segments * self.nb_frames))) + ubg = np.concatenate((ubg, np.full(self.model.nb_segments * self.nb_frames, np.inf))) r = _solve_nlp(method, nlp, Q_init, lbg, ubg, options) Qopt = r["x"].reshape((12 * self.model.nb_segments, self.nb_frames)).toarray() @@ -256,4 +306,16 @@ def _constraints(self, Q) -> MX: phik.append(self._model_mx.joint_constraints(Q_f)) return vertcat(*phir, *phik) + def _direct_frame_constraints(self, Q): + """Computes the direct frame constraints and handle single frame or multi frames""" + nb_frames = 1 if self._frame_per_frame else self.nb_frames + direct_frame_constraints = [] + for f in range(nb_frames): + Q_f = NaturalCoordinates(Q[:, f]) + for ii in range(self.model.nb_segments): + u, v, w = Q_f.vector(ii).to_uvw() + direct_frame_constraints.append(sarrus(horzcat(u, v, w))) + return vertcat(*direct_frame_constraints) + + # todo: def sol() -> dict that returns the details of the inverse kinematics such as all the metrics, etc... From c9cdd7b198173ce7d4b61d83e3f629fe12c70ae8 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Tue, 28 Mar 2023 00:05:47 -0400 Subject: [PATCH 14/40] mx_to_sx to get faster and check determinant sign --- bionc/bionc_casadi/natural_segment.py | 2 +- bionc/bionc_numpy/inverse_kinematics.py | 47 ++++++++++++++++++++++--- 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/bionc/bionc_casadi/natural_segment.py b/bionc/bionc_casadi/natural_segment.py index 8b00f11c..b657c678 100644 --- a/bionc/bionc_casadi/natural_segment.py +++ b/bionc/bionc_casadi/natural_segment.py @@ -2,7 +2,7 @@ import numpy as np from casadi import MX -from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, sum2 +from casadi import cos, sin, transpose, vertcat, sqrt, inv, dot, sum1, cross, norm_2 from ..bionc_casadi.natural_coordinates import SegmentNaturalCoordinates from ..bionc_casadi.natural_velocities import SegmentNaturalVelocities diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index 2553ac76..a3915cd2 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/bionc/bionc_numpy/inverse_kinematics.py @@ -1,4 +1,4 @@ -from casadi import vertcat, horzcat, MX, Function, nlpsol +from casadi import vertcat, horzcat, MX, Function, nlpsol, SX, Function import numpy as np from pyomeca import Markers @@ -8,6 +8,26 @@ ) from ..protocols.biomechanical_model import GenericBiomechanicalModel as BiomechanicalModel +from ..bionc_numpy.natural_coordinates import NaturalCoordinates as NaturalCoordinatesNumpy + + +def _mx_to_sx(mx: MX, symbolics: list[MX]) -> SX: + """ + Converts a MX to a SX + + Parameters + ---------- + mx : MX + The MX to convert + symbolics : list[MX] + The symbolics to use + + Returns + ------- + The converted SX + """ + f = Function("f", symbolics, [mx]).expand() + return f(*symbolics) def _solve_nlp(method: str, nlp: dict, Q_init: np.ndarray, lbg: np.ndarray, ubg: np.ndarray, options: dict): @@ -122,6 +142,7 @@ def __init__( Q_init: np.ndarray | NaturalCoordinates = None, solve_frame_per_frame: bool = True, active_direct_frame_constraints: bool = False, + use_sx: bool = True, ): """ Parameters @@ -140,6 +161,7 @@ def __init__( self._frame_per_frame = solve_frame_per_frame self._active_direct_frame_constraints = active_direct_frame_constraints + self.use_sx = use_sx if not isinstance(model, BiomechanicalModel): raise ValueError("model must be a BiomechanicalModel") @@ -165,6 +187,7 @@ def __init__( self.Q_init = Q_init self.Qopt = None + self.segment_determinants = None self.nb_frames = self.experimental_markers.shape[2] self.nb_markers = self.experimental_markers.shape[1] @@ -239,10 +262,11 @@ def solve( ubg = np.concatenate((ubg, np.full(self.model.nb_segments, np.inf))) nlp = dict( x=self._vert_Q_sym, - g=constraints, + g=_mx_to_sx(constraints,[self._vert_Q_sym]) if self.use_sx else constraints, ) for f in range(self.nb_frames): - nlp["f"] = self._objective_function(self._Q_sym, self.experimental_markers[:, :, f]) + objective = self._objective_function(self._Q_sym, self.experimental_markers[:, :, f]) + nlp["f"] = _mx_to_sx(objective,[self._vert_Q_sym]) if self.use_sx else objective Q_init = self.Q_init[:, f : f + 1] r = _solve_nlp(method, nlp, Q_init, lbg, ubg, options) Qopt[:, f : f + 1] = r["x"].toarray() @@ -253,8 +277,8 @@ def solve( objective = self._objective(self._Q_sym, self.experimental_markers) nlp = dict( x=self._vert_Q_sym, - f=objective, - g=constraints, + f=_mx_to_sx(objective,[self._vert_Q_sym]) if self.use_sx else objective, + g=_mx_to_sx(constraints,[self._vert_Q_sym]) if self.use_sx else constraints, ) Q_init = self.Q_init.reshape((12 * self.model.nb_segments * self.nb_frames, 1)) lbg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) @@ -267,6 +291,8 @@ def solve( self.Qopt = Qopt.reshape((12 * self.model.nb_segments, self.nb_frames)) + self.check_segment_determinants() + return Qopt def _declare_sym_Q(self) -> tuple[MX, MX]: @@ -317,5 +343,16 @@ def _direct_frame_constraints(self, Q): direct_frame_constraints.append(sarrus(horzcat(u, v, w))) return vertcat(*direct_frame_constraints) + def check_segment_determinants(self): + """Checks the determinant of each segment frame""" + self.segment_determinants = np.zeros((self.model.nb_segments, self.nb_frames)) + for i in range(0, self.Qopt.shape[1]): + Qi = NaturalCoordinatesNumpy(self.Qopt)[:, i:i + 1] + for s in range(0, self.model.nb_segments): + u, v, w = Qi.vector(s).to_uvw() + matrix = np.concatenate((u[:, np.newaxis], v[:, np.newaxis], w[:, np.newaxis]), axis=1) + self.segment_determinants[s, i] = np.linalg.det(matrix) + if self.segment_determinants[s, i] < 0: + print(f"Warning: frame {i} segment {s} has a negative determinant") # todo: def sol() -> dict that returns the details of the inverse kinematics such as all the metrics, etc... From f050630e5fcb29a0f0a268ed29b220c14fcc6587 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Tue, 28 Mar 2023 08:20:19 -0400 Subject: [PATCH 15/40] docs strings like the name of the pR --- bionc/bionc_numpy/inverse_kinematics.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index a3915cd2..b28b0e46 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/bionc/bionc_numpy/inverse_kinematics.py @@ -133,6 +133,8 @@ class InverseKinematics: builds the constraints to satisfy animate() Animates the solution of the inverse kinematics + _active_direct_frame_constraints() + builds the constraints to ensure that the determinant of the matrix [u, v, w] is positive """ def __init__( @@ -156,7 +158,10 @@ def __init__( solve_frame_per_frame : bool If True, the inverse kinematics is solved frame per frame, otherwise it is solved for the whole motion active_direct_frame_constraints : bool - If True, the direct frame constraints are active, otherwise they are not + If True, the direct frame constraints are active, otherwise they are not. + It ensures that rigid body constraints lead to positive determinants or the matrix [u, v, w]. + use_sx : bool + If True, the symbolic variables are SX, otherwise they are MX (SX are faster but take more memory) """ self._frame_per_frame = solve_frame_per_frame From 631d392b0e4952af468b9e8d361b46825debe8f1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Fri, 31 Mar 2023 18:07:14 -0400 Subject: [PATCH 16/40] No working yet... espacially for moments. --- bionc/__init__.py | 1 + bionc/bionc_numpy/__init__.py | 1 + bionc/bionc_numpy/biomechanical_model.py | 10 +- bionc/bionc_numpy/external_force.py | 72 +++ bionc/bionc_numpy/natural_coordinates.py | 143 ++++++ bionc/bionc_numpy/natural_external_forces.py | 460 ++++++++++++++++++ .../forward_dynamics/pendulum_with_force.nmod | Bin 0 -> 4122 bytes .../forward_dynamics/pendulum_with_force.py | 290 +++++++++++ .../inverse_kinematics_study.py | 37 +- tests/test_external_force.py | 70 +++ 10 files changed, 1070 insertions(+), 14 deletions(-) create mode 100644 bionc/bionc_numpy/external_force.py create mode 100644 bionc/bionc_numpy/natural_external_forces.py create mode 100644 examples/forward_dynamics/pendulum_with_force.nmod create mode 100644 examples/forward_dynamics/pendulum_with_force.py create mode 100644 tests/test_external_force.py diff --git a/bionc/__init__.py b/bionc/__init__.py index 9db019fe..3f25c921 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -18,6 +18,7 @@ InertiaParameters, BiomechanicalModel, JointType, + ExternalForceList, ExternalForce, ) from .protocols import natural_coordinates diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 77afdcc4..3184dce2 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -16,3 +16,4 @@ from .joint import Joint, GroundJoint from .natural_vector import NaturalVector from .inverse_kinematics import InverseKinematics +from .natural_coordinates import ExternalForceList, ExternalForce diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index c8f1b019..bdd7735c 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -6,6 +6,7 @@ from .natural_accelerations import NaturalAccelerations from ..protocols.biomechanical_model import GenericBiomechanicalModel from .inverse_kinematics import InverseKinematics +from .natural_coordinates import ExternalForceList class BiomechanicalModel(GenericBiomechanicalModel): @@ -632,7 +633,7 @@ def forward_dynamics( self, Q: NaturalCoordinates, Qdot: NaturalCoordinates, - # external_forces: ExternalForces + external_forces: ExternalForceList = None, stabilization: dict = None, ) -> np.ndarray: """ @@ -644,6 +645,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 stabilization: dict Dictionary containing the Baumgarte's stabilization parameters: * alpha: float @@ -660,6 +663,9 @@ 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) + # KKT system # [G, K.T] [Qddot] = [forces] # [K, 0 ] [lambda] = [biais] @@ -667,7 +673,7 @@ def forward_dynamics( lower_KKT_matrix = np.concatenate((K, np.zeros((K.shape[0], K.shape[0]))), axis=1) KKT_matrix = np.concatenate((upper_KKT_matrix, lower_KKT_matrix), axis=0) - forces = self.weight() + forces = self.weight() + fext biais = -Kdot @ Qdot if stabilization is not None: diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py new file mode 100644 index 00000000..50f3201e --- /dev/null +++ b/bionc/bionc_numpy/external_force.py @@ -0,0 +1,72 @@ +import numpy as np +# from .natural_coordinates import SegmentNaturalCoordinates +# from .natural_vector import NaturalVector + + +def compute_pseudo_interpolation_matrix(Qi): + """ + Return the pseudo interpolation matrix + """ + pseudo_interpolation_matrix = np.zeros((3,12)) + + pseudo_interpolation_matrix[0, 9:12] = Qi.u + pseudo_interpolation_matrix[1, 0:3] = Qi.v + pseudo_interpolation_matrix[2, 3:6] = -Qi.w + pseudo_interpolation_matrix[2, 6:3] = Qi.w + + return pseudo_interpolation_matrix + + +def compute_force_moment_transformation_matrix(Qi): + """ + Return the force moment transformation matrix + """ + # default we apply force at the proximal point + force_moment_transformation_matrix = np.zeros((3,12)) + + force_moment_transformation_matrix[:, 0] = np.cross(Qi.w, Qi.u) + force_moment_transformation_matrix[:, 1] = np.cross(Qi.u, Qi.v) + force_moment_transformation_matrix[:, 2] = np.cross(-Qi.v, Qi.w) + + return force_moment_transformation_matrix + + +def to_natural_force(Qi, external_forces: np.ndarray, application_point_in_global: np.ndarray): + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + external_forces: np.ndarray + External forces in cartesian coordinates + + Returns + ------- + + """ + torque = external_forces[3:6] + force = external_forces[0:3] + + pseudo_interpolation_matrix = compute_pseudo_interpolation_matrix(Qi) + force_moment_transformation_matrix = compute_force_moment_transformation_matrix(Qi) + + fext = NaturalVector.proximal().interlopate() @ force + fext += force_moment_transformation_matrix.T @ torque + fext += force_moment_transformation_matrix.T @ np.cross(application_point_in_global - Qi.rp, force) + + return fext + + +# try the functions +# Qi = SegmentNaturalCoordinates.from_components( +# u=np.array([1, 0, 0]), +# rp=np.array([0, 0, 0]), +# rd=np.array([0, 0, 0]), +# w=np.array([0, 0, 1]), +# ) +# external_force = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) +# application_point_in_local = np.array([0.1, 0.2, 0.3]) +# natural_force = to_natural_force(Qi, external_force, application_point_in_local) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 2674aabe..8f1fed6a 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -248,3 +248,146 @@ def v(self, segment_idx: int): def vector(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) return SegmentNaturalCoordinates(self[array_idx, :].to_array()) + + +class ExternalForce: + def __init__(self, + application_point_in_local: np.ndarray, + external_forces: np.ndarray): + self.application_point_in_local = application_point_in_local + self.external_forces = external_forces + + @classmethod + def from_components(cls, application_point_in_global: np.ndarray, force: np.ndarray, torque: np.ndarray): + return cls(application_point_in_global, np.concatenate((torque, force))) + + @property + def force(self): + return self.external_forces[3:6] + + @property + def torque(self): + return self.external_forces[0:3] + + @staticmethod + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): + """ + Return the force moment transformation matrix + """ + # default we apply force at the proximal point + + left_interpolation_matrix = np.zeros((12, 3)) + + left_interpolation_matrix[9:12, 0] = Qi.u + left_interpolation_matrix[0:3, 1] = Qi.v + left_interpolation_matrix[3:6, 2] = -Qi.w + left_interpolation_matrix[6:9, 2] = Qi.w + + # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar + lever_arm_force_matrix = np.zeros((3, 3)) + + lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) + lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) + lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) + + return (left_interpolation_matrix @ lever_arm_force_matrix).T + + def to_natural_force(self, Qi: SegmentNaturalCoordinates): + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + + """ + + pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) + point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + application_point_in_global = np.array(point_interpolation_matrix @ Qi).squeeze() + + fext = point_interpolation_matrix.T @ self.force + fext += pseudo_interpolation_matrix.T @ self.torque + fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + return np.array(fext) + + +class ExternalForceList: + """ + This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. + All segment forces are expressed in natural coordinates to be added to the equation of motion as: + + Q @ Qddot + K^T @ lambda = Weight + f_ext + + Attributes + ---------- + external_forces : list + List of ExternalForces + + Examples + -------- + >>> from bionc import ExternalForceList, ExternalForce + >>> import numpy as np + >>> f_ext = ExternalForceList.empty_from_nb_segment(2) + >>> segment_force = ExternalForce(np.array([0,1,1.1]), np.zeros(12,1)) + >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) + """ + + def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): + + if external_forces is None: + raise ValueError("f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)") + self.external_forces = external_forces + + @property + def nb_segments(self) -> int: + return len(self.external_forces) + + @classmethod + def empty_from_nb_segment(cls, nb_segment: int): + """ + Create an empty NaturalExternalForceList from the model size + """ + return cls(external_forces=[[] for _ in range(nb_segment)]) + + def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + return self.external_forces[segment_index] + + def add_external_force(self, segment_index: int, external_force: ExternalForce): + self.external_forces[segment_index].append(external_force) + + def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: + """ + Converts and sums all the segment natural external forces to the full vector of natural external forces + + Parameters + ---------- + Q : NaturalCoordinates + The natural coordinates of the model + """ + + if len(self.external_forces) != Q.nb_qi(): + raise ValueError( + "The number of segment in the model and the number of segment in the external forces must be the same") + + natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) + for segment_index, segment_external_forces in enumerate(self.external_forces): + segment_natural_external_forces = np.zeros((12, 1)) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[:, np.newaxis] + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def __iter__(self): + return iter(self.external_forces) + + def __len__(self): + return len(self.external_forces) diff --git a/bionc/bionc_numpy/natural_external_forces.py b/bionc/bionc_numpy/natural_external_forces.py new file mode 100644 index 00000000..839494cb --- /dev/null +++ b/bionc/bionc_numpy/natural_external_forces.py @@ -0,0 +1,460 @@ +# import numpy as np +# from bionc import SegmentNaturalCoordinates, NaturalCoordinates +# from bionc.bionc_numpy import NaturalVector + + +# class ExternalForce: +# def __init__(self, +# application_point_in_local: np.ndarray, +# external_forces: np.ndarray): +# self.application_point_in_local = application_point_in_local +# self.external_forces = external_forces +# +# @classmethod +# def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): +# return cls(application_point_in_local, np.concatenate((torque, force))) +# +# @property +# def force(self): +# return self.external_forces[3:6] +# +# @property +# def torque(self): +# return self.external_forces[0:3] +# +# @staticmethod +# def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): +# """ +# Return the force moment transformation matrix +# """ +# # default we apply force at the proximal point +# +# left_interpolation_matrix = np.zeros((12, 3)) +# +# left_interpolation_matrix[9:12, 0] = Qi.u +# left_interpolation_matrix[0:3, 1] = Qi.v +# left_interpolation_matrix[3:6, 2] = -Qi.w +# left_interpolation_matrix[6:9, 2] = Qi.w +# +# # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar +# lever_arm_force_matrix = np.zeros((3, 3)) +# +# lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) +# lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) +# lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) +# +# return (left_interpolation_matrix @ lever_arm_force_matrix).T +# +# def to_natural_force(self, Qi: SegmentNaturalCoordinates): +# """ +# Apply external forces to the segment +# +# Parameters +# ---------- +# Qi: SegmentNaturalCoordinates +# Segment natural coordinates +# +# Returns +# ------- +# +# """ +# +# pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) +# +# fext = NaturalVector.proximal().interpolate().T @ self.force +# fext += pseudo_interpolation_matrix.T @ self.torque +# fext += pseudo_interpolation_matrix.T @ np.cross(self.application_point_in_local - Qi.rp, self.force) +# +# return np.array(fext) +# +# +# class ExternalForceList: +# """ +# This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. +# All segment forces are expressed in natural coordinates to be added to the equation of motion as: +# +# Q @ Qddot + K^T @ lambda = Weight + f_ext +# +# Attributes +# ---------- +# external_forces : list +# List of ExternalForces +# +# Examples +# -------- +# >>> from bionc import ExternalForceList, ExternalForce +# >>> import numpy as np +# >>> f_ext = ExternalForceList.empty_from_nb_segment(2) +# >>> segment_force = ExternalForce(np.array([0,1,1.1]), np.zeros(12,1)) +# >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) +# """ +# +# def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): +# +# if external_forces is None: +# raise ValueError("f_ext must be a list of ExternalForces, or use the classmethod" +# "NaturalExternalForceList.empty_from_nb_segment(nb_segment)") +# self.external_forces = external_forces +# +# @property +# def nb_segments(self) -> int: +# return len(self.external_forces) +# +# @classmethod +# def empty_from_nb_segment(cls, nb_segment: int): +# """ +# Create an empty NaturalExternalForceList from the model size +# """ +# return cls(external_forces=[[] for _ in range(nb_segment)]) +# +# def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: +# return self.external_forces[segment_index] +# +# def add_external_force(self, segment_index: int, external_force: ExternalForce): +# self.external_forces[segment_index].append(external_force) +# +# def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: +# """ +# Converts and sums all the segment natural external forces to the full vector of natural external forces +# +# Parameters +# ---------- +# Q : NaturalCoordinates +# The natural coordinates of the model +# """ +# +# if len(self.external_forces) != Q.nb_qi: +# raise ValueError( +# "The number of segment in the model and the number of segment in the external forces must be the same") +# +# natural_external_forces = np.zeros((12 * Q.nb_qi, 1)) +# for segment_index, segment_external_forces in enumerate(self.external_forces): +# segment_natural_external_forces = np.zeros((12, 1)) +# slice_index = slice(segment_index * 12, (segment_index + 1) * 12) +# for external_force in segment_external_forces: +# segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index)) +# natural_external_forces[slice_index] = segment_natural_external_forces +# +# return natural_external_forces +# +# def __iter__(self): +# return iter(self.external_forces) +# +# def __len__(self): +# return len(self.external_forces) + + +# +# +# def cartesian_force_to_natural_force_from_interpolation_matrix(cartesian_force: np.ndarray, interpolation_matrix: InterpolationMatrix): +# """ +# Convert the cartesian force to the natural force +# """ +# if cartesian_force.shape[0] != 3: +# raise ValueError("cartesian_force must be a 3x1 numpy array") +# if cartesian_force.shape.__len__() == 1: +# cartesian_force = cartesian_force[:, np.newaxis] +# +# if isinstance(interpolation_matrix, np.ndarray): +# interpolation_matrix = InterpolationMatrix(interpolation_matrix) +# +# return np.array(interpolation_matrix @ cartesian_force) +# +# def cartesian_force_to_natural_force_from_segment(cartesian_force: np.ndarray, segment_index: int, Q: NaturalCoordinates): +# """ +# Convert the cartesian force to the natural force +# """ +# if cartesian_force.shape[0] != 3: +# raise ValueError("cartesian_force must be a 3x1 numpy array") +# if cartesian_force.shape.__len__() == 1: +# cartesian_force = cartesian_force[:, np.newaxis] +# +# Qi = Q.vector(segment_idx=segment_index) +# # default we apply force at the proximal point +# interpolation_matrix = NaturalVector.proximal().interpolate() +# +# return np.array(segment.interpolation_matrix @ cartesian_force) +# +# def cartesian +# +# +# # There should be several format +# # one in cartesian coordinates +# # one in natural coordinates +# # capitaliser pour créer des efforts generalizee ? +# +# # class SegmentNaturalExternalForce(np.ndarray): +# # """ +# # This class is made to handle External Forces of a Segment in natural coordinates format +# # """ +# # +# # def __new__(cls, input_array: np.ndarray | list | tuple): +# # """ +# # Create a new instance of the class. +# # """ +# # +# # obj = np.asarray(input_array).view(cls) +# # +# # if obj.shape[0] != 12: +# # raise ValueError("input_array must be a 12x1 numpy array") +# # +# # if obj.shape.__len__() == 1: +# # obj = obj[:, np.newaxis] +# # +# # return obj +# # +# # @classmethod +# # def from_cartesian_components( +# # cls, +# # force: np.ndarray | list = None, +# # torque: np.ndarray | list = None, +# # ): +# # """ +# # Constructor of the class from the components of the natural coordinates +# # +# # Parameters +# # ---------- +# # force : np.ndarray | list +# # Force vector in R0 +# # torque : np.ndarray | list +# # Torque vector in R0 +# # # todo : application_point : NaturalVector +# # # Application point in R0 +# # # todo : segment : int +# # # Segment to which the external force is applied +# # # todo : segment_application_point : NaturalVector +# # # Application point in the segment frame +# # +# # Returns +# # ------- +# # ExternalForces +# # """ +# # +# # if force is None: +# # raise ValueError("force must be a numpy array (3x1) or a list of 3 elements") +# # if torque is None: +# # raise ValueError("torque must be a numpy array (3x1) or a list of 3 elements") +# # +# # if not isinstance(force, np.ndarray): +# # force = np.array(force) +# # if not isinstance(torque, np.ndarray): +# # torque = np.array(torque) +# # +# # if force.shape[0] != 3: +# # raise ValueError("force must be a 3x1 numpy array") +# # if torque.shape[0] != 3: +# # raise ValueError("torque must be a 3x1 numpy array") +# # +# # return cls(np.concatenate((force, torque), axis=0)) +# +# class SegmentExternalForce: +# """ +# This class is made to handle External Forces of a Segment in natural coordinates format +# """ +# +# def __init__(self, +# cartesian_force: np.ndarray | list = None, +# cartesian_torque: np.ndarray | list = None, +# segment_index: int = None, +# application_point: NaturalVector = None, +# ): +# self.cartesian_force = cartesian_force +# self.cartesian_torque = cartesian_torque +# self.segment_index = segment_index +# self.application_point = application_point +# +# def to_natural_force(self, Q: NaturalCoordinates): +# """ +# Convert the cartesian force to the natural force +# """ +# interpolation_matrix_proximal_point = NaturalVector.proximal().interpolate() +# force = interpolation_matrix_proximal_point @ self.cartesian_force +# +# +# return np.array(interpolation_matrix @ cartesian_force) +# +# def pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): +# """ +# Return the pseudo interpolation matrix +# """ +# pseudo_interpolation_matrix = np.zeros((3,12)) +# +# pseudo_interpolation_matrix[0, 9:12] = Qi.u +# pseudo_interpolation_matrix[1, 0:3] = Qi.v +# pseudo_interpolation_matrix[2, 3:6] = -Qi.w +# pseudo_interpolation_matrix[2, 6:3] = Qi.w +# +# return pseudo_interpolation_matrix +# +# def force_moment_transformation_matrix(Qi: SegmentNaturalCoordinates): +# """ +# Return the force moment transformation matrix +# """ +# # default we apply force at the proximal point +# force_moment_transformation_matrix = np.zeros((3,12)) +# +# force_moment_transformation_matrix[:, 0] = np.cross(Qi.w, Qi.u) +# force_moment_transformation_matrix[:, 1] = np.cross(Qi.u, Qi.v) +# force_moment_transformation_matrix[:, 2] = np.cross(-Qi.v, Qi.w) +# +# return force_moment_transformation_matrix +# +# def apply_external_forces(Qi: SegmentNaturalCoordinates, external_forces: np.ndarray, application_point_in_local:np.ndarray): +# """ +# Apply external forces to the segment +# +# Parameters +# ---------- +# Qi: SegmentNaturalCoordinates +# Segment natural coordinates +# +# external_forces: np.ndarray +# External forces in cartesian coordinates +# +# Returns +# ------- +# +# """ +# torque = external_forces[3:6] +# force = external_forces[0:3] +# +# pseudo_interpolation_matrix = pseudo_interpolation_matrix(Qi) +# force_moment_transformation_matrix = force_moment_transformation_matrix(Qi) +# +# fext = pseudo_interpolation_matrix.T @ force +# fext += force_moment_transformation_matrix.T @ torque +# fext += force_moment_transformation_matrix.T @ np.cross(application_point_in_local - Qi.rp, force) +# +# return fext +# +# +# class NaturalExternalForces(np.ndarray): +# """ +# This class is made to handle External Forces for all segments in natural coordinates format +# """ +# +# def __new__(cls, input_array: np.ndarray): +# """ +# Create a new instance of the class. +# """ +# +# obj = np.asarray(input_array).view(input_array) +# +# if obj.shape.__len__() == 1: +# obj = obj[:, np.newaxis] +# +# if obj.shape[0] % 12 != 0: +# raise ValueError("input_array must be a [12xN, 1] numpy array") +# +# return obj +# +# @classmethod +# def from_segment_natural_forces(cls, segment_natural_forces: list | tuple): +# """ +# Constructor of the class from the components of the natural coordinates +# +# Parameters +# ---------- +# segment_natural_forces : list +# List of segment natural forces +# +# """ +# if not isinstance(segment_natural_forces, tuple | list): +# raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") +# +# for Q in segment_natural_forces: +# if not isinstance(Q, SegmentNaturalExternalForce): +# raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") +# +# input_array = np.concatenate(segment_natural_forces, axis=0) +# return cls(input_array) +# +# def to_array(self): +# return np.array(self).squeeze() +# +# def vector(self, segment_idx: int) -> SegmentNaturalExternalForce: +# """ +# Return the vector of the external forces +# """ +# return SegmentNaturalExternalForce(self[segment_idx * 12 : (segment_idx + 1) * 12, :].to_array()) +# +# # @classmethod +# # def from_components( +# # cls, +# # force: np.ndarray | list = None, +# # torque: np.ndarray | list = None, +# # application_point: NaturalVector = None, +# # segment: int = None, +# # segment_application_point: NaturalVector = None, +# # ): +# # """ +# # Constructor of the class from the components of the natural coordinates +# # +# # Parameters +# # ---------- +# # force : np.ndarray | list +# # Force vector in R0 +# # torque : np.ndarray | list +# # Torque vector in R0 +# # application_point : NaturalVector +# # Application point in R0 +# # segment : int +# # Segment to which the external force is applied +# # segment_application_point : NaturalVector +# # Application point in the segment frame +# # +# # Returns +# # ------- +# # ExternalForces +# # """ +# # +# # if force is None: +# # raise ValueError("force must be a numpy array (3x1) or a list of 3 elements") +# # if torque is None: +# # raise ValueError("torque must be a numpy array (3x1) or a list of 3 elements") +# # +# # if not isinstance(force, np.ndarray): +# # force = np.array(force) +# # if not isinstance(torque, np.ndarray): +# # torque = np.array(torque) +# # +# # if force.shape[0] != 3: +# # raise ValueError("force must be a 3x1 numpy array") +# # if torque.shape[0] != 3: +# # raise ValueError("torque must be a 3x1 numpy array") +# # +# # external_force = +# # +# # return cls(np.concatenate((force, torque), axis=0)) +# +# +# +# +# # +# +# +# +# +# cartesian_fext = CartesianExternalForces.from_world( +# force=[0, 0, 0], +# torque=[0, 0, 0], +# application_point=CartesianVector.from_components([0, 0, 0]), +# ) +# cartesian_fext.in_ +# segment = 1, +# segment_application_point = CartesianVector.from_components([0, 0, 0]), +# +# +# segment_external_force = SegmentNaturalExternalForces.from_cartesian( +# cartesian_fext, +# segment=1, +# segment_application_point: Marker| other| ...= ..., +# Qi=..., +# ) +# +# # doesn't' look comaptible ... :/ +# external_forces = NaturalExternalForces() +# external_forces.add() +# external_forces.element(1) -> SegmentNaturalExternalForces +# external_forces.in_segement(2) -> tuple[SegmentNaturalExternalForces, ...] +# external_forces.to_equations() diff --git a/examples/forward_dynamics/pendulum_with_force.nmod b/examples/forward_dynamics/pendulum_with_force.nmod new file mode 100644 index 0000000000000000000000000000000000000000..f91c1971fb77eba415d124a80971a00884f1b013 GIT binary patch literal 4122 zcmeHJO>Z1E7~V}X$tu}26hT@LLbOtcL5$N0 z#qx0fuX}%g{rM+%MSwRSvq~jhJ6<)7X0eWv5{+3=<*}Sqqf)H?SS0#c z63`?c6DjVv?WZ{;#qGmJ^GU5Tc-)+}kC7Gi7Kz2`1T$!rawvj`CTT3`h6wEQWJB#D ze4Nws~i4pWkki3v>Xt4Fu?xi0cM<(3C=&Owc%9vJNX>? z7(FIpQv>S`3D8Ze5cgegA_Jo%BgK|Q_BPZ*XzVI6HXn9k8>;F~r|o8ZEnOnkn|RsK zP)19!p&PyR_^I-k(U+q8Z8ImLuM}IZ(}(lyFh;k1Y5DL*-8P2RB_iOoeqciQ?W5C9 zh`5!Hka`N@rs~1~YLI?2r>XznSWS>&)E9#DATSqC>%y37RXzGHAm^g@U2qo9!=vdO z9<6s{9*u7X%cJp~hGON$USIqaqHCX$TOPH9uZg!6v?d;&sXFHHXzd&GXnZFQ8s7|l q9-m((ej35@_|7Z2%VR9?==ti-8}EO~W9@GFW6WB}V;XBU8vFy&>HN?D literal 0 HcmV?d00001 diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py new file mode 100644 index 00000000..70ea6746 --- /dev/null +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -0,0 +1,290 @@ +import numpy as np + +from bionc.bionc_numpy import ( + BiomechanicalModel, + NaturalSegment, + JointType, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, + ExternalForceList, + ExternalForce, +) +from bionc import NaturalAxis, CartesianAxis + + +def drop_the_pendulum( + model: BiomechanicalModel, + Q_init: NaturalCoordinates, + Qdot_init: NaturalVelocities, + external_forces: ExternalForceList, + t_final: float = 2, + steps_per_second: int = 50, +): + """ + This function simulates the dynamics of a natural segment falling from 0m during 2s + + Parameters + ---------- + model : BiomechanicalModel + The model to be simulated + Q_init : SegmentNaturalCoordinates + The initial natural coordinates of the segment + Qdot_init : SegmentNaturalVelocities + The initial natural velocities of the segment + t_final : float, optional + The final time of the simulation, by default 2 + steps_per_second : int, optional + The number of steps per second, by default 50 + + Returns + ------- + tuple: + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + """ + + print("Evaluate Rigid Body Constraints:") + print(model.rigid_body_constraints(Q_init)) + print("Evaluate Rigid Body Constraints Jacobian Derivative:") + print(model.rigid_body_constraint_jacobian_derivative(Qdot_init)) + + if (model.rigid_body_constraints(Q_init) > 1e-6).any(): + print(model.rigid_body_constraints(Q_init)) + raise ValueError( + "The segment natural coordinates don't satisfy the rigid body constraint, at initial conditions." + ) + + t_final = t_final # [s] + steps_per_second = steps_per_second + time_steps = np.linspace(0, t_final, steps_per_second * t_final + 1) + + # initial conditions, x0 = [Qi, Qidot] + states_0 = np.concatenate((Q_init.to_array(), Qdot_init.to_array()), axis=0) + + fext = external_forces + + # Create the forward dynamics function Callable (f(t, x) -> xdot) + def dynamics(t, states): + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + qddot, lambdas = model.forward_dynamics( + NaturalCoordinates(states[idx_coordinates]), + NaturalVelocities(states[idx_velocities]), + external_forces=fext, + ) + return np.concatenate((states[idx_velocities], qddot.to_array()), axis=0), lambdas + + # Solve the Initial Value Problem (IVP) for each time step + normalize_idx = model.normalized_coordinates + all_states = RK4(t=time_steps, f=lambda t, states: dynamics(t, states)[0], y0=states_0, normalize_idx=normalize_idx) + + return time_steps, all_states, dynamics + + +def RK4( + t: np.ndarray, + f, + y0: np.ndarray, + normalize_idx: tuple[tuple[int, ...]] = None, + args=(), +) -> np.ndarray: + """ + Runge-Kutta 4th order method + + Parameters + ---------- + t : array_like + time steps + f : Callable + function to be integrated in the form f(t, y, *args) + y0 : np.ndarray + initial conditions of states + normalize_idx : tuple(tuple) + indices of states to be normalized together + + Returns + ------- + y : array_like + states for each time step + + """ + n = len(t) + y = np.zeros((len(y0), n)) + y[:, 0] = y0 + for i in range(n - 1): + h = t[i + 1] - t[i] + yi = np.squeeze(y[:, i]) + k1 = f(t[i], yi, *args) + k2 = f(t[i] + h / 2.0, yi + k1 * h / 2.0, *args) + k3 = f(t[i] + h / 2.0, yi + k2 * h / 2.0, *args) + k4 = f(t[i] + h, yi + k3 * h, *args) + y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + + if normalize_idx is not None: + for idx in normalize_idx: + y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) + return y + + +def post_computations(model: BiomechanicalModel, time_steps: np.ndarray, all_states: np.ndarray, dynamics): + """ + This function computes: + - the rigid body constraint error + - the rigid body constraint jacobian derivative error + - the joint constraint error + - the lagrange multipliers of the rigid body constraint + + Parameters + ---------- + model : NaturalSegment + The segment to be simulated + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + + Returns + ------- + tuple: + rigid_body_constraint_error : np.ndarray + The rigid body constraint error at each time step + rigid_body_constraint_jacobian_derivative_error : np.ndarray + The rigid body constraint jacobian derivative error at each time step + joint_constraints: np.ndarray + The joint constraints at each time step + lambdas : np.ndarray + The lagrange multipliers of the rigid body constraint at each time step + """ + idx_coordinates = slice(0, model.nb_Q) + idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) + + # compute the quantities of interest after the integration + all_lambdas = np.zeros((model.nb_holonomic_constraints, len(time_steps))) + defects = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + defects_dot = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) + joint_defects = np.zeros((model.nb_joint_constraints, len(time_steps))) + joint_defects_dot = np.zeros((model.nb_joint_constraints, len(time_steps))) + + for i in range(len(time_steps)): + defects[:, i] = model.rigid_body_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + defects_dot[:, i] = model.rigid_body_constraints_derivative( + NaturalCoordinates(all_states[idx_coordinates, i]), NaturalVelocities(all_states[idx_velocities, i]) + ) + + joint_defects[:, i] = model.joint_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) + # todo : to be implemented + # joint_defects_dot = model.joint_constraints_derivative( + # NaturalCoordinates(all_states[idx_coordinates, i]), + # NaturalVelocities(all_states[idx_velocities, i])) + # ) + + all_lambdas[:, i : i + 1] = dynamics(time_steps[i], all_states[:, i])[1] + + return defects, defects_dot, joint_defects, all_lambdas + + +if __name__ == "__main__": + # Let's create a model + model = BiomechanicalModel() + # fill the biomechanical model with the segment + model["pendulum"] = NaturalSegment( + name="pendulum", + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates a orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=1, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="pendulum", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + + model.save("pendulum_with_force.nmod") + + print(model.joints) + print(model.nb_joints) + print(model.nb_joint_constraints) + + Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) + Q = NaturalCoordinates(Qi) + Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + Qdot = NaturalVelocities(Qdoti) + + print(model.joint_constraints(Q)) + print(model.joint_constraints_jacobian(Q)) + print(model.holonomic_constraints(Q)) + print(model.holonomic_constraints_jacobian(Q)) + + # The actual simulation + t_final = 10 + + # add an external force applied on the segment 0 + fext = ExternalForceList.empty_from_nb_segment(1) + force1 = ExternalForce.from_components( + # force=np.array([0, 0, 1 * 9.81]), + # # force=np.array([0, 0, 0]), + # torque=np.array([0, 0, 0]), + # application_point_in_global=np.array([0, -0.5, 0]), + force=np.array([0, 0, 0]), + # force=np.array([0, 0, 0]), + torque=np.array([-5, 0, 0]), + application_point_in_global=np.array([0, 0, 0]), + ) + fext.add_external_force( + external_force=force1, + segment_index=0 + ) + + time_steps, all_states, dynamics = drop_the_pendulum( + model=model, + Q_init=Q, + Qdot_init=Qdot, + external_forces=fext, + t_final=t_final, + steps_per_second=60, + ) + + # defects, defects_dot, joint_defects, all_lambdas = post_computations( + # model=model, + # time_steps=time_steps, + # all_states=all_states, + # dynamics=dynamics, + # ) + + from viz import plot_series + + # Plot the results + # the following graphs have to be near zero the more the simulation is long, the more constraints drift from zero + # plot_series(time_steps, defects, legend="rigid_constraint") # Phi_r + # plot_series(time_steps, defects_dot, legend="rigid_constraint_derivative") # Phi_r_dot + # plot_series(time_steps, joint_defects, legend="joint_constraint") # Phi_j + # the lagrange multipliers are the forces applied to maintain the system (rigidbody and joint constraints) + # plot_series(time_steps, all_lambdas, legend="lagrange_multipliers") # lambda + + # animate the motion + from bionc import Viz + + viz = Viz(model) + viz.animate(all_states[:12, :], None) diff --git a/examples/inverse_kinematics/inverse_kinematics_study.py b/examples/inverse_kinematics/inverse_kinematics_study.py index 57d05e38..475411ea 100644 --- a/examples/inverse_kinematics/inverse_kinematics_study.py +++ b/examples/inverse_kinematics/inverse_kinematics_study.py @@ -25,26 +25,39 @@ def main(): markers = markers + np.random.normal(0, 0.01, markers.shape) # add noise # you can import the class from bionc - ik_solver = InverseKinematics(model, markers, solve_frame_per_frame=True) - # or you can use the model method - ik_solver = model.inverse_kinematics(markers, solve_frame_per_frame=True) - - tic0 = time.time() - Qopt_sqp = ik_solver.solve(method="sqpmethod") # tend to be faster (with limited-memory hessian approximation) - toc0 = time.time() + ik_solver = InverseKinematics(model, markers, solve_frame_per_frame=True, active_direct_frame_constraints=True, use_sx=True) tic1 = time.time() - Qopt_ipopt = ik_solver.solve(method="ipopt") # tend to find lower cost functions but may flip axis. + Qopt_ipopt = ik_solver.solve(method="sqpmethod") # tend to find lower cost functions but may flip axis. toc1 = time.time() - print(f"Time to solve 200 frames with sqpmethod: {toc0 - tic0}") print(f"time to solve 200 frames with ipopt: {toc1 - tic1}") - return ik_solver, Qopt_sqp, Qopt_ipopt, model, markers + return ik_solver, Qopt_ipopt, model, markers if __name__ == "__main__": - ik_solver, Qopt, _, model, markers = main() + ik_solver, Qopt, model, markers = main() + + # display the results of the optimization + import matplotlib.pyplot as plt + plt.figure() + from bionc import NaturalCoordinates + det = np.zeros((model.nb_segments, Qopt.shape[1])) + for i in range(0, Qopt.shape[1]): + Qi = NaturalCoordinates(Qopt)[:, i:i+1] + for s in range(0, model.nb_segments): + u, v, w = Qi.vector(s).to_uvw() + matrix = np.concatenate((u[:,np.newaxis], v[:,np.newaxis], w[:,np.newaxis]), axis=1) + det[s, i] = np.linalg.det(matrix) + if det[s, i] < 0: + print(f"frame {i} segment {s} has a negative determinant") + + plt.plot(det.T, label=model.segment_names, marker="o", ms=1.5) + # set ylim -5 5 + plt.ylim(-5, 5) + plt.legend() + plt.show() viz = Viz( model, @@ -52,4 +65,4 @@ def main(): show_xp_markers=True, show_model_markers=True, ) - viz.animate(Qopt, markers_xp=markers) + viz.animate(Qopt[:, 197:198], markers_xp=markers[:,:,197:198]) diff --git a/tests/test_external_force.py b/tests/test_external_force.py new file mode 100644 index 00000000..c08dee10 --- /dev/null +++ b/tests/test_external_force.py @@ -0,0 +1,70 @@ +import pytest +from .utils import TestUtils +import numpy as np + + +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + # "casadi", + ], +) +def test_joints(bionc_type): + if bionc_type == "casadi": + from bionc.bionc_casadi import ( + BiomechanicalModel, + NaturalSegment, + SegmentNaturalCoordinates, SegmentNaturalVelocities, + Joint, + GroundJoint, + ExternalForceList, ExternalForce, + ) + else: + from bionc.bionc_numpy import ( + BiomechanicalModel, + NaturalSegment, + SegmentNaturalCoordinates,SegmentNaturalVelocities, + ExternalForceList, ExternalForce, + ) + + if bionc_type == "numpy": + from bionc.bionc_numpy import NaturalCoordinates, NaturalVelocities + else: + from bionc.bionc_casadi import NaturalCoordinates, NaturalVelocities + + bionc = TestUtils.bionc_folder() + module = TestUtils.load_module(bionc + "/examples/forward_dynamics/n_link_pendulum.py") + + nb_segments = 1 + model = module.build_n_link_pendulum(nb_segments=nb_segments) + if bionc_type == "casadi": + model = model.to_mx() + + tuple_of_Q = [ + SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, -i, 0], rd=[0, -i - 1, 0], w=[0, 0, 1]) + for i in range(0, nb_segments) + ] + Q = NaturalCoordinates.from_qi(tuple(tuple_of_Q)) + + tuple_of_Qdot = [ + SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + for i in range(0, nb_segments) + ] + Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) + + fext = ExternalForceList.empty_from_nb_segment(nb_segments) + force1 = ExternalForce.from_components( + # force=np.array([0, 0, 1 * 9.81]), + force=np.array([0, 0, 0]), + torque=np.array([0, 0, 0]), + application_point_in_global=np.array([0, 0.1, 0]), + ) + fext.add_external_force(external_force=force1, segment_index=0) + + Qddot, lagrange_multipliers = model.forward_dynamics(Q, Qdot, external_forces=fext) + print(Qddot) + print(lagrange_multipliers) + + + From b7f084e4e7b7c5597bff027cb9bdbdff0b2b5765 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 12:33:11 -0400 Subject: [PATCH 17/40] doc and fix --- bionc/bionc_numpy/natural_coordinates.py | 32 ++++++++++++++++++------ 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 8f1fed6a..a503a66b 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -258,19 +258,37 @@ def __init__(self, self.external_forces = external_forces @classmethod - def from_components(cls, application_point_in_global: np.ndarray, force: np.ndarray, torque: np.ndarray): - return cls(application_point_in_global, np.concatenate((torque, force))) + def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + + """ + + return cls(application_point_in_local, np.concatenate((torque, force))) @property - def force(self): + def force(self) -> np.ndarray: return self.external_forces[3:6] @property - def torque(self): + def torque(self) -> np.ndarray: return self.external_forces[0:3] @staticmethod - def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> np.ndarray: """ Return the force moment transformation matrix """ @@ -290,9 +308,9 @@ def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) - return (left_interpolation_matrix @ lever_arm_force_matrix).T + return (left_interpolation_matrix @ np.linalg.inv(lever_arm_force_matrix)).T - def to_natural_force(self, Qi: SegmentNaturalCoordinates): + def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: """ Apply external forces to the segment From 428c7f9c6814acef52591124855e873d7ed69eb3 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:07:05 -0400 Subject: [PATCH 18/40] commenting moment transport for now --- bionc/bionc_numpy/natural_coordinates.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index a503a66b..e651e068 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -330,7 +330,9 @@ def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: fext = point_interpolation_matrix.T @ self.force fext += pseudo_interpolation_matrix.T @ self.torque - fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + # Bour's formula to transport the moment from the application point to the proximal point + # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) return np.array(fext) From 261e1171eeb326d83a09ae701732587feda89574 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:07:17 -0400 Subject: [PATCH 19/40] test working --- .../forward_dynamics/pendulum_with_force.py | 227 +++++++----------- pendulum_with_force.nmod | Bin 0 -> 4122 bytes tests/test_external_force.py | 88 +++---- 3 files changed, 128 insertions(+), 187 deletions(-) create mode 100644 pendulum_with_force.nmod diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py index 70ea6746..fe1a1e56 100644 --- a/examples/forward_dynamics/pendulum_with_force.py +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -14,6 +14,83 @@ from bionc import NaturalAxis, CartesianAxis +def build_pendulum(): + # Let's create a model + model = BiomechanicalModel() + # fill the biomechanical model with the segment + model["pendulum"] = NaturalSegment( + name="pendulum", + alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates a orthogonal coordinate system + beta=np.pi / 2, + gamma=np.pi / 2, + length=1, + mass=1, + center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system + inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system + ) + # add a revolute joint (still experimental) + # if you want to add a revolute joint, + # you need to ensure that x is always orthogonal to u and v + model._add_joint( + dict( + name="hinge", + joint_type=JointType.GROUND_REVOLUTE, + parent="GROUND", + child="pendulum", + parent_axis=[CartesianAxis.X, CartesianAxis.X], + child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis + theta=[np.pi / 2, np.pi / 2], + ) + ) + + model.save("pendulum_with_force.nmod") + + return model + + +def apply_force_and_drop_pendulum(t_final: float = 10, external_forces: ExternalForceList = None): + """ + This function is used to test the external force + + Parameters + ---------- + t_final: float + The final time of the simulation + external_forces: ExternalForceList + The external forces applied to the model + + Returns + ------- + tuple[BiomechanicalModel, np.ndarray, np.ndarray, Callable]: + model : BiomechanicalModel + The model to be simulated + time_steps : np.ndarray + The time steps of the simulation + all_states : np.ndarray + The states of the system at each time step X = [Q, Qdot] + dynamics : Callable + The dynamics of the system, f(t, X) = [Xdot, lambdas] + + """ + model = build_pendulum() + + Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) + Q = NaturalCoordinates(Qi) + Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + Qdot = NaturalVelocities(Qdoti) + + time_steps, all_states, dynamics = drop_the_pendulum( + model=model, + Q_init=Q, + Qdot_init=Qdot, + external_forces=external_forces, + t_final=t_final, + steps_per_second=60, + ) + + return model, time_steps, all_states, dynamics + + def drop_the_pendulum( model: BiomechanicalModel, Q_init: NaturalCoordinates, @@ -33,6 +110,8 @@ def drop_the_pendulum( The initial natural coordinates of the segment Qdot_init : SegmentNaturalVelocities The initial natural velocities of the segment + external_forces : ExternalForceList + The external forces applied to the model t_final : float, optional The final time of the simulation, by default 2 steps_per_second : int, optional @@ -127,161 +206,35 @@ def RK4( k4 = f(t[i] + h, yi + k3 * h, *args) y[:, i + 1] = yi + (h / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4) + # verify after each time step the normalization of the states if normalize_idx is not None: for idx in normalize_idx: y[idx, i + 1] = y[idx, i + 1] / np.linalg.norm(y[idx, i + 1]) return y -def post_computations(model: BiomechanicalModel, time_steps: np.ndarray, all_states: np.ndarray, dynamics): - """ - This function computes: - - the rigid body constraint error - - the rigid body constraint jacobian derivative error - - the joint constraint error - - the lagrange multipliers of the rigid body constraint - - Parameters - ---------- - model : NaturalSegment - The segment to be simulated - time_steps : np.ndarray - The time steps of the simulation - all_states : np.ndarray - The states of the system at each time step X = [Q, Qdot] - dynamics : Callable - The dynamics of the system, f(t, X) = [Xdot, lambdas] - - Returns - ------- - tuple: - rigid_body_constraint_error : np.ndarray - The rigid body constraint error at each time step - rigid_body_constraint_jacobian_derivative_error : np.ndarray - The rigid body constraint jacobian derivative error at each time step - joint_constraints: np.ndarray - The joint constraints at each time step - lambdas : np.ndarray - The lagrange multipliers of the rigid body constraint at each time step - """ - idx_coordinates = slice(0, model.nb_Q) - idx_velocities = slice(model.nb_Q, model.nb_Q + model.nb_Qdot) - - # compute the quantities of interest after the integration - all_lambdas = np.zeros((model.nb_holonomic_constraints, len(time_steps))) - defects = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) - defects_dot = np.zeros((model.nb_rigid_body_constraints, len(time_steps))) - joint_defects = np.zeros((model.nb_joint_constraints, len(time_steps))) - joint_defects_dot = np.zeros((model.nb_joint_constraints, len(time_steps))) - - for i in range(len(time_steps)): - defects[:, i] = model.rigid_body_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) - defects_dot[:, i] = model.rigid_body_constraints_derivative( - NaturalCoordinates(all_states[idx_coordinates, i]), NaturalVelocities(all_states[idx_velocities, i]) - ) - - joint_defects[:, i] = model.joint_constraints(NaturalCoordinates(all_states[idx_coordinates, i])) - # todo : to be implemented - # joint_defects_dot = model.joint_constraints_derivative( - # NaturalCoordinates(all_states[idx_coordinates, i]), - # NaturalVelocities(all_states[idx_velocities, i])) - # ) - - all_lambdas[:, i : i + 1] = dynamics(time_steps[i], all_states[:, i])[1] - - return defects, defects_dot, joint_defects, all_lambdas - - if __name__ == "__main__": - # Let's create a model - model = BiomechanicalModel() - # fill the biomechanical model with the segment - model["pendulum"] = NaturalSegment( - name="pendulum", - alpha=np.pi / 2, # setting alpha, beta, gamma to pi/2 creates a orthogonal coordinate system - beta=np.pi / 2, - gamma=np.pi / 2, - length=1, - mass=1, - center_of_mass=np.array([0, -0.5, 0]), # in segment coordinates system - inertia=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]), # in segment coordinates system - ) - # add a revolute joint (still experimental) - # if you want to add a revolute joint, - # you need to ensure that x is always orthogonal to u and v - model._add_joint( - dict( - name="hinge", - joint_type=JointType.GROUND_REVOLUTE, - parent="GROUND", - child="pendulum", - parent_axis=[CartesianAxis.X, CartesianAxis.X], - child_axis=[NaturalAxis.V, NaturalAxis.W], # meaning we pivot around the cartesian x-axis - theta=[np.pi / 2, np.pi / 2], - ) - ) - - model.save("pendulum_with_force.nmod") - - print(model.joints) - print(model.nb_joints) - print(model.nb_joint_constraints) - - Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) - Q = NaturalCoordinates(Qi) - Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) - Qdot = NaturalVelocities(Qdoti) - - print(model.joint_constraints(Q)) - print(model.joint_constraints_jacobian(Q)) - print(model.holonomic_constraints(Q)) - print(model.holonomic_constraints_jacobian(Q)) - - # The actual simulation - t_final = 10 # add an external force applied on the segment 0 + # first build the object fext = ExternalForceList.empty_from_nb_segment(1) + # then add a force force1 = ExternalForce.from_components( - # force=np.array([0, 0, 1 * 9.81]), - # # force=np.array([0, 0, 0]), - # torque=np.array([0, 0, 0]), - # application_point_in_global=np.array([0, -0.5, 0]), - force=np.array([0, 0, 0]), + force=np.array([0, 0, 1 * 9.81]), # force=np.array([0, 0, 0]), - torque=np.array([-5, 0, 0]), - application_point_in_global=np.array([0, 0, 0]), + torque=np.array([0, 0, 0]), + application_point_in_local=np.array([0, -0.5, 0]), + # force=np.array([0, 0, 0]), + # torque=np.array([-1 * 9.81 * 0.50, 0, 0]), + # application_point_in_local=np.array([0, 0, 0]), ) + # then add the force to the list on segment 0 fext.add_external_force( external_force=force1, segment_index=0 ) - time_steps, all_states, dynamics = drop_the_pendulum( - model=model, - Q_init=Q, - Qdot_init=Qdot, - external_forces=fext, - t_final=t_final, - steps_per_second=60, - ) - - # defects, defects_dot, joint_defects, all_lambdas = post_computations( - # model=model, - # time_steps=time_steps, - # all_states=all_states, - # dynamics=dynamics, - # ) - - from viz import plot_series - - # Plot the results - # the following graphs have to be near zero the more the simulation is long, the more constraints drift from zero - # plot_series(time_steps, defects, legend="rigid_constraint") # Phi_r - # plot_series(time_steps, defects_dot, legend="rigid_constraint_derivative") # Phi_r_dot - # plot_series(time_steps, joint_defects, legend="joint_constraint") # Phi_j - # the lagrange multipliers are the forces applied to maintain the system (rigidbody and joint constraints) - # plot_series(time_steps, all_lambdas, legend="lagrange_multipliers") # lambda + model, time_steps, all_states, dynamics = apply_force_and_drop_pendulum(t_final=10, external_forces=fext) # animate the motion from bionc import Viz diff --git a/pendulum_with_force.nmod b/pendulum_with_force.nmod new file mode 100644 index 0000000000000000000000000000000000000000..f91c1971fb77eba415d124a80971a00884f1b013 GIT binary patch literal 4122 zcmeHJO>Z1E7~V}X$tu}26hT@LLbOtcL5$N0 z#qx0fuX}%g{rM+%MSwRSvq~jhJ6<)7X0eWv5{+3=<*}Sqqf)H?SS0#c z63`?c6DjVv?WZ{;#qGmJ^GU5Tc-)+}kC7Gi7Kz2`1T$!rawvj`CTT3`h6wEQWJB#D ze4Nws~i4pWkki3v>Xt4Fu?xi0cM<(3C=&Owc%9vJNX>? z7(FIpQv>S`3D8Ze5cgegA_Jo%BgK|Q_BPZ*XzVI6HXn9k8>;F~r|o8ZEnOnkn|RsK zP)19!p&PyR_^I-k(U+q8Z8ImLuM}IZ(}(lyFh;k1Y5DL*-8P2RB_iOoeqciQ?W5C9 zh`5!Hka`N@rs~1~YLI?2r>XznSWS>&)E9#DATSqC>%y37RXzGHAm^g@U2qo9!=vdO z9<6s{9*u7X%cJp~hGON$USIqaqHCX$TOPH9uZg!6v?d;&sXFHHXzd&GXnZFQ8s7|l q9-m((ej35@_|7Z2%VR9?==ti-8}EO~W9@GFW6WB}V;XBU8vFy&>HN?D literal 0 HcmV?d00001 diff --git a/tests/test_external_force.py b/tests/test_external_force.py index c08dee10..0379b16d 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -7,64 +7,52 @@ "bionc_type", [ "numpy", - # "casadi", ], ) -def test_joints(bionc_type): - if bionc_type == "casadi": - from bionc.bionc_casadi import ( - BiomechanicalModel, - NaturalSegment, - SegmentNaturalCoordinates, SegmentNaturalVelocities, - Joint, - GroundJoint, - ExternalForceList, ExternalForce, - ) - else: - from bionc.bionc_numpy import ( - BiomechanicalModel, - NaturalSegment, - SegmentNaturalCoordinates,SegmentNaturalVelocities, - ExternalForceList, ExternalForce, - ) - - if bionc_type == "numpy": - from bionc.bionc_numpy import NaturalCoordinates, NaturalVelocities - else: - from bionc.bionc_casadi import NaturalCoordinates, NaturalVelocities +@pytest.mark.parametrize( + "external_force_tuple", + [ + (np.array([0, 0, 1 * 9.81]), np.array([0, 0, 0]), np.array([0, -0.5, 0])), + (np.array([0, 0, 0]), np.array([-1 * 9.81 * 0.50, 0, 0]), np.array([0, 0, 0])), + ], +) +def test_external_force(bionc_type, external_force_tuple): + from bionc.bionc_numpy import ( + ExternalForceList, ExternalForce, + SegmentNaturalCoordinates, NaturalCoordinates, + SegmentNaturalVelocities, NaturalVelocities, + ) bionc = TestUtils.bionc_folder() - module = TestUtils.load_module(bionc + "/examples/forward_dynamics/n_link_pendulum.py") - - nb_segments = 1 - model = module.build_n_link_pendulum(nb_segments=nb_segments) - if bionc_type == "casadi": - model = model.to_mx() - - tuple_of_Q = [ - SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, -i, 0], rd=[0, -i - 1, 0], w=[0, 0, 1]) - for i in range(0, nb_segments) - ] - Q = NaturalCoordinates.from_qi(tuple(tuple_of_Q)) - - tuple_of_Qdot = [ - SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) - for i in range(0, nb_segments) - ] - Qdot = NaturalVelocities.from_qdoti(tuple(tuple_of_Qdot)) - - fext = ExternalForceList.empty_from_nb_segment(nb_segments) + module = TestUtils.load_module(bionc + "/examples/forward_dynamics/pendulum_with_force.py") + + fext = ExternalForceList.empty_from_nb_segment(1) force1 = ExternalForce.from_components( - # force=np.array([0, 0, 1 * 9.81]), - force=np.array([0, 0, 0]), - torque=np.array([0, 0, 0]), - application_point_in_global=np.array([0, 0.1, 0]), + force=external_force_tuple[0], + torque=external_force_tuple[1], + application_point_in_local=external_force_tuple[2], ) fext.add_external_force(external_force=force1, segment_index=0) - Qddot, lagrange_multipliers = model.forward_dynamics(Q, Qdot, external_forces=fext) - print(Qddot) - print(lagrange_multipliers) + _, _, all_states, _ = module.apply_force_and_drop_pendulum(t_final=1, external_forces=fext) + + # check that the pendulum is not moving + Qi = SegmentNaturalCoordinates.from_components(u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1]) + Q = NaturalCoordinates(Qi) + Qdoti = SegmentNaturalVelocities.from_components(udot=[0, 0, 0], rpdot=[0, 0, 0], rddot=[0, 0, 0], wdot=[0, 0, 0]) + Qdot = NaturalVelocities(Qdoti) + + np.testing.assert_allclose(all_states[:12, 0], Q.to_array(), atol=1e-6) + np.testing.assert_allclose(all_states[:12, -1], Q.to_array(), atol=1e-6) + + np.testing.assert_allclose(all_states[12:, 0], Qdot.to_array(), atol=1e-6) + np.testing.assert_allclose(all_states[12:, -1], Qdot.to_array(), atol=1e-6) + + + + + + From 5254b04270783fb9460fd9fed99acbd7fce90f5b Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:38:43 -0400 Subject: [PATCH 20/40] test subsequent functions --- tests/test_external_force.py | 114 +++++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index 0379b16d..fb83c67b 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -49,6 +49,120 @@ def test_external_force(bionc_type, external_force_tuple): np.testing.assert_allclose(all_states[12:, -1], Qdot.to_array(), atol=1e-6) +@pytest.mark.parametrize( + "bionc_type", + [ + "numpy", + # "casadi", + ], +) +def test_external_force(bionc_type): + if bionc_type == "numpy": + from bionc.bionc_numpy import ( + ExternalForceList, ExternalForce, + SegmentNaturalCoordinates, NaturalCoordinates, + SegmentNaturalVelocities, NaturalVelocities, + ) + else: + from bionc.bionc_casadi import ( + ExternalForceList, ExternalForce, + SegmentNaturalCoordinates, NaturalCoordinates, + SegmentNaturalVelocities, NaturalVelocities, + ) + + force1 = ExternalForce.from_components( + force=np.array([0.01, 0.02, 0.03]), + torque=np.array([0.04, 0.05, 0.06]), + application_point_in_local=np.array([0.07, 0.08, 0.09]), + ) + force2 = ExternalForce.from_components( + force=np.array([0.11, 0.12, 0.13]), + torque=np.array([0.14, 0.15, 0.16]), + application_point_in_local=np.array([0.17, 0.18, 0.19]), + ) + + TestUtils.assert_equal(force1.torque, np.array([0.04, 0.05, 0.06])) + TestUtils.assert_equal(force1.force, np.array([0.01, 0.02, 0.03])) + TestUtils.assert_equal(force2.torque, np.array([0.14, 0.15, 0.16])) + TestUtils.assert_equal(force2.force, np.array([0.11, 0.12, 0.13])) + + fext = ExternalForceList.empty_from_nb_segment(3) + fext.add_external_force(external_force=force1, segment_index=0) + fext.add_external_force(external_force=force2, segment_index=2) + + # check that the pendulum is not moving + Q0 = SegmentNaturalCoordinates.from_components( + u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1], + ) + Q1 = Q0 + 0.1 + Q2 = Q1 + 0.1 + Q = NaturalCoordinates.from_qi((Q0, Q1, Q2)) + + pseudo_interpolation_matrix = force2.compute_pseudo_interpolation_matrix(Q2) + natural_force =force2.to_natural_force(Q2) + natural_forces = fext.to_natural_external_forces(Q) + + TestUtils.assert_equal(pseudo_interpolation_matrix, np.array([[ 0. , 0.14285714, 0. , 0.17142857, 0.17142857, + 1.02857143, -0.17142857, -0.17142857, -1.02857143, 0. , + 0. , 0. ], + [ 0. , 0.14285714, 0. , 0.02857143, 0.02857143, + 0.17142857, -0.02857143, -0.02857143, -0.17142857, 0.85714286, + 0.14285714, 0.14285714], + [ 0. , 0.85714286, 0. , 0.02857143, 0.02857143, + 0.17142857, -0.02857143, -0.02857143, -0.17142857, 0. , + 0. , 0. ]])) + + TestUtils.assert_equal( + natural_force, + np.array([ 0.0187 , 0.19897143, 0.0221 , 0.16265714, 0.17445714, + 0.35054286, -0.05265714, -0.05445714, -0.22054286, 0.14947143, + 0.04422857, 0.04612857]), + ) + + TestUtils.assert_equal( + np.array([[0.0007], + [0.0614], + [0.0021], + [0.0108], + [0.0216], + [0.0724], + [-0.0008], + [-0.0016], + [-0.0424], + [0.0509], + [0.0018], + [0.0027], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.], + [0.0187], + [0.19897143], + [0.0221], + [0.16265714], + [0.17445714], + [0.35054286], + [-0.05265714], + [-0.05445714], + [-0.22054286], + [0.14947143], + [0.04422857], + [0.04612857]]), + natural_forces + ) + + + + + From ece63e9619f589d357b24b9ecfaeb0160941ae24 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:38:52 -0400 Subject: [PATCH 21/40] doc --- bionc/bionc_numpy/natural_coordinates.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index e651e068..79b1c5b4 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -291,6 +291,16 @@ def torque(self) -> np.ndarray: def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> np.ndarray: """ Return the force moment transformation matrix + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + np.ndarray + The force moment transformation matrix """ # default we apply force at the proximal point @@ -321,6 +331,8 @@ def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: Returns ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates """ @@ -354,7 +366,7 @@ class ExternalForceList: >>> from bionc import ExternalForceList, ExternalForce >>> import numpy as np >>> f_ext = ExternalForceList.empty_from_nb_segment(2) - >>> segment_force = ExternalForce(np.array([0,1,1.1]), np.zeros(12,1)) + >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) """ From dd9b6248b395c06f718100e086bb26e67076f27d Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:39:06 -0400 Subject: [PATCH 22/40] better example --- examples/forward_dynamics/pendulum_with_force.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py index fe1a1e56..2cfa386d 100644 --- a/examples/forward_dynamics/pendulum_with_force.py +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -220,10 +220,11 @@ def RK4( fext = ExternalForceList.empty_from_nb_segment(1) # then add a force force1 = ExternalForce.from_components( + # this force will prevent the pendulum to fall force=np.array([0, 0, 1 * 9.81]), - # force=np.array([0, 0, 0]), torque=np.array([0, 0, 0]), application_point_in_local=np.array([0, -0.5, 0]), + # this moment will prevent the pendulum to fall # force=np.array([0, 0, 0]), # torque=np.array([-1 * 9.81 * 0.50, 0, 0]), # application_point_in_local=np.array([0, 0, 0]), From efc0080f80be8ff7618fb1da0b82b2963f062470 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:39:50 -0400 Subject: [PATCH 23/40] linted with black --- bionc/__init__.py | 3 +- bionc/bionc_numpy/biomechanical_model.py | 4 +- bionc/bionc_numpy/external_force.py | 5 +- bionc/bionc_numpy/inverse_kinematics.py | 10 +- bionc/bionc_numpy/natural_coordinates.py | 18 +- .../forward_dynamics/pendulum_with_force.py | 6 +- .../inverse_kinematics_study.py | 12 +- tests/test_external_force.py | 201 +++++++++++------- tests/test_joints.py | 2 +- 9 files changed, 162 insertions(+), 99 deletions(-) diff --git a/bionc/__init__.py b/bionc/__init__.py index 3f25c921..3413c288 100644 --- a/bionc/__init__.py +++ b/bionc/__init__.py @@ -18,7 +18,8 @@ InertiaParameters, BiomechanicalModel, JointType, - ExternalForceList, ExternalForce, + ExternalForceList, + ExternalForce, ) from .protocols import natural_coordinates diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index bdd7735c..5a325956 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -663,7 +663,9 @@ 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 + 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) # KKT system diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index 50f3201e..a852a0d0 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -1,4 +1,5 @@ import numpy as np + # from .natural_coordinates import SegmentNaturalCoordinates # from .natural_vector import NaturalVector @@ -7,7 +8,7 @@ def compute_pseudo_interpolation_matrix(Qi): """ Return the pseudo interpolation matrix """ - pseudo_interpolation_matrix = np.zeros((3,12)) + pseudo_interpolation_matrix = np.zeros((3, 12)) pseudo_interpolation_matrix[0, 9:12] = Qi.u pseudo_interpolation_matrix[1, 0:3] = Qi.v @@ -22,7 +23,7 @@ def compute_force_moment_transformation_matrix(Qi): Return the force moment transformation matrix """ # default we apply force at the proximal point - force_moment_transformation_matrix = np.zeros((3,12)) + force_moment_transformation_matrix = np.zeros((3, 12)) force_moment_transformation_matrix[:, 0] = np.cross(Qi.w, Qi.u) force_moment_transformation_matrix[:, 1] = np.cross(Qi.u, Qi.v) diff --git a/bionc/bionc_numpy/inverse_kinematics.py b/bionc/bionc_numpy/inverse_kinematics.py index b28b0e46..ceaa19ca 100644 --- a/bionc/bionc_numpy/inverse_kinematics.py +++ b/bionc/bionc_numpy/inverse_kinematics.py @@ -267,11 +267,11 @@ def solve( ubg = np.concatenate((ubg, np.full(self.model.nb_segments, np.inf))) nlp = dict( x=self._vert_Q_sym, - g=_mx_to_sx(constraints,[self._vert_Q_sym]) if self.use_sx else constraints, + g=_mx_to_sx(constraints, [self._vert_Q_sym]) if self.use_sx else constraints, ) for f in range(self.nb_frames): objective = self._objective_function(self._Q_sym, self.experimental_markers[:, :, f]) - nlp["f"] = _mx_to_sx(objective,[self._vert_Q_sym]) if self.use_sx else objective + nlp["f"] = _mx_to_sx(objective, [self._vert_Q_sym]) if self.use_sx else objective Q_init = self.Q_init[:, f : f + 1] r = _solve_nlp(method, nlp, Q_init, lbg, ubg, options) Qopt[:, f : f + 1] = r["x"].toarray() @@ -282,8 +282,8 @@ def solve( objective = self._objective(self._Q_sym, self.experimental_markers) nlp = dict( x=self._vert_Q_sym, - f=_mx_to_sx(objective,[self._vert_Q_sym]) if self.use_sx else objective, - g=_mx_to_sx(constraints,[self._vert_Q_sym]) if self.use_sx else constraints, + f=_mx_to_sx(objective, [self._vert_Q_sym]) if self.use_sx else objective, + g=_mx_to_sx(constraints, [self._vert_Q_sym]) if self.use_sx else constraints, ) Q_init = self.Q_init.reshape((12 * self.model.nb_segments * self.nb_frames, 1)) lbg = np.zeros(self.model.nb_holonomic_constraints * self.nb_frames) @@ -352,7 +352,7 @@ def check_segment_determinants(self): """Checks the determinant of each segment frame""" self.segment_determinants = np.zeros((self.model.nb_segments, self.nb_frames)) for i in range(0, self.Qopt.shape[1]): - Qi = NaturalCoordinatesNumpy(self.Qopt)[:, i:i + 1] + Qi = NaturalCoordinatesNumpy(self.Qopt)[:, i : i + 1] for s in range(0, self.model.nb_segments): u, v, w = Qi.vector(s).to_uvw() matrix = np.concatenate((u[:, np.newaxis], v[:, np.newaxis], w[:, np.newaxis]), axis=1) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 79b1c5b4..162c2437 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -251,9 +251,7 @@ def vector(self, segment_idx: int): class ExternalForce: - def __init__(self, - application_point_in_local: np.ndarray, - external_forces: np.ndarray): + def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): self.application_point_in_local = application_point_in_local self.external_forces = external_forces @@ -371,10 +369,11 @@ class ExternalForceList: """ def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): - if external_forces is None: - raise ValueError("f_ext must be a list of ExternalForces, or use the classmethod" - "NaturalExternalForceList.empty_from_nb_segment(nb_segment)") + raise ValueError( + "f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" + ) self.external_forces = external_forces @property @@ -406,14 +405,17 @@ def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: if len(self.external_forces) != Q.nb_qi(): raise ValueError( - "The number of segment in the model and the number of segment in the external forces must be the same") + "The number of segment in the model and the number of segment in the external forces must be the same" + ) natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) for segment_index, segment_external_forces in enumerate(self.external_forces): segment_natural_external_forces = np.zeros((12, 1)) slice_index = slice(segment_index * 12, (segment_index + 1) * 12) for external_force in segment_external_forces: - segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[:, np.newaxis] + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ + :, np.newaxis + ] natural_external_forces[slice_index, 0:1] = segment_natural_external_forces return natural_external_forces diff --git a/examples/forward_dynamics/pendulum_with_force.py b/examples/forward_dynamics/pendulum_with_force.py index 2cfa386d..ad457f1e 100644 --- a/examples/forward_dynamics/pendulum_with_force.py +++ b/examples/forward_dynamics/pendulum_with_force.py @@ -214,7 +214,6 @@ def RK4( if __name__ == "__main__": - # add an external force applied on the segment 0 # first build the object fext = ExternalForceList.empty_from_nb_segment(1) @@ -230,10 +229,7 @@ def RK4( # application_point_in_local=np.array([0, 0, 0]), ) # then add the force to the list on segment 0 - fext.add_external_force( - external_force=force1, - segment_index=0 - ) + fext.add_external_force(external_force=force1, segment_index=0) model, time_steps, all_states, dynamics = apply_force_and_drop_pendulum(t_final=10, external_forces=fext) diff --git a/examples/inverse_kinematics/inverse_kinematics_study.py b/examples/inverse_kinematics/inverse_kinematics_study.py index 475411ea..78256dff 100644 --- a/examples/inverse_kinematics/inverse_kinematics_study.py +++ b/examples/inverse_kinematics/inverse_kinematics_study.py @@ -25,7 +25,9 @@ def main(): markers = markers + np.random.normal(0, 0.01, markers.shape) # add noise # you can import the class from bionc - ik_solver = InverseKinematics(model, markers, solve_frame_per_frame=True, active_direct_frame_constraints=True, use_sx=True) + ik_solver = InverseKinematics( + model, markers, solve_frame_per_frame=True, active_direct_frame_constraints=True, use_sx=True + ) tic1 = time.time() Qopt_ipopt = ik_solver.solve(method="sqpmethod") # tend to find lower cost functions but may flip axis. @@ -41,14 +43,16 @@ def main(): # display the results of the optimization import matplotlib.pyplot as plt + plt.figure() from bionc import NaturalCoordinates + det = np.zeros((model.nb_segments, Qopt.shape[1])) for i in range(0, Qopt.shape[1]): - Qi = NaturalCoordinates(Qopt)[:, i:i+1] + Qi = NaturalCoordinates(Qopt)[:, i : i + 1] for s in range(0, model.nb_segments): u, v, w = Qi.vector(s).to_uvw() - matrix = np.concatenate((u[:,np.newaxis], v[:,np.newaxis], w[:,np.newaxis]), axis=1) + matrix = np.concatenate((u[:, np.newaxis], v[:, np.newaxis], w[:, np.newaxis]), axis=1) det[s, i] = np.linalg.det(matrix) if det[s, i] < 0: print(f"frame {i} segment {s} has a negative determinant") @@ -65,4 +69,4 @@ def main(): show_xp_markers=True, show_model_markers=True, ) - viz.animate(Qopt[:, 197:198], markers_xp=markers[:,:,197:198]) + viz.animate(Qopt[:, 197:198], markers_xp=markers[:, :, 197:198]) diff --git a/tests/test_external_force.py b/tests/test_external_force.py index fb83c67b..b23dfcf3 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -18,9 +18,12 @@ ) def test_external_force(bionc_type, external_force_tuple): from bionc.bionc_numpy import ( - ExternalForceList, ExternalForce, - SegmentNaturalCoordinates, NaturalCoordinates, - SegmentNaturalVelocities, NaturalVelocities, + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, ) bionc = TestUtils.bionc_folder() @@ -59,15 +62,21 @@ def test_external_force(bionc_type, external_force_tuple): def test_external_force(bionc_type): if bionc_type == "numpy": from bionc.bionc_numpy import ( - ExternalForceList, ExternalForce, - SegmentNaturalCoordinates, NaturalCoordinates, - SegmentNaturalVelocities, NaturalVelocities, + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, ) else: from bionc.bionc_casadi import ( - ExternalForceList, ExternalForce, - SegmentNaturalCoordinates, NaturalCoordinates, - SegmentNaturalVelocities, NaturalVelocities, + ExternalForceList, + ExternalForce, + SegmentNaturalCoordinates, + NaturalCoordinates, + SegmentNaturalVelocities, + NaturalVelocities, ) force1 = ExternalForce.from_components( @@ -92,81 +101,129 @@ def test_external_force(bionc_type): # check that the pendulum is not moving Q0 = SegmentNaturalCoordinates.from_components( - u=[1, 0, 0], rp=[0, 0, 0], rd=[0, -1, 0], w=[0, 0, 1], + u=[1, 0, 0], + rp=[0, 0, 0], + rd=[0, -1, 0], + w=[0, 0, 1], ) Q1 = Q0 + 0.1 Q2 = Q1 + 0.1 Q = NaturalCoordinates.from_qi((Q0, Q1, Q2)) pseudo_interpolation_matrix = force2.compute_pseudo_interpolation_matrix(Q2) - natural_force =force2.to_natural_force(Q2) + natural_force = force2.to_natural_force(Q2) natural_forces = fext.to_natural_external_forces(Q) - TestUtils.assert_equal(pseudo_interpolation_matrix, np.array([[ 0. , 0.14285714, 0. , 0.17142857, 0.17142857, - 1.02857143, -0.17142857, -0.17142857, -1.02857143, 0. , - 0. , 0. ], - [ 0. , 0.14285714, 0. , 0.02857143, 0.02857143, - 0.17142857, -0.02857143, -0.02857143, -0.17142857, 0.85714286, - 0.14285714, 0.14285714], - [ 0. , 0.85714286, 0. , 0.02857143, 0.02857143, - 0.17142857, -0.02857143, -0.02857143, -0.17142857, 0. , - 0. , 0. ]])) + TestUtils.assert_equal( + pseudo_interpolation_matrix, + np.array( + [ + [ + 0.0, + 0.14285714, + 0.0, + 0.17142857, + 0.17142857, + 1.02857143, + -0.17142857, + -0.17142857, + -1.02857143, + 0.0, + 0.0, + 0.0, + ], + [ + 0.0, + 0.14285714, + 0.0, + 0.02857143, + 0.02857143, + 0.17142857, + -0.02857143, + -0.02857143, + -0.17142857, + 0.85714286, + 0.14285714, + 0.14285714, + ], + [ + 0.0, + 0.85714286, + 0.0, + 0.02857143, + 0.02857143, + 0.17142857, + -0.02857143, + -0.02857143, + -0.17142857, + 0.0, + 0.0, + 0.0, + ], + ] + ), + ) TestUtils.assert_equal( natural_force, - np.array([ 0.0187 , 0.19897143, 0.0221 , 0.16265714, 0.17445714, - 0.35054286, -0.05265714, -0.05445714, -0.22054286, 0.14947143, - 0.04422857, 0.04612857]), + np.array( + [ + 0.0187, + 0.19897143, + 0.0221, + 0.16265714, + 0.17445714, + 0.35054286, + -0.05265714, + -0.05445714, + -0.22054286, + 0.14947143, + 0.04422857, + 0.04612857, + ] + ), ) TestUtils.assert_equal( - np.array([[0.0007], - [0.0614], - [0.0021], - [0.0108], - [0.0216], - [0.0724], - [-0.0008], - [-0.0016], - [-0.0424], - [0.0509], - [0.0018], - [0.0027], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.], - [0.0187], - [0.19897143], - [0.0221], - [0.16265714], - [0.17445714], - [0.35054286], - [-0.05265714], - [-0.05445714], - [-0.22054286], - [0.14947143], - [0.04422857], - [0.04612857]]), - natural_forces + np.array( + [ + [0.0007], + [0.0614], + [0.0021], + [0.0108], + [0.0216], + [0.0724], + [-0.0008], + [-0.0016], + [-0.0424], + [0.0509], + [0.0018], + [0.0027], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0], + [0.0187], + [0.19897143], + [0.0221], + [0.16265714], + [0.17445714], + [0.35054286], + [-0.05265714], + [-0.05445714], + [-0.22054286], + [0.14947143], + [0.04422857], + [0.04612857], + ] + ), + natural_forces, ) - - - - - - - - - - - - diff --git a/tests/test_joints.py b/tests/test_joints.py index 1dd235c6..482e0a25 100644 --- a/tests/test_joints.py +++ b/tests/test_joints.py @@ -575,4 +575,4 @@ def test_joints(bionc_type, joint_type: JointType): # j_jacobian_sym = jacobian(j_constraints_sym, sym) # j_jacobian_func = Function("j_jacobian_func", [sym], [j_jacobian_sym]) # -# jacobian_mx = j_jacobian_func(np.arange(24)).toarray() \ No newline at end of file +# jacobian_mx = j_jacobian_func(np.arange(24)).toarray() From 375054f28d7c0a18cfdee16e1773f63beb75b163 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:45:16 -0400 Subject: [PATCH 24/40] docstring --- bionc/bionc_numpy/natural_coordinates.py | 54 ++++++++++++++++++++++-- 1 file changed, 51 insertions(+), 3 deletions(-) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 162c2437..5e0f79fd 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -251,6 +251,29 @@ def vector(self, segment_idx: int): class ExternalForce: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + external_forces : np.ndarray + The external force vector in the global coordinate system (torque, force) + + Methods + ------- + from_components(application_point_in_local, force, torque) + This function creates an external force from its components. + force + This function returns the force vector of the external force. + torque + This function returns the torque vector of the external force. + compute_pseudo_interpolation_matrix() + This function computes the pseudo interpolation matrix of the external force. + to_natural_force + This function returns the external force in the natural coordinate format. + """ def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): self.application_point_in_local = application_point_in_local self.external_forces = external_forces @@ -272,17 +295,18 @@ def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarr Returns ------- ExternalForce - """ return cls(application_point_in_local, np.concatenate((torque, force))) @property def force(self) -> np.ndarray: + """ The force vector in the global coordinate system """ return self.external_forces[3:6] @property def torque(self) -> np.ndarray: + """ The torque vector in the global coordinate system """ return self.external_forces[0:3] @staticmethod @@ -331,7 +355,6 @@ def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: ------- np.ndarray The external forces adequately transformed for the equation of motion in natural coordinates - """ pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) @@ -357,7 +380,20 @@ class ExternalForceList: Attributes ---------- external_forces : list - List of ExternalForces + List of ExternalForces for each segment + + Methods + ------- + add_external_force(segment_index, external_force) + This function adds an external force to the list of external forces. + empty_from_nb_segment(nb_segment) + This function creates an empty ExternalForceList from the number of segments. + to_natural_external_forces(Q) + This function returns the external forces in the natural coordinate format. + segment_external_forces(segment_index) + This function returns the external forces of a segment. + nb_segments + This function returns the number of segments. Examples -------- @@ -378,6 +414,7 @@ def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): @property def nb_segments(self) -> int: + """ Returns the number of segments """ return len(self.external_forces) @classmethod @@ -388,9 +425,20 @@ def empty_from_nb_segment(cls, nb_segment: int): return cls(external_forces=[[] for _ in range(nb_segment)]) def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + """ Returns the external forces of the segment """ return self.external_forces[segment_index] def add_external_force(self, segment_index: int, external_force: ExternalForce): + """ + Add an external force to the segment + + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force to add + """ self.external_forces[segment_index].append(external_force) def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: From 344d84ba7dede04be1b15aa239e7a389c77611e5 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 13:47:04 -0400 Subject: [PATCH 25/40] black --- bionc/bionc_numpy/natural_coordinates.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index 5e0f79fd..f40789b6 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -274,6 +274,7 @@ class ExternalForce: to_natural_force This function returns the external force in the natural coordinate format. """ + def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): self.application_point_in_local = application_point_in_local self.external_forces = external_forces @@ -301,12 +302,12 @@ def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarr @property def force(self) -> np.ndarray: - """ The force vector in the global coordinate system """ + """The force vector in the global coordinate system""" return self.external_forces[3:6] @property def torque(self) -> np.ndarray: - """ The torque vector in the global coordinate system """ + """The torque vector in the global coordinate system""" return self.external_forces[0:3] @staticmethod @@ -414,7 +415,7 @@ def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): @property def nb_segments(self) -> int: - """ Returns the number of segments """ + """Returns the number of segments""" return len(self.external_forces) @classmethod @@ -425,7 +426,7 @@ def empty_from_nb_segment(cls, nb_segment: int): return cls(external_forces=[[] for _ in range(nb_segment)]) def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: - """ Returns the external forces of the segment """ + """Returns the external forces of the segment""" return self.external_forces[segment_index] def add_external_force(self, segment_index: int, external_force: ExternalForce): From 0767e417cfccfd4eb52577962dc54822e3e789f0 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 14:13:08 -0400 Subject: [PATCH 26/40] codecov --- .github/workflows/codecov.yml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 .github/workflows/codecov.yml diff --git a/.github/workflows/codecov.yml b/.github/workflows/codecov.yml new file mode 100644 index 00000000..f5f3975f --- /dev/null +++ b/.github/workflows/codecov.yml @@ -0,0 +1,22 @@ +name: API workflow + +on: [push, pull_request] + +jobs: + build: + runs-on: ubuntu-latest + name: Test python API + steps: + - uses: actions/checkout@master + - name: Install requirements + run: pip install -r requirements.txt + - name: Run tests and collect coverage + run: pytest --cov . + - name: Upload coverage reports to Codecov +# run: | +# # Replace `linux` below with the appropriate OS +# # Options are `alpine`, `linux`, `macos`, `windows` +# curl -Os https://uploader.codecov.io/latest/linux/codecov +# chmod +x codecov +# ./codecov -t ${CODECOV_TOKEN} + - uses: codecov/codecov-action@v3 \ No newline at end of file From f85752671bde7b638ab2012299c0f62e66ad601f Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 14:16:38 -0400 Subject: [PATCH 27/40] cov --- .github/workflows/codecov.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/codecov.yml b/.github/workflows/codecov.yml index f5f3975f..7a80c07a 100644 --- a/.github/workflows/codecov.yml +++ b/.github/workflows/codecov.yml @@ -19,4 +19,7 @@ jobs: # curl -Os https://uploader.codecov.io/latest/linux/codecov # chmod +x codecov # ./codecov -t ${CODECOV_TOKEN} - - uses: codecov/codecov-action@v3 \ No newline at end of file + - uses: codecov/codecov-action@v3 + name: Upload coverage to Codecov + with: + token: ${{ secrets.CODECOV_TOKEN }} \ No newline at end of file From a29ab4c1b126e2b358531c291f7dc5079470d832 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 14:21:40 -0400 Subject: [PATCH 28/40] codecov are you here? --- .github/workflows/codecov.yml | 5 +++-- .github/workflows/run_tests.yml | 10 +++++++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/.github/workflows/codecov.yml b/.github/workflows/codecov.yml index 7a80c07a..62262f7a 100644 --- a/.github/workflows/codecov.yml +++ b/.github/workflows/codecov.yml @@ -1,6 +1,7 @@ -name: API workflow +# Check if the tests covers all the code +name: Codecov(erage). -on: [push, pull_request] +on: [pull_request] jobs: build: diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 70e8319d..842ee53c 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -51,10 +51,18 @@ jobs: run: pytest -v --color=yes --cov-report term-missing --cov=bionc tests if: matrix.label == 'osx-64' +# - name: Upload coverage to Codecov +# run: codecov +# if: matrix.label == 'linux-64' + - name: Upload coverage to Codecov - run: codecov + uses: codecov/codecov-action@v3 + with: + token: ${{ secrets.CODECOV_TOKEN }} + fail_ci_if_error: true if: matrix.label == 'linux-64' + - name: Test installed version of bioviz on LINUX run: | python setup.py install From 65a66b281b1061863ef7e4b1a61362fb81b34449 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 14:54:42 -0400 Subject: [PATCH 29/40] .coverage --- .github/workflows/run_tests.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 842ee53c..851122a9 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -39,7 +39,7 @@ jobs: mamba list - name: Install extra dependencies - run: mamba install pytest-cov pytest pytest-cov codecov -cconda-forge + run: mamba install pytest-cov pytest codecov -cconda-forge - name: Run the actual tests on LINUX run: | @@ -60,6 +60,7 @@ jobs: with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: true + directory: ./coverage if: matrix.label == 'linux-64' From de1e51eba86260cca858c14a770a692f47f453e1 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:06:29 -0400 Subject: [PATCH 30/40] cov --- .github/workflows/run_tests.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 851122a9..00a3aaa0 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -21,7 +21,7 @@ jobs: run: shell: bash -l {0} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@master - name: Setup environment uses: conda-incubator/setup-miniconda@v2 @@ -48,7 +48,7 @@ jobs: if: matrix.label == 'linux-64' - name: Run the actual tests - run: pytest -v --color=yes --cov-report term-missing --cov=bionc tests + run: pytest -v --color=yes --cov-report=xml --cov=bionc tests if: matrix.label == 'osx-64' # - name: Upload coverage to Codecov @@ -60,7 +60,7 @@ jobs: with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: true - directory: ./coverage + directory: ./coverage.xml if: matrix.label == 'linux-64' From 65f28f565cc7ab6ff66037cd76b28904b3717728 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:08:01 -0400 Subject: [PATCH 31/40] coverage.xml --- .github/workflows/run_tests.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index 00a3aaa0..a6a72c3c 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -60,7 +60,8 @@ jobs: with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: true - directory: ./coverage.xml +# directory: ./coverage.xml + files: ./coverage.xml if: matrix.label == 'linux-64' From 9f3808a1b413a8e30ce1d5feb2ca2275815981b6 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:11:46 -0400 Subject: [PATCH 32/40] directory --- .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 a6a72c3c..cd1d1dbc 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -60,7 +60,7 @@ jobs: with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: true -# directory: ./coverage.xml + directory: /home/runner/work/bioNC/bioNC files: ./coverage.xml if: matrix.label == 'linux-64' From a5f189993d7f4ca46083f8b420408ce915a735d4 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:18:13 -0400 Subject: [PATCH 33/40] again --- .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 cd1d1dbc..ab1792ab 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -60,7 +60,7 @@ jobs: with: token: ${{ secrets.CODECOV_TOKEN }} fail_ci_if_error: true - directory: /home/runner/work/bioNC/bioNC + directory: /home/runner/work/bioNC files: ./coverage.xml if: matrix.label == 'linux-64' From 939b2ab6eca041238be3583ec5d68670e4fadf08 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:28:55 -0400 Subject: [PATCH 34/40] cove --- .github/workflows/run_tests.yml | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/.github/workflows/run_tests.yml b/.github/workflows/run_tests.yml index ab1792ab..4d8005dc 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -51,19 +51,19 @@ jobs: run: pytest -v --color=yes --cov-report=xml --cov=bionc tests if: matrix.label == 'osx-64' -# - name: Upload coverage to Codecov -# run: codecov -# if: matrix.label == 'linux-64' - - name: Upload coverage to Codecov - uses: codecov/codecov-action@v3 - with: - token: ${{ secrets.CODECOV_TOKEN }} - fail_ci_if_error: true - directory: /home/runner/work/bioNC - files: ./coverage.xml + run: codecov --${{ secrets.CODECOV_TOKEN }} if: matrix.label == 'linux-64' +# - name: Upload coverage to Codecov +# uses: codecov/codecov-action@v3 +# with: +# token: ${{ secrets.CODECOV_TOKEN }} +# fail_ci_if_error: true +# directory: /home/runner/work/bioNC +# files: ./coverage.xml +# if: matrix.label == 'linux-64' + - name: Test installed version of bioviz on LINUX run: | From 7b90fc3ad4c7202f18147779066375308b513927 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:32:51 -0400 Subject: [PATCH 35/40] token wesh --- .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 4d8005dc..af893f6b 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -52,7 +52,7 @@ jobs: if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov --${{ secrets.CODECOV_TOKEN }} + run: codecov --secrets.CODECOV_TOKEN if: matrix.label == 'linux-64' # - name: Upload coverage to Codecov From 0492620d4a011804ddf15ca209c4b72c50e26dd2 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:33:21 -0400 Subject: [PATCH 36/40] --token== --- .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 af893f6b..8899134b 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -52,7 +52,7 @@ jobs: if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov --secrets.CODECOV_TOKEN + run: codecov --token=${{ secrets.CODECOV_TOKEN }} if: matrix.label == 'linux-64' # - name: Upload coverage to Codecov From c990661a33b999e1420aa2e8b6b1288623d26d76 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:35:54 -0400 Subject: [PATCH 37/40] no = --- .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 8899134b..38351cb6 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -52,7 +52,7 @@ jobs: if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov --token=${{ secrets.CODECOV_TOKEN }} + run: codecov --token ${{ secrets.CODECOV_TOKEN }} if: matrix.label == 'linux-64' # - name: Upload coverage to Codecov From 376b59c26ddc1e79e9ae8482d3d7e4742e7d02be Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:41:23 -0400 Subject: [PATCH 38/40] does this will be uploaded ? --- .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..05f330ac 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -52,7 +52,7 @@ jobs: if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov --token ${{ secrets.CODECOV_TOKEN }} + run: codecov --token ${{ secrets.CODECOV_TOKEN }} --file coverage.xml if: matrix.label == 'linux-64' # - name: Upload coverage to Codecov From e5862dfc65cb921f9c36288e455dd48f4014f8f0 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 15:51:27 -0400 Subject: [PATCH 39/40] let it go --- .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 05f330ac..38351cb6 100644 --- a/.github/workflows/run_tests.yml +++ b/.github/workflows/run_tests.yml @@ -52,7 +52,7 @@ jobs: if: matrix.label == 'osx-64' - name: Upload coverage to Codecov - run: codecov --token ${{ secrets.CODECOV_TOKEN }} --file coverage.xml + run: codecov --token ${{ secrets.CODECOV_TOKEN }} if: matrix.label == 'linux-64' # - name: Upload coverage to Codecov From 11356407a7764d6d80ee2218f12ed2d93ac14083 Mon Sep 17 00:00:00 2001 From: Ipuch Date: Sat, 1 Apr 2023 16:18:44 -0400 Subject: [PATCH 40/40] Casadi and tests --- bionc/bionc_casadi/__init__.py | 1 + bionc/bionc_casadi/external_force.py | 229 +++++++++ bionc/bionc_numpy/__init__.py | 2 +- bionc/bionc_numpy/biomechanical_model.py | 2 +- bionc/bionc_numpy/external_force.py | 257 +++++++++-- bionc/bionc_numpy/natural_coordinates.py | 226 --------- bionc/bionc_numpy/natural_external_forces.py | 460 ------------------- tests/test_external_force.py | 12 +- 8 files changed, 447 insertions(+), 742 deletions(-) create mode 100644 bionc/bionc_casadi/external_force.py delete mode 100644 bionc/bionc_numpy/natural_external_forces.py diff --git a/bionc/bionc_casadi/__init__.py b/bionc/bionc_casadi/__init__.py index 42f3ca16..ce98bc7b 100644 --- a/bionc/bionc_casadi/__init__.py +++ b/bionc/bionc_casadi/__init__.py @@ -14,3 +14,4 @@ from .inertia_parameters import InertiaParameters from .joint import Joint, GroundJoint from .natural_vector import NaturalVector +from .external_force import ExternalForceList, ExternalForce diff --git a/bionc/bionc_casadi/external_force.py b/bionc/bionc_casadi/external_force.py new file mode 100644 index 00000000..1ae61690 --- /dev/null +++ b/bionc/bionc_casadi/external_force.py @@ -0,0 +1,229 @@ +from casadi import MX, inv, cross +import numpy as np + +from .natural_vector import NaturalVector +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates + + +class ExternalForce: + """ + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + external_forces : np.ndarray + The external force vector in the global coordinate system (torque, force) + + Methods + ------- + from_components(application_point_in_local, force, torque) + This function creates an external force from its components. + force + This function returns the force vector of the external force. + torque + This function returns the torque vector of the external force. + compute_pseudo_interpolation_matrix() + This function computes the pseudo interpolation matrix of the external force. + to_natural_force + This function returns the external force in the natural coordinate format. + """ + + def __init__(self, application_point_in_local: np.ndarray | MX, external_forces: np.ndarray | MX): + self.application_point_in_local = MX(application_point_in_local) + 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): + """ + This function creates an external force from its components. + + Parameters + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system + + Returns + ------- + ExternalForce + """ + + return cls(application_point_in_local, np.concatenate((torque, force))) + + @property + def force(self) -> MX: + """The force vector in the global coordinate system""" + return self.external_forces[3:6] + + @property + def torque(self) -> MX: + """The torque vector in the global coordinate system""" + return self.external_forces[0:3] + + @staticmethod + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> MX: + """ + Return the force moment transformation matrix + + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment + + Returns + ------- + np.ndarray + The force moment transformation matrix + """ + # default we apply force at the proximal point + + left_interpolation_matrix = MX.zeros((12, 3)) + + left_interpolation_matrix[9:12, 0] = Qi.u + left_interpolation_matrix[0:3, 1] = Qi.v + left_interpolation_matrix[3:6, 2] = -Qi.w + left_interpolation_matrix[6:9, 2] = Qi.w + + # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar + lever_arm_force_matrix = MX.zeros((3, 3)) + + lever_arm_force_matrix[:, 0] = cross(Qi.w, Qi.u) + lever_arm_force_matrix[:, 1] = cross(Qi.u, Qi.v) + lever_arm_force_matrix[:, 2] = cross(-Qi.v, Qi.w) + + return (left_interpolation_matrix @ inv(lever_arm_force_matrix)).T # NOTE: inv may induce symbolic error. + + def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> MX: + """ + Apply external forces to the segment + + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates + + Returns + ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates + """ + + pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) + point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + application_point_in_global = point_interpolation_matrix @ Qi + + fext = point_interpolation_matrix.T @ self.force + fext += pseudo_interpolation_matrix.T @ self.torque + + # Bour's formula to transport the moment from the application point to the proximal point + # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + return fext + + +class ExternalForceList: + """ + This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. + All segment forces are expressed in natural coordinates to be added to the equation of motion as: + + Q @ Qddot + K^T @ lambda = Weight + f_ext + + Attributes + ---------- + external_forces : list + List of ExternalForces for each segment + + Methods + ------- + add_external_force(segment_index, external_force) + This function adds an external force to the list of external forces. + empty_from_nb_segment(nb_segment) + This function creates an empty ExternalForceList from the number of segments. + to_natural_external_forces(Q) + This function returns the external forces in the natural coordinate format. + segment_external_forces(segment_index) + This function returns the external forces of a segment. + nb_segments + This function returns the number of segments. + + Examples + -------- + >>> from bionc import ExternalForceList, ExternalForce + >>> import numpy as np + >>> f_ext = ExternalForceList.empty_from_nb_segment(2) + >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) + >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) + """ + + def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): + if external_forces is None: + raise ValueError( + "f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" + ) + self.external_forces = external_forces + + @property + def nb_segments(self) -> int: + """Returns the number of segments""" + return len(self.external_forces) + + @classmethod + def empty_from_nb_segment(cls, nb_segment: int): + """ + Create an empty NaturalExternalForceList from the model size + """ + return cls(external_forces=[[] for _ in range(nb_segment)]) + + def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + """Returns the external forces of the segment""" + return self.external_forces[segment_index] + + def add_external_force(self, segment_index: int, external_force: ExternalForce): + """ + Add an external force to the segment + + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force to add + """ + self.external_forces[segment_index].append(external_force) + + def to_natural_external_forces(self, Q: NaturalCoordinates) -> MX: + """ + Converts and sums all the segment natural external forces to the full vector of natural external forces + + Parameters + ---------- + Q : NaturalCoordinates + The natural coordinates of the model + """ + + if len(self.external_forces) != Q.nb_qi(): + raise ValueError( + "The number of segment in the model and the number of segment in the external forces must be the same" + ) + + natural_external_forces = MX.zeros((12 * Q.nb_qi(), 1)) + for segment_index, segment_external_forces in enumerate(self.external_forces): + segment_natural_external_forces = MX.zeros((12, 1)) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index)) + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def __iter__(self): + return iter(self.external_forces) + + def __len__(self): + return len(self.external_forces) diff --git a/bionc/bionc_numpy/__init__.py b/bionc/bionc_numpy/__init__.py index 3184dce2..9b8c1847 100644 --- a/bionc/bionc_numpy/__init__.py +++ b/bionc/bionc_numpy/__init__.py @@ -16,4 +16,4 @@ from .joint import Joint, GroundJoint from .natural_vector import NaturalVector from .inverse_kinematics import InverseKinematics -from .natural_coordinates import ExternalForceList, ExternalForce +from .external_force import ExternalForceList, ExternalForce diff --git a/bionc/bionc_numpy/biomechanical_model.py b/bionc/bionc_numpy/biomechanical_model.py index 5a325956..d748421b 100644 --- a/bionc/bionc_numpy/biomechanical_model.py +++ b/bionc/bionc_numpy/biomechanical_model.py @@ -6,7 +6,7 @@ from .natural_accelerations import NaturalAccelerations from ..protocols.biomechanical_model import GenericBiomechanicalModel from .inverse_kinematics import InverseKinematics -from .natural_coordinates import ExternalForceList +from .external_force import ExternalForceList class BiomechanicalModel(GenericBiomechanicalModel): diff --git a/bionc/bionc_numpy/external_force.py b/bionc/bionc_numpy/external_force.py index a852a0d0..f8ad4c77 100644 --- a/bionc/bionc_numpy/external_force.py +++ b/bionc/bionc_numpy/external_force.py @@ -1,73 +1,230 @@ import numpy as np -# from .natural_coordinates import SegmentNaturalCoordinates -# from .natural_vector import NaturalVector +from .natural_vector import NaturalVector +from .natural_coordinates import SegmentNaturalCoordinates, NaturalCoordinates -def compute_pseudo_interpolation_matrix(Qi): +class ExternalForce: """ - Return the pseudo interpolation matrix + This class represents an external force applied to a segment. + + Attributes + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + external_forces : np.ndarray + The external force vector in the global coordinate system (torque, force) + + Methods + ------- + from_components(application_point_in_local, force, torque) + This function creates an external force from its components. + force + This function returns the force vector of the external force. + torque + This function returns the torque vector of the external force. + compute_pseudo_interpolation_matrix() + This function computes the pseudo interpolation matrix of the external force. + to_natural_force + This function returns the external force in the natural coordinate format. """ - pseudo_interpolation_matrix = np.zeros((3, 12)) - pseudo_interpolation_matrix[0, 9:12] = Qi.u - pseudo_interpolation_matrix[1, 0:3] = Qi.v - pseudo_interpolation_matrix[2, 3:6] = -Qi.w - pseudo_interpolation_matrix[2, 6:3] = Qi.w + def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): + self.application_point_in_local = application_point_in_local + self.external_forces = external_forces - return pseudo_interpolation_matrix + @classmethod + def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): + """ + This function creates an external force from its components. + Parameters + ---------- + application_point_in_local : np.ndarray + The application point of the force in the natural coordinate system of the segment + force + The force vector in the global coordinate system + torque + The torque vector in the global coordinate system -def compute_force_moment_transformation_matrix(Qi): - """ - Return the force moment transformation matrix - """ - # default we apply force at the proximal point - force_moment_transformation_matrix = np.zeros((3, 12)) + Returns + ------- + ExternalForce + """ - force_moment_transformation_matrix[:, 0] = np.cross(Qi.w, Qi.u) - force_moment_transformation_matrix[:, 1] = np.cross(Qi.u, Qi.v) - force_moment_transformation_matrix[:, 2] = np.cross(-Qi.v, Qi.w) + return cls(application_point_in_local, np.concatenate((torque, force))) - return force_moment_transformation_matrix + @property + def force(self) -> np.ndarray: + """The force vector in the global coordinate system""" + return self.external_forces[3:6] + @property + def torque(self) -> np.ndarray: + """The torque vector in the global coordinate system""" + return self.external_forces[0:3] -def to_natural_force(Qi, external_forces: np.ndarray, application_point_in_global: np.ndarray): - """ - Apply external forces to the segment + @staticmethod + def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Return the force moment transformation matrix - Parameters - ---------- - Qi: SegmentNaturalCoordinates - Segment natural coordinates + Parameters + ---------- + Qi : SegmentNaturalCoordinates + The natural coordinates of the segment - external_forces: np.ndarray - External forces in cartesian coordinates + Returns + ------- + np.ndarray + The force moment transformation matrix + """ + # default we apply force at the proximal point - Returns - ------- + left_interpolation_matrix = np.zeros((12, 3)) - """ - torque = external_forces[3:6] - force = external_forces[0:3] + left_interpolation_matrix[9:12, 0] = Qi.u + left_interpolation_matrix[0:3, 1] = Qi.v + left_interpolation_matrix[3:6, 2] = -Qi.w + left_interpolation_matrix[6:9, 2] = Qi.w + + # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar + lever_arm_force_matrix = np.zeros((3, 3)) + + lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) + lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) + lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) + + return (left_interpolation_matrix @ np.linalg.inv(lever_arm_force_matrix)).T - pseudo_interpolation_matrix = compute_pseudo_interpolation_matrix(Qi) - force_moment_transformation_matrix = compute_force_moment_transformation_matrix(Qi) + def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: + """ + Apply external forces to the segment - fext = NaturalVector.proximal().interlopate() @ force - fext += force_moment_transformation_matrix.T @ torque - fext += force_moment_transformation_matrix.T @ np.cross(application_point_in_global - Qi.rp, force) + Parameters + ---------- + Qi: SegmentNaturalCoordinates + Segment natural coordinates - return fext + Returns + ------- + np.ndarray + The external forces adequately transformed for the equation of motion in natural coordinates + """ + pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) + point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() + application_point_in_global = np.array(point_interpolation_matrix @ Qi).squeeze() + + fext = point_interpolation_matrix.T @ self.force + fext += pseudo_interpolation_matrix.T @ self.torque + + # Bour's formula to transport the moment from the application point to the proximal point + # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) + + return np.array(fext) + + +class ExternalForceList: + """ + This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. + All segment forces are expressed in natural coordinates to be added to the equation of motion as: + + Q @ Qddot + K^T @ lambda = Weight + f_ext + + Attributes + ---------- + external_forces : list + List of ExternalForces for each segment + + Methods + ------- + add_external_force(segment_index, external_force) + This function adds an external force to the list of external forces. + empty_from_nb_segment(nb_segment) + This function creates an empty ExternalForceList from the number of segments. + to_natural_external_forces(Q) + This function returns the external forces in the natural coordinate format. + segment_external_forces(segment_index) + This function returns the external forces of a segment. + nb_segments + This function returns the number of segments. + + Examples + -------- + >>> from bionc import ExternalForceList, ExternalForce + >>> import numpy as np + >>> f_ext = ExternalForceList.empty_from_nb_segment(2) + >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) + >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) + """ -# try the functions -# Qi = SegmentNaturalCoordinates.from_components( -# u=np.array([1, 0, 0]), -# rp=np.array([0, 0, 0]), -# rd=np.array([0, 0, 0]), -# w=np.array([0, 0, 1]), -# ) -# external_force = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) -# application_point_in_local = np.array([0.1, 0.2, 0.3]) -# natural_force = to_natural_force(Qi, external_force, application_point_in_local) + def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): + if external_forces is None: + raise ValueError( + "f_ext must be a list of ExternalForces, or use the classmethod" + "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" + ) + self.external_forces = external_forces + + @property + def nb_segments(self) -> int: + """Returns the number of segments""" + return len(self.external_forces) + + @classmethod + def empty_from_nb_segment(cls, nb_segment: int): + """ + Create an empty NaturalExternalForceList from the model size + """ + return cls(external_forces=[[] for _ in range(nb_segment)]) + + def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: + """Returns the external forces of the segment""" + return self.external_forces[segment_index] + + def add_external_force(self, segment_index: int, external_force: ExternalForce): + """ + Add an external force to the segment + + Parameters + ---------- + segment_index: int + The index of the segment + external_force: + The external force to add + """ + self.external_forces[segment_index].append(external_force) + + def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: + """ + Converts and sums all the segment natural external forces to the full vector of natural external forces + + Parameters + ---------- + Q : NaturalCoordinates + The natural coordinates of the model + """ + + if len(self.external_forces) != Q.nb_qi(): + raise ValueError( + "The number of segment in the model and the number of segment in the external forces must be the same" + ) + + natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) + for segment_index, segment_external_forces in enumerate(self.external_forces): + segment_natural_external_forces = np.zeros((12, 1)) + slice_index = slice(segment_index * 12, (segment_index + 1) * 12) + for external_force in segment_external_forces: + segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ + :, np.newaxis + ] + natural_external_forces[slice_index, 0:1] = segment_natural_external_forces + + return natural_external_forces + + def __iter__(self): + return iter(self.external_forces) + + def __len__(self): + return len(self.external_forces) diff --git a/bionc/bionc_numpy/natural_coordinates.py b/bionc/bionc_numpy/natural_coordinates.py index f40789b6..2674aabe 100644 --- a/bionc/bionc_numpy/natural_coordinates.py +++ b/bionc/bionc_numpy/natural_coordinates.py @@ -248,229 +248,3 @@ def v(self, segment_idx: int): def vector(self, segment_idx: int): array_idx = np.arange(segment_idx * 12, (segment_idx + 1) * 12) return SegmentNaturalCoordinates(self[array_idx, :].to_array()) - - -class ExternalForce: - """ - This class represents an external force applied to a segment. - - Attributes - ---------- - application_point_in_local : np.ndarray - The application point of the force in the natural coordinate system of the segment - external_forces : np.ndarray - The external force vector in the global coordinate system (torque, force) - - Methods - ------- - from_components(application_point_in_local, force, torque) - This function creates an external force from its components. - force - This function returns the force vector of the external force. - torque - This function returns the torque vector of the external force. - compute_pseudo_interpolation_matrix() - This function computes the pseudo interpolation matrix of the external force. - to_natural_force - This function returns the external force in the natural coordinate format. - """ - - def __init__(self, application_point_in_local: np.ndarray, external_forces: np.ndarray): - self.application_point_in_local = application_point_in_local - self.external_forces = external_forces - - @classmethod - def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): - """ - This function creates an external force from its components. - - Parameters - ---------- - application_point_in_local : np.ndarray - The application point of the force in the natural coordinate system of the segment - force - The force vector in the global coordinate system - torque - The torque vector in the global coordinate system - - Returns - ------- - ExternalForce - """ - - return cls(application_point_in_local, np.concatenate((torque, force))) - - @property - def force(self) -> np.ndarray: - """The force vector in the global coordinate system""" - return self.external_forces[3:6] - - @property - def torque(self) -> np.ndarray: - """The torque vector in the global coordinate system""" - return self.external_forces[0:3] - - @staticmethod - def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates) -> np.ndarray: - """ - Return the force moment transformation matrix - - Parameters - ---------- - Qi : SegmentNaturalCoordinates - The natural coordinates of the segment - - Returns - ------- - np.ndarray - The force moment transformation matrix - """ - # default we apply force at the proximal point - - left_interpolation_matrix = np.zeros((12, 3)) - - left_interpolation_matrix[9:12, 0] = Qi.u - left_interpolation_matrix[0:3, 1] = Qi.v - left_interpolation_matrix[3:6, 2] = -Qi.w - left_interpolation_matrix[6:9, 2] = Qi.w - - # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar - lever_arm_force_matrix = np.zeros((3, 3)) - - lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) - lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) - lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) - - return (left_interpolation_matrix @ np.linalg.inv(lever_arm_force_matrix)).T - - def to_natural_force(self, Qi: SegmentNaturalCoordinates) -> np.ndarray: - """ - Apply external forces to the segment - - Parameters - ---------- - Qi: SegmentNaturalCoordinates - Segment natural coordinates - - Returns - ------- - np.ndarray - The external forces adequately transformed for the equation of motion in natural coordinates - """ - - pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) - point_interpolation_matrix = NaturalVector(self.application_point_in_local).interpolate() - application_point_in_global = np.array(point_interpolation_matrix @ Qi).squeeze() - - fext = point_interpolation_matrix.T @ self.force - fext += pseudo_interpolation_matrix.T @ self.torque - - # Bour's formula to transport the moment from the application point to the proximal point - # fext += pseudo_interpolation_matrix.T @ np.cross(application_point_in_global - Qi.rp, self.force) - - return np.array(fext) - - -class ExternalForceList: - """ - This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. - All segment forces are expressed in natural coordinates to be added to the equation of motion as: - - Q @ Qddot + K^T @ lambda = Weight + f_ext - - Attributes - ---------- - external_forces : list - List of ExternalForces for each segment - - Methods - ------- - add_external_force(segment_index, external_force) - This function adds an external force to the list of external forces. - empty_from_nb_segment(nb_segment) - This function creates an empty ExternalForceList from the number of segments. - to_natural_external_forces(Q) - This function returns the external forces in the natural coordinate format. - segment_external_forces(segment_index) - This function returns the external forces of a segment. - nb_segments - This function returns the number of segments. - - Examples - -------- - >>> from bionc import ExternalForceList, ExternalForce - >>> import numpy as np - >>> f_ext = ExternalForceList.empty_from_nb_segment(2) - >>> segment_force = ExternalForce(force=np.array([0,1,1.1]), torque=np.zeros(3), application_point_in_local=np.array([0,0.5,0])) - >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) - """ - - def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): - if external_forces is None: - raise ValueError( - "f_ext must be a list of ExternalForces, or use the classmethod" - "NaturalExternalForceList.empty_from_nb_segment(nb_segment)" - ) - self.external_forces = external_forces - - @property - def nb_segments(self) -> int: - """Returns the number of segments""" - return len(self.external_forces) - - @classmethod - def empty_from_nb_segment(cls, nb_segment: int): - """ - Create an empty NaturalExternalForceList from the model size - """ - return cls(external_forces=[[] for _ in range(nb_segment)]) - - def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: - """Returns the external forces of the segment""" - return self.external_forces[segment_index] - - def add_external_force(self, segment_index: int, external_force: ExternalForce): - """ - Add an external force to the segment - - Parameters - ---------- - segment_index: int - The index of the segment - external_force: - The external force to add - """ - self.external_forces[segment_index].append(external_force) - - def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: - """ - Converts and sums all the segment natural external forces to the full vector of natural external forces - - Parameters - ---------- - Q : NaturalCoordinates - The natural coordinates of the model - """ - - if len(self.external_forces) != Q.nb_qi(): - raise ValueError( - "The number of segment in the model and the number of segment in the external forces must be the same" - ) - - natural_external_forces = np.zeros((12 * Q.nb_qi(), 1)) - for segment_index, segment_external_forces in enumerate(self.external_forces): - segment_natural_external_forces = np.zeros((12, 1)) - slice_index = slice(segment_index * 12, (segment_index + 1) * 12) - for external_force in segment_external_forces: - segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index))[ - :, np.newaxis - ] - natural_external_forces[slice_index, 0:1] = segment_natural_external_forces - - return natural_external_forces - - def __iter__(self): - return iter(self.external_forces) - - def __len__(self): - return len(self.external_forces) diff --git a/bionc/bionc_numpy/natural_external_forces.py b/bionc/bionc_numpy/natural_external_forces.py deleted file mode 100644 index 839494cb..00000000 --- a/bionc/bionc_numpy/natural_external_forces.py +++ /dev/null @@ -1,460 +0,0 @@ -# import numpy as np -# from bionc import SegmentNaturalCoordinates, NaturalCoordinates -# from bionc.bionc_numpy import NaturalVector - - -# class ExternalForce: -# def __init__(self, -# application_point_in_local: np.ndarray, -# external_forces: np.ndarray): -# self.application_point_in_local = application_point_in_local -# self.external_forces = external_forces -# -# @classmethod -# def from_components(cls, application_point_in_local: np.ndarray, force: np.ndarray, torque: np.ndarray): -# return cls(application_point_in_local, np.concatenate((torque, force))) -# -# @property -# def force(self): -# return self.external_forces[3:6] -# -# @property -# def torque(self): -# return self.external_forces[0:3] -# -# @staticmethod -# def compute_pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): -# """ -# Return the force moment transformation matrix -# """ -# # default we apply force at the proximal point -# -# left_interpolation_matrix = np.zeros((12, 3)) -# -# left_interpolation_matrix[9:12, 0] = Qi.u -# left_interpolation_matrix[0:3, 1] = Qi.v -# left_interpolation_matrix[3:6, 2] = -Qi.w -# left_interpolation_matrix[6:9, 2] = Qi.w -# -# # Matrix of lever arms for forces equivalent to moment at proximal endpoint, denoted Bstar -# lever_arm_force_matrix = np.zeros((3, 3)) -# -# lever_arm_force_matrix[:, 0] = np.cross(Qi.w, Qi.u) -# lever_arm_force_matrix[:, 1] = np.cross(Qi.u, Qi.v) -# lever_arm_force_matrix[:, 2] = np.cross(-Qi.v, Qi.w) -# -# return (left_interpolation_matrix @ lever_arm_force_matrix).T -# -# def to_natural_force(self, Qi: SegmentNaturalCoordinates): -# """ -# Apply external forces to the segment -# -# Parameters -# ---------- -# Qi: SegmentNaturalCoordinates -# Segment natural coordinates -# -# Returns -# ------- -# -# """ -# -# pseudo_interpolation_matrix = self.compute_pseudo_interpolation_matrix(Qi) -# -# fext = NaturalVector.proximal().interpolate().T @ self.force -# fext += pseudo_interpolation_matrix.T @ self.torque -# fext += pseudo_interpolation_matrix.T @ np.cross(self.application_point_in_local - Qi.rp, self.force) -# -# return np.array(fext) -# -# -# class ExternalForceList: -# """ -# This class is made to handle all the external forces of each segment, if none are provided, it will be an empty list. -# All segment forces are expressed in natural coordinates to be added to the equation of motion as: -# -# Q @ Qddot + K^T @ lambda = Weight + f_ext -# -# Attributes -# ---------- -# external_forces : list -# List of ExternalForces -# -# Examples -# -------- -# >>> from bionc import ExternalForceList, ExternalForce -# >>> import numpy as np -# >>> f_ext = ExternalForceList.empty_from_nb_segment(2) -# >>> segment_force = ExternalForce(np.array([0,1,1.1]), np.zeros(12,1)) -# >>> f_ext.add_external_force(segment_index=0, external_force=segment_force) -# """ -# -# def __init__(self, external_forces: list[list[ExternalForce, ...]] = None): -# -# if external_forces is None: -# raise ValueError("f_ext must be a list of ExternalForces, or use the classmethod" -# "NaturalExternalForceList.empty_from_nb_segment(nb_segment)") -# self.external_forces = external_forces -# -# @property -# def nb_segments(self) -> int: -# return len(self.external_forces) -# -# @classmethod -# def empty_from_nb_segment(cls, nb_segment: int): -# """ -# Create an empty NaturalExternalForceList from the model size -# """ -# return cls(external_forces=[[] for _ in range(nb_segment)]) -# -# def segment_external_forces(self, segment_index: int) -> list[ExternalForce]: -# return self.external_forces[segment_index] -# -# def add_external_force(self, segment_index: int, external_force: ExternalForce): -# self.external_forces[segment_index].append(external_force) -# -# def to_natural_external_forces(self, Q: NaturalCoordinates) -> np.ndarray: -# """ -# Converts and sums all the segment natural external forces to the full vector of natural external forces -# -# Parameters -# ---------- -# Q : NaturalCoordinates -# The natural coordinates of the model -# """ -# -# if len(self.external_forces) != Q.nb_qi: -# raise ValueError( -# "The number of segment in the model and the number of segment in the external forces must be the same") -# -# natural_external_forces = np.zeros((12 * Q.nb_qi, 1)) -# for segment_index, segment_external_forces in enumerate(self.external_forces): -# segment_natural_external_forces = np.zeros((12, 1)) -# slice_index = slice(segment_index * 12, (segment_index + 1) * 12) -# for external_force in segment_external_forces: -# segment_natural_external_forces += external_force.to_natural_force(Q.vector(segment_index)) -# natural_external_forces[slice_index] = segment_natural_external_forces -# -# return natural_external_forces -# -# def __iter__(self): -# return iter(self.external_forces) -# -# def __len__(self): -# return len(self.external_forces) - - -# -# -# def cartesian_force_to_natural_force_from_interpolation_matrix(cartesian_force: np.ndarray, interpolation_matrix: InterpolationMatrix): -# """ -# Convert the cartesian force to the natural force -# """ -# if cartesian_force.shape[0] != 3: -# raise ValueError("cartesian_force must be a 3x1 numpy array") -# if cartesian_force.shape.__len__() == 1: -# cartesian_force = cartesian_force[:, np.newaxis] -# -# if isinstance(interpolation_matrix, np.ndarray): -# interpolation_matrix = InterpolationMatrix(interpolation_matrix) -# -# return np.array(interpolation_matrix @ cartesian_force) -# -# def cartesian_force_to_natural_force_from_segment(cartesian_force: np.ndarray, segment_index: int, Q: NaturalCoordinates): -# """ -# Convert the cartesian force to the natural force -# """ -# if cartesian_force.shape[0] != 3: -# raise ValueError("cartesian_force must be a 3x1 numpy array") -# if cartesian_force.shape.__len__() == 1: -# cartesian_force = cartesian_force[:, np.newaxis] -# -# Qi = Q.vector(segment_idx=segment_index) -# # default we apply force at the proximal point -# interpolation_matrix = NaturalVector.proximal().interpolate() -# -# return np.array(segment.interpolation_matrix @ cartesian_force) -# -# def cartesian -# -# -# # There should be several format -# # one in cartesian coordinates -# # one in natural coordinates -# # capitaliser pour créer des efforts generalizee ? -# -# # class SegmentNaturalExternalForce(np.ndarray): -# # """ -# # This class is made to handle External Forces of a Segment in natural coordinates format -# # """ -# # -# # def __new__(cls, input_array: np.ndarray | list | tuple): -# # """ -# # Create a new instance of the class. -# # """ -# # -# # obj = np.asarray(input_array).view(cls) -# # -# # if obj.shape[0] != 12: -# # raise ValueError("input_array must be a 12x1 numpy array") -# # -# # if obj.shape.__len__() == 1: -# # obj = obj[:, np.newaxis] -# # -# # return obj -# # -# # @classmethod -# # def from_cartesian_components( -# # cls, -# # force: np.ndarray | list = None, -# # torque: np.ndarray | list = None, -# # ): -# # """ -# # Constructor of the class from the components of the natural coordinates -# # -# # Parameters -# # ---------- -# # force : np.ndarray | list -# # Force vector in R0 -# # torque : np.ndarray | list -# # Torque vector in R0 -# # # todo : application_point : NaturalVector -# # # Application point in R0 -# # # todo : segment : int -# # # Segment to which the external force is applied -# # # todo : segment_application_point : NaturalVector -# # # Application point in the segment frame -# # -# # Returns -# # ------- -# # ExternalForces -# # """ -# # -# # if force is None: -# # raise ValueError("force must be a numpy array (3x1) or a list of 3 elements") -# # if torque is None: -# # raise ValueError("torque must be a numpy array (3x1) or a list of 3 elements") -# # -# # if not isinstance(force, np.ndarray): -# # force = np.array(force) -# # if not isinstance(torque, np.ndarray): -# # torque = np.array(torque) -# # -# # if force.shape[0] != 3: -# # raise ValueError("force must be a 3x1 numpy array") -# # if torque.shape[0] != 3: -# # raise ValueError("torque must be a 3x1 numpy array") -# # -# # return cls(np.concatenate((force, torque), axis=0)) -# -# class SegmentExternalForce: -# """ -# This class is made to handle External Forces of a Segment in natural coordinates format -# """ -# -# def __init__(self, -# cartesian_force: np.ndarray | list = None, -# cartesian_torque: np.ndarray | list = None, -# segment_index: int = None, -# application_point: NaturalVector = None, -# ): -# self.cartesian_force = cartesian_force -# self.cartesian_torque = cartesian_torque -# self.segment_index = segment_index -# self.application_point = application_point -# -# def to_natural_force(self, Q: NaturalCoordinates): -# """ -# Convert the cartesian force to the natural force -# """ -# interpolation_matrix_proximal_point = NaturalVector.proximal().interpolate() -# force = interpolation_matrix_proximal_point @ self.cartesian_force -# -# -# return np.array(interpolation_matrix @ cartesian_force) -# -# def pseudo_interpolation_matrix(Qi: SegmentNaturalCoordinates): -# """ -# Return the pseudo interpolation matrix -# """ -# pseudo_interpolation_matrix = np.zeros((3,12)) -# -# pseudo_interpolation_matrix[0, 9:12] = Qi.u -# pseudo_interpolation_matrix[1, 0:3] = Qi.v -# pseudo_interpolation_matrix[2, 3:6] = -Qi.w -# pseudo_interpolation_matrix[2, 6:3] = Qi.w -# -# return pseudo_interpolation_matrix -# -# def force_moment_transformation_matrix(Qi: SegmentNaturalCoordinates): -# """ -# Return the force moment transformation matrix -# """ -# # default we apply force at the proximal point -# force_moment_transformation_matrix = np.zeros((3,12)) -# -# force_moment_transformation_matrix[:, 0] = np.cross(Qi.w, Qi.u) -# force_moment_transformation_matrix[:, 1] = np.cross(Qi.u, Qi.v) -# force_moment_transformation_matrix[:, 2] = np.cross(-Qi.v, Qi.w) -# -# return force_moment_transformation_matrix -# -# def apply_external_forces(Qi: SegmentNaturalCoordinates, external_forces: np.ndarray, application_point_in_local:np.ndarray): -# """ -# Apply external forces to the segment -# -# Parameters -# ---------- -# Qi: SegmentNaturalCoordinates -# Segment natural coordinates -# -# external_forces: np.ndarray -# External forces in cartesian coordinates -# -# Returns -# ------- -# -# """ -# torque = external_forces[3:6] -# force = external_forces[0:3] -# -# pseudo_interpolation_matrix = pseudo_interpolation_matrix(Qi) -# force_moment_transformation_matrix = force_moment_transformation_matrix(Qi) -# -# fext = pseudo_interpolation_matrix.T @ force -# fext += force_moment_transformation_matrix.T @ torque -# fext += force_moment_transformation_matrix.T @ np.cross(application_point_in_local - Qi.rp, force) -# -# return fext -# -# -# class NaturalExternalForces(np.ndarray): -# """ -# This class is made to handle External Forces for all segments in natural coordinates format -# """ -# -# def __new__(cls, input_array: np.ndarray): -# """ -# Create a new instance of the class. -# """ -# -# obj = np.asarray(input_array).view(input_array) -# -# if obj.shape.__len__() == 1: -# obj = obj[:, np.newaxis] -# -# if obj.shape[0] % 12 != 0: -# raise ValueError("input_array must be a [12xN, 1] numpy array") -# -# return obj -# -# @classmethod -# def from_segment_natural_forces(cls, segment_natural_forces: list | tuple): -# """ -# Constructor of the class from the components of the natural coordinates -# -# Parameters -# ---------- -# segment_natural_forces : list -# List of segment natural forces -# -# """ -# if not isinstance(segment_natural_forces, tuple | list): -# raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") -# -# for Q in segment_natural_forces: -# if not isinstance(Q, SegmentNaturalExternalForce): -# raise ValueError("tuple_of_Q must be a tuple of SegmentNaturalCoordinates") -# -# input_array = np.concatenate(segment_natural_forces, axis=0) -# return cls(input_array) -# -# def to_array(self): -# return np.array(self).squeeze() -# -# def vector(self, segment_idx: int) -> SegmentNaturalExternalForce: -# """ -# Return the vector of the external forces -# """ -# return SegmentNaturalExternalForce(self[segment_idx * 12 : (segment_idx + 1) * 12, :].to_array()) -# -# # @classmethod -# # def from_components( -# # cls, -# # force: np.ndarray | list = None, -# # torque: np.ndarray | list = None, -# # application_point: NaturalVector = None, -# # segment: int = None, -# # segment_application_point: NaturalVector = None, -# # ): -# # """ -# # Constructor of the class from the components of the natural coordinates -# # -# # Parameters -# # ---------- -# # force : np.ndarray | list -# # Force vector in R0 -# # torque : np.ndarray | list -# # Torque vector in R0 -# # application_point : NaturalVector -# # Application point in R0 -# # segment : int -# # Segment to which the external force is applied -# # segment_application_point : NaturalVector -# # Application point in the segment frame -# # -# # Returns -# # ------- -# # ExternalForces -# # """ -# # -# # if force is None: -# # raise ValueError("force must be a numpy array (3x1) or a list of 3 elements") -# # if torque is None: -# # raise ValueError("torque must be a numpy array (3x1) or a list of 3 elements") -# # -# # if not isinstance(force, np.ndarray): -# # force = np.array(force) -# # if not isinstance(torque, np.ndarray): -# # torque = np.array(torque) -# # -# # if force.shape[0] != 3: -# # raise ValueError("force must be a 3x1 numpy array") -# # if torque.shape[0] != 3: -# # raise ValueError("torque must be a 3x1 numpy array") -# # -# # external_force = -# # -# # return cls(np.concatenate((force, torque), axis=0)) -# -# -# -# -# # -# -# -# -# -# cartesian_fext = CartesianExternalForces.from_world( -# force=[0, 0, 0], -# torque=[0, 0, 0], -# application_point=CartesianVector.from_components([0, 0, 0]), -# ) -# cartesian_fext.in_ -# segment = 1, -# segment_application_point = CartesianVector.from_components([0, 0, 0]), -# -# -# segment_external_force = SegmentNaturalExternalForces.from_cartesian( -# cartesian_fext, -# segment=1, -# segment_application_point: Marker| other| ...= ..., -# Qi=..., -# ) -# -# # doesn't' look comaptible ... :/ -# external_forces = NaturalExternalForces() -# external_forces.add() -# external_forces.element(1) -> SegmentNaturalExternalForces -# external_forces.in_segement(2) -> tuple[SegmentNaturalExternalForces, ...] -# external_forces.to_equations() diff --git a/tests/test_external_force.py b/tests/test_external_force.py index b23dfcf3..aa007aa3 100644 --- a/tests/test_external_force.py +++ b/tests/test_external_force.py @@ -56,7 +56,7 @@ def test_external_force(bionc_type, external_force_tuple): "bionc_type", [ "numpy", - # "casadi", + "casadi", ], ) def test_external_force(bionc_type): @@ -106,8 +106,8 @@ def test_external_force(bionc_type): rd=[0, -1, 0], w=[0, 0, 1], ) - Q1 = Q0 + 0.1 - Q2 = Q1 + 0.1 + Q1 = SegmentNaturalCoordinates(Q0 + 0.1) + Q2 = SegmentNaturalCoordinates(Q1 + 0.1) Q = NaturalCoordinates.from_qi((Q0, Q1, Q2)) pseudo_interpolation_matrix = force2.compute_pseudo_interpolation_matrix(Q2) @@ -162,6 +162,7 @@ def test_external_force(bionc_type): ], ] ), + expand=False, ) TestUtils.assert_equal( @@ -182,9 +183,11 @@ def test_external_force(bionc_type): 0.04612857, ] ), + expand=False, ) TestUtils.assert_equal( + natural_forces, np.array( [ [0.0007], @@ -225,5 +228,6 @@ def test_external_force(bionc_type): [0.04612857], ] ), - natural_forces, + expand=False, + squeeze=False, )