NaN detected for output jac_g_x

OpenSim Moco is a software toolkit to solve optimal control problems with musculoskeletal models defined in OpenSim using the direct collocation method.
POST REPLY
User avatar
Pagnon David
Posts: 82
Joined: Mon Jan 06, 2014 3:13 am

NaN detected for output jac_g_x

Post by Pagnon David » Thu Nov 02, 2023 10:36 am

Hi,

I am trying to run a torque-driven tracking simulation of a walking sequence.
The model is Rajagopal2015.
I do not provide any experimental ground reaction forces, instead I add contact spheres to the model.
I do provide a .mot file with joint angles.

When I run it, I am welcomed with a CasADi error, which tells me NaN detected for output jac_g_x. The "differential equation defects" output show that the L2 norm across mesh and the max abs value (L1 norm) are equal to -nan for every joint speed at the first frame.
However, the speeds in my tracked_states.sto file seem to be reasonable.
Same for the last two kinematic constraints, having nana values. (Why are there 5? I should only have 2 of them).


Full error message:

Code: Select all

[info] ========================================================================
[info] MocoCasADiSolver starting.
[info] Thu Nov  2 17:47:54 2023
[info] ------------------------------------------------------------------------
Costs: (total: 2)
  state_tracking. MocoStateTrackingGoal, enabled: true, mode: cost, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tilt/value, weight: 10.0
        state: /jointset/ground_pelvis/pelvis_list/value, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_rotation/value, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tx/value, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_ty/value, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tz/value, weight: 1.0
        state: /jointset/hip_r/hip_flexion_r/value, weight: 10.0
        state: /jointset/hip_r/hip_adduction_r/value, weight: 1.0
        state: /jointset/hip_r/hip_rotation_r/value, weight: 1.0
        state: /jointset/walker_knee_r/knee_angle_r/value, weight: 10.0
        state: /jointset/patellofemoral_r/knee_angle_r_beta/value, weight: 0.0
        state: /jointset/ankle_r/ankle_angle_r/value, weight: 10.0
        state: /jointset/hip_l/hip_flexion_l/value, weight: 10.0
        state: /jointset/hip_l/hip_adduction_l/value, weight: 1.0
        state: /jointset/hip_l/hip_rotation_l/value, weight: 1.0
        state: /jointset/walker_knee_l/knee_angle_l/value, weight: 10.0
        state: /jointset/patellofemoral_l/knee_angle_l_beta/value, weight: 0.0
        state: /jointset/ankle_l/ankle_angle_l/value, weight: 10.0
        state: /jointset/back/lumbar_extension/value, weight: 10.0
        state: /jointset/back/lumbar_bending/value, weight: 1.0
        state: /jointset/back/lumbar_rotation/value, weight: 1.0
        state: /jointset/acromial_r/arm_flex_r/value, weight: 1.0
        state: /jointset/acromial_r/arm_add_r/value, weight: 1.0
        state: /jointset/acromial_r/arm_rot_r/value, weight: 1.0
        state: /jointset/elbow_r/elbow_flex_r/value, weight: 1.0
        state: /jointset/radioulnar_r/pro_sup_r/value, weight: 1.0
        state: /jointset/acromial_l/arm_flex_l/value, weight: 1.0
        state: /jointset/acromial_l/arm_add_l/value, weight: 1.0
        state: /jointset/acromial_l/arm_rot_l/value, weight: 1.0
        state: /jointset/elbow_l/elbow_flex_l/value, weight: 1.0
        state: /jointset/radioulnar_l/pro_sup_l/value, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tilt/speed, weight: 10.0
        state: /jointset/ground_pelvis/pelvis_list/speed, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_rotation/speed, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tx/speed, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_ty/speed, weight: 1.0
        state: /jointset/ground_pelvis/pelvis_tz/speed, weight: 1.0
        state: /jointset/hip_r/hip_flexion_r/speed, weight: 10.0
        state: /jointset/hip_r/hip_adduction_r/speed, weight: 1.0
        state: /jointset/hip_r/hip_rotation_r/speed, weight: 1.0
        state: /jointset/walker_knee_r/knee_angle_r/speed, weight: 10.0
        state: /jointset/patellofemoral_r/knee_angle_r_beta/speed, weight: 0.0
        state: /jointset/ankle_r/ankle_angle_r/speed, weight: 10.0
        state: /jointset/hip_l/hip_flexion_l/speed, weight: 10.0
        state: /jointset/hip_l/hip_adduction_l/speed, weight: 1.0
        state: /jointset/hip_l/hip_rotation_l/speed, weight: 1.0
        state: /jointset/walker_knee_l/knee_angle_l/speed, weight: 10.0
        state: /jointset/patellofemoral_l/knee_angle_l_beta/speed, weight: 0.0
        state: /jointset/ankle_l/ankle_angle_l/speed, weight: 10.0
        state: /jointset/back/lumbar_extension/speed, weight: 10.0
        state: /jointset/back/lumbar_bending/speed, weight: 1.0
        state: /jointset/back/lumbar_rotation/speed, weight: 1.0
        state: /jointset/acromial_r/arm_flex_r/speed, weight: 1.0
        state: /jointset/acromial_r/arm_add_r/speed, weight: 1.0
        state: /jointset/acromial_r/arm_rot_r/speed, weight: 1.0
        state: /jointset/elbow_r/elbow_flex_r/speed, weight: 1.0
        state: /jointset/radioulnar_r/pro_sup_r/speed, weight: 1.0
        state: /jointset/acromial_l/arm_flex_l/speed, weight: 1.0
        state: /jointset/acromial_l/arm_add_l/speed, weight: 1.0
        state: /jointset/acromial_l/arm_rot_l/speed, weight: 1.0
        state: /jointset/elbow_l/elbow_flex_l/speed, weight: 1.0
        state: /jointset/radioulnar_l/pro_sup_l/speed, weight: 1.0
  control_effort. MocoControlGoal, enabled: true, mode: cost, weight: 0.1
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_tilt, weight: 1.0
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_list, weight: 1.0
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_rotation, weight: 1.0
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_tx, weight: 1.0
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_ty, weight: 1.0
        control: /forceset/reserve_jointset_ground_pelvis_pelvis_tz, weight: 1.0
        control: /forceset/reserve_jointset_hip_r_hip_flexion_r, weight: 1.0
        control: /forceset/reserve_jointset_hip_r_hip_adduction_r, weight: 1.0
        control: /forceset/reserve_jointset_hip_r_hip_rotation_r, weight: 1.0
        control: /forceset/reserve_jointset_walker_knee_r_knee_angle_r, weight: 1.0
        control: /forceset/reserve_jointset_ankle_r_ankle_angle_r, weight: 1.0
        control: /forceset/reserve_jointset_hip_l_hip_flexion_l, weight: 1.0
        control: /forceset/reserve_jointset_hip_l_hip_adduction_l, weight: 1.0
        control: /forceset/reserve_jointset_hip_l_hip_rotation_l, weight: 1.0
        control: /forceset/reserve_jointset_walker_knee_l_knee_angle_l, weight: 1.0
        control: /forceset/reserve_jointset_ankle_l_ankle_angle_l, weight: 1.0
        control: /forceset/reserve_jointset_back_lumbar_extension, weight: 1.0
        control: /forceset/reserve_jointset_back_lumbar_bending, weight: 1.0
        control: /forceset/reserve_jointset_back_lumbar_rotation, weight: 1.0
        control: /forceset/reserve_jointset_acromial_r_arm_flex_r, weight: 1.0
        control: /forceset/reserve_jointset_acromial_r_arm_add_r, weight: 1.0
        control: /forceset/reserve_jointset_acromial_r_arm_rot_r, weight: 1.0
        control: /forceset/reserve_jointset_elbow_r_elbow_flex_r, weight: 1.0
        control: /forceset/reserve_jointset_radioulnar_r_pro_sup_r, weight: 1.0
        control: /forceset/reserve_jointset_acromial_l_arm_flex_l, weight: 1.0
        control: /forceset/reserve_jointset_acromial_l_arm_add_l, weight: 1.0
        control: /forceset/reserve_jointset_acromial_l_arm_rot_l, weight: 1.0
        control: /forceset/reserve_jointset_elbow_l_elbow_flex_l, weight: 1.0
        control: /forceset/reserve_jointset_radioulnar_l_pro_sup_l, weight: 1.0
Endpoint constraints: (total: 1)
  periodicityGoal. MocoPeriodicityGoal, enabled: true, mode: endpoint_constraint
        state periodicity pairs:
                initial: /jointset/ground_pelvis/pelvis_tilt/value, final: /jointset/ground_pelvis/pelvis_tilt/value
                initial: /jointset/ground_pelvis/pelvis_tilt/speed, final: /jointset/ground_pelvis/pelvis_tilt/speed
                initial: /jointset/ground_pelvis/pelvis_list/value, final: /jointset/ground_pelvis/pelvis_list/value
                initial: /jointset/ground_pelvis/pelvis_list/speed, final: /jointset/ground_pelvis/pelvis_list/speed
                initial: /jointset/ground_pelvis/pelvis_rotation/value, final: /jointset/ground_pelvis/pelvis_rotation/value
                initial: /jointset/ground_pelvis/pelvis_rotation/speed, final: /jointset/ground_pelvis/pelvis_rotation/speed
                initial: /jointset/ground_pelvis/pelvis_tx/speed, final: /jointset/ground_pelvis/pelvis_tx/speed
                initial: /jointset/ground_pelvis/pelvis_ty/value, final: /jointset/ground_pelvis/pelvis_ty/value
                initial: /jointset/ground_pelvis/pelvis_ty/speed, final: /jointset/ground_pelvis/pelvis_ty/speed
                initial: /jointset/ground_pelvis/pelvis_tz/value, final: /jointset/ground_pelvis/pelvis_tz/value
                initial: /jointset/ground_pelvis/pelvis_tz/speed, final: /jointset/ground_pelvis/pelvis_tz/speed
                initial: /jointset/hip_r/hip_flexion_r/value, final: /jointset/hip_r/hip_flexion_r/value
                initial: /jointset/hip_r/hip_flexion_r/speed, final: /jointset/hip_r/hip_flexion_r/speed
                initial: /jointset/hip_r/hip_adduction_r/value, final: /jointset/hip_r/hip_adduction_r/value
                initial: /jointset/hip_r/hip_adduction_r/speed, final: /jointset/hip_r/hip_adduction_r/speed
                initial: /jointset/hip_r/hip_rotation_r/value, final: /jointset/hip_r/hip_rotation_r/value
                initial: /jointset/hip_r/hip_rotation_r/speed, final: /jointset/hip_r/hip_rotation_r/speed
                initial: /jointset/walker_knee_r/knee_angle_r/value, final: /jointset/walker_knee_r/knee_angle_r/value
                initial: /jointset/walker_knee_r/knee_angle_r/speed, final: /jointset/walker_knee_r/knee_angle_r/speed
                initial: /jointset/patellofemoral_r/knee_angle_r_beta/value, final: /jointset/patellofemoral_r/knee_angle_r_beta/value
                initial: /jointset/patellofemoral_r/knee_angle_r_beta/speed, final: /jointset/patellofemoral_r/knee_angle_r_beta/speed
                initial: /jointset/ankle_r/ankle_angle_r/value, final: /jointset/ankle_r/ankle_angle_r/value
                initial: /jointset/ankle_r/ankle_angle_r/speed, final: /jointset/ankle_r/ankle_angle_r/speed
                initial: /jointset/hip_l/hip_flexion_l/value, final: /jointset/hip_l/hip_flexion_l/value
                initial: /jointset/hip_l/hip_flexion_l/speed, final: /jointset/hip_l/hip_flexion_l/speed
                initial: /jointset/hip_l/hip_adduction_l/value, final: /jointset/hip_l/hip_adduction_l/value
                initial: /jointset/hip_l/hip_adduction_l/speed, final: /jointset/hip_l/hip_adduction_l/speed
                initial: /jointset/hip_l/hip_rotation_l/value, final: /jointset/hip_l/hip_rotation_l/value
                initial: /jointset/hip_l/hip_rotation_l/speed, final: /jointset/hip_l/hip_rotation_l/speed
                initial: /jointset/walker_knee_l/knee_angle_l/value, final: /jointset/walker_knee_l/knee_angle_l/value
                initial: /jointset/walker_knee_l/knee_angle_l/speed, final: /jointset/walker_knee_l/knee_angle_l/speed
                initial: /jointset/patellofemoral_l/knee_angle_l_beta/value, final: /jointset/patellofemoral_l/knee_angle_l_beta/value
                initial: /jointset/patellofemoral_l/knee_angle_l_beta/speed, final: /jointset/patellofemoral_l/knee_angle_l_beta/speed
                initial: /jointset/ankle_l/ankle_angle_l/value, final: /jointset/ankle_l/ankle_angle_l/value
                initial: /jointset/ankle_l/ankle_angle_l/speed, final: /jointset/ankle_l/ankle_angle_l/speed
                initial: /jointset/back/lumbar_extension/value, final: /jointset/back/lumbar_extension/value
                initial: /jointset/back/lumbar_extension/speed, final: /jointset/back/lumbar_extension/speed
                initial: /jointset/back/lumbar_bending/value, final: /jointset/back/lumbar_bending/value
                initial: /jointset/back/lumbar_bending/speed, final: /jointset/back/lumbar_bending/speed
                initial: /jointset/back/lumbar_rotation/value, final: /jointset/back/lumbar_rotation/value
                initial: /jointset/back/lumbar_rotation/speed, final: /jointset/back/lumbar_rotation/speed
                initial: /jointset/acromial_r/arm_flex_r/value, final: /jointset/acromial_r/arm_flex_r/value
                initial: /jointset/acromial_r/arm_flex_r/speed, final: /jointset/acromial_r/arm_flex_r/speed
                initial: /jointset/acromial_r/arm_add_r/value, final: /jointset/acromial_r/arm_add_r/value
                initial: /jointset/acromial_r/arm_add_r/speed, final: /jointset/acromial_r/arm_add_r/speed
                initial: /jointset/acromial_r/arm_rot_r/value, final: /jointset/acromial_r/arm_rot_r/value
                initial: /jointset/acromial_r/arm_rot_r/speed, final: /jointset/acromial_r/arm_rot_r/speed
                initial: /jointset/elbow_r/elbow_flex_r/value, final: /jointset/elbow_r/elbow_flex_r/value
                initial: /jointset/elbow_r/elbow_flex_r/speed, final: /jointset/elbow_r/elbow_flex_r/speed
                initial: /jointset/radioulnar_r/pro_sup_r/value, final: /jointset/radioulnar_r/pro_sup_r/value
                initial: /jointset/radioulnar_r/pro_sup_r/speed, final: /jointset/radioulnar_r/pro_sup_r/speed
                initial: /jointset/acromial_l/arm_flex_l/value, final: /jointset/acromial_l/arm_flex_l/value
                initial: /jointset/acromial_l/arm_flex_l/speed, final: /jointset/acromial_l/arm_flex_l/speed
                initial: /jointset/acromial_l/arm_add_l/value, final: /jointset/acromial_l/arm_add_l/value
                initial: /jointset/acromial_l/arm_add_l/speed, final: /jointset/acromial_l/arm_add_l/speed
                initial: /jointset/acromial_l/arm_rot_l/value, final: /jointset/acromial_l/arm_rot_l/value
                initial: /jointset/acromial_l/arm_rot_l/speed, final: /jointset/acromial_l/arm_rot_l/speed
                initial: /jointset/elbow_l/elbow_flex_l/value, final: /jointset/elbow_l/elbow_flex_l/value
                initial: /jointset/elbow_l/elbow_flex_l/speed, final: /jointset/elbow_l/elbow_flex_l/speed
                initial: /jointset/radioulnar_l/pro_sup_l/value, final: /jointset/radioulnar_l/pro_sup_l/value
                initial: /jointset/radioulnar_l/pro_sup_l/speed, final: /jointset/radioulnar_l/pro_sup_l/speed
        control periodicity pairs:
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_tilt, final: /forceset/reserve_jointset_ground_pelvis_pelvis_tilt
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_list, final: /forceset/reserve_jointset_ground_pelvis_pelvis_list
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_rotation, final: /forceset/reserve_jointset_ground_pelvis_pelvis_rotation
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_tx, final: /forceset/reserve_jointset_ground_pelvis_pelvis_tx
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_ty, final: /forceset/reserve_jointset_ground_pelvis_pelvis_ty
                initial: /forceset/reserve_jointset_ground_pelvis_pelvis_tz, final: /forceset/reserve_jointset_ground_pelvis_pelvis_tz
                initial: /forceset/reserve_jointset_hip_r_hip_flexion_r, final: /forceset/reserve_jointset_hip_r_hip_flexion_r
                initial: /forceset/reserve_jointset_hip_r_hip_adduction_r, final: /forceset/reserve_jointset_hip_r_hip_adduction_r
                initial: /forceset/reserve_jointset_hip_r_hip_rotation_r, final: /forceset/reserve_jointset_hip_r_hip_rotation_r
                initial: /forceset/reserve_jointset_walker_knee_r_knee_angle_r, final: /forceset/reserve_jointset_walker_knee_r_knee_angle_r
                initial: /forceset/reserve_jointset_ankle_r_ankle_angle_r, final: /forceset/reserve_jointset_ankle_r_ankle_angle_r
                initial: /forceset/reserve_jointset_hip_l_hip_flexion_l, final: /forceset/reserve_jointset_hip_l_hip_flexion_l
                initial: /forceset/reserve_jointset_hip_l_hip_adduction_l, final: /forceset/reserve_jointset_hip_l_hip_adduction_l
                initial: /forceset/reserve_jointset_hip_l_hip_rotation_l, final: /forceset/reserve_jointset_hip_l_hip_rotation_l
                initial: /forceset/reserve_jointset_walker_knee_l_knee_angle_l, final: /forceset/reserve_jointset_walker_knee_l_knee_angle_l
                initial: /forceset/reserve_jointset_ankle_l_ankle_angle_l, final: /forceset/reserve_jointset_ankle_l_ankle_angle_l
                initial: /forceset/reserve_jointset_back_lumbar_extension, final: /forceset/reserve_jointset_back_lumbar_extension
                initial: /forceset/reserve_jointset_back_lumbar_bending, final: /forceset/reserve_jointset_back_lumbar_bending
                initial: /forceset/reserve_jointset_back_lumbar_rotation, final: /forceset/reserve_jointset_back_lumbar_rotation
                initial: /forceset/reserve_jointset_acromial_r_arm_flex_r, final: /forceset/reserve_jointset_acromial_r_arm_flex_r
                initial: /forceset/reserve_jointset_acromial_r_arm_add_r, final: /forceset/reserve_jointset_acromial_r_arm_add_r
                initial: /forceset/reserve_jointset_acromial_r_arm_rot_r, final: /forceset/reserve_jointset_acromial_r_arm_rot_r
                initial: /forceset/reserve_jointset_elbow_r_elbow_flex_r, final: /forceset/reserve_jointset_elbow_r_elbow_flex_r
                initial: /forceset/reserve_jointset_radioulnar_r_pro_sup_r, final: /forceset/reserve_jointset_radioulnar_r_pro_sup_r
                initial: /forceset/reserve_jointset_acromial_l_arm_flex_l, final: /forceset/reserve_jointset_acromial_l_arm_flex_l
                initial: /forceset/reserve_jointset_acromial_l_arm_add_l, final: /forceset/reserve_jointset_acromial_l_arm_add_l
                initial: /forceset/reserve_jointset_acromial_l_arm_rot_l, final: /forceset/reserve_jointset_acromial_l_arm_rot_l
                initial: /forceset/reserve_jointset_elbow_l_elbow_flex_l, final: /forceset/reserve_jointset_elbow_l_elbow_flex_l
                initial: /forceset/reserve_jointset_radioulnar_l_pro_sup_l, final: /forceset/reserve_jointset_radioulnar_l_pro_sup_l
Kinematic constraints: (total: 2)
  kinematic_constraint_cid31. MocoConstraintInfo. number of scalar equations: 3. bounds: 0, 0, 0
  kinematic_constraint_cid32. MocoConstraintInfo. number of scalar equations: 3. bounds: 0, 0, 0
Path constraints: none
States: (total: 62)
  /jointset/elbow_r/elbow_flex_r/value. bounds: [0.464956, 0.71639]
  /jointset/ankle_l/ankle_angle_l/value. bounds: [-0.306115, 0.451045]
  /jointset/ground_pelvis/pelvis_ty/value. bounds: [1.11955, 1.18149]
  /jointset/ground_pelvis/pelvis_tilt/value. bounds: [0.092006, 0.209357]
  /jointset/radioulnar_l/pro_sup_l/speed. bounds: [-1.10741, 1.03605]
  /jointset/ground_pelvis/pelvis_tx/value. bounds: [-0.118882, 1.59347]
  /jointset/ground_pelvis/pelvis_list/value. bounds: [-0.0805271, 0.00846509]
  /jointset/radioulnar_r/pro_sup_r/value. bounds: [1.01911, 1.37975]
  /jointset/walker_knee_r/knee_angle_r/value. bounds: [-0.10704, 1.20856]
  /jointset/ground_pelvis/pelvis_rotation/value. bounds: [-0.0378854, 0.0892274]
  /jointset/hip_l/hip_rotation_l/value. bounds: [-0.0784135, 0.0698596]
  /jointset/ground_pelvis/pelvis_tz/value. bounds: [-0.226631, -0.192007]
  /jointset/hip_r/hip_adduction_r/speed. bounds: [-0.721713, 0.643402]
  /jointset/ankle_r/ankle_angle_r/value. bounds: [-0.326511, 0.295517]
  /jointset/hip_r/hip_flexion_r/value. bounds: [-0.529838, 0.420977]
  /jointset/acromial_r/arm_flex_r/speed. bounds: [-1.24287, 1.24431]
  /jointset/hip_r/hip_adduction_r/value. bounds: [0.0156354, 0.152637]
  /jointset/back/lumbar_rotation/speed. bounds: [-0.507833, 0.332529]
  /jointset/hip_r/hip_rotation_r/value. bounds: [-0.112406, 0.139114]
  /jointset/patellofemoral_r/knee_angle_r_beta/value. bounds: [-0.10704, 1.20856]
  /jointset/back/lumbar_extension/speed. bounds: [-0.877735, 0.943441]
  /jointset/hip_l/hip_flexion_l/value. bounds: [-0.498266, 0.420109]
  /jointset/hip_l/hip_adduction_l/value. bounds: [-0.119125, 0.0903451]
  /jointset/patellofemoral_l/knee_angle_l_beta/value. bounds: [-0.112397, 1.30559]
  /jointset/walker_knee_l/knee_angle_l/value. bounds: [-0.112397, 1.30559]
  /jointset/back/lumbar_extension/value. bounds: [-0.422765, -0.277762]
  /jointset/hip_l/hip_flexion_l/speed. bounds: [-2.21982, 4.17921]
  /jointset/acromial_l/arm_rot_l/value. bounds: [-0.254088, 0.0387493]
  /jointset/back/lumbar_bending/value. bounds: [-0.0182619, 0.175578]
  /jointset/radioulnar_r/pro_sup_r/speed. bounds: [-1.40283, 1.50569]
  /jointset/walker_knee_r/knee_angle_r/speed. bounds: [-6.04487, 7.05972]
  /jointset/ground_pelvis/pelvis_rotation/speed. bounds: [-0.608971, 0.698332]
  /jointset/back/lumbar_rotation/value. bounds: [-0.127879, 0.00695324]
  /jointset/acromial_r/arm_flex_r/value. bounds: [-0.19792, 0.0645823]
  /jointset/acromial_l/arm_flex_l/value. bounds: [-0.148341, 0.01429]
  /jointset/acromial_r/arm_add_r/value. bounds: [-0.225864, -0.155167]
  /jointset/elbow_l/elbow_flex_l/speed. bounds: [-1.09022, 1.1126]
  /jointset/acromial_l/arm_add_l/speed. bounds: [-0.39668, 0.418597]
  /jointset/acromial_r/arm_rot_r/value. bounds: [-0.13045, 0.0913855]
  /jointset/elbow_l/elbow_flex_l/value. bounds: [0.440103, 0.668468]
  /jointset/acromial_l/arm_add_l/value. bounds: [-0.242784, -0.144686]
  /jointset/radioulnar_l/pro_sup_l/value. bounds: [1.12366, 1.31522]
  /jointset/ground_pelvis/pelvis_tilt/speed. bounds: [-0.824047, 0.742108]
  /jointset/ground_pelvis/pelvis_list/speed. bounds: [-0.561121, 0.401743]
  /jointset/ground_pelvis/pelvis_tx/speed. bounds: [0.706459, 1.50205]
  /jointset/elbow_r/elbow_flex_r/speed. bounds: [-1.87524, 2.36461]
  /jointset/ankle_l/ankle_angle_l/speed. bounds: [-5.29542, 3.36384]
  /jointset/ground_pelvis/pelvis_ty/speed. bounds: [-0.27702, 0.392014]
  /jointset/hip_l/hip_rotation_l/speed. bounds: [-0.854917, 1.26085]
  /jointset/ground_pelvis/pelvis_tz/speed. bounds: [-0.173938, 0.200676]
  /jointset/ankle_r/ankle_angle_r/speed. bounds: [-4.68474, 2.83541]
  /jointset/hip_r/hip_flexion_r/speed. bounds: [-2.74046, 3.81117]
  /jointset/hip_r/hip_rotation_r/speed. bounds: [-0.726281, 0.938878]
  /jointset/patellofemoral_r/knee_angle_r_beta/speed. bounds: [-6.04487, 7.05972]
  /jointset/hip_l/hip_adduction_l/speed. bounds: [-0.903177, 0.872155]
  /jointset/patellofemoral_l/knee_angle_l_beta/speed. bounds: [-6.71555, 7.40838]
  /jointset/walker_knee_l/knee_angle_l/speed. bounds: [-6.71555, 7.40838]
  /jointset/acromial_l/arm_rot_l/speed. bounds: [-0.977273, 0.962702]
  /jointset/back/lumbar_bending/speed. bounds: [-0.71354, 0.639823]
  /jointset/acromial_l/arm_flex_l/speed. bounds: [-0.622638, 0.718523]
  /jointset/acromial_r/arm_add_r/speed. bounds: [-0.388541, 0.321832]
  /jointset/acromial_r/arm_rot_r/speed. bounds: [-1.58841, 1.29845]
Controls: (total: 29)
  /forceset/reserve_jointset_ground_pelvis_pelvis_ty. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_r_hip_flexion_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_l_arm_rot_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_ground_pelvis_pelvis_tilt. bounds: [-inf, inf]
  /forceset/reserve_jointset_radioulnar_l_pro_sup_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_ground_pelvis_pelvis_tx. bounds: [-inf, inf]
  /forceset/reserve_jointset_ground_pelvis_pelvis_list. bounds: [-inf, inf]
  /forceset/reserve_jointset_ground_pelvis_pelvis_tz. bounds: [-inf, inf]
  /forceset/reserve_jointset_ground_pelvis_pelvis_rotation. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_r_arm_add_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_r_hip_adduction_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_r_hip_rotation_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_walker_knee_r_knee_angle_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_ankle_r_ankle_angle_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_l_hip_flexion_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_l_hip_adduction_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_walker_knee_l_knee_angle_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_hip_l_hip_rotation_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_ankle_l_ankle_angle_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_back_lumbar_extension. bounds: [-inf, inf]
  /forceset/reserve_jointset_back_lumbar_bending. bounds: [-inf, inf]
  /forceset/reserve_jointset_back_lumbar_rotation. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_r_arm_flex_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_r_arm_rot_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_elbow_r_elbow_flex_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_radioulnar_r_pro_sup_r. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_l_arm_flex_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_acromial_l_arm_add_l. bounds: [-inf, inf]
  /forceset/reserve_jointset_elbow_l_elbow_flex_l. bounds: [-inf, inf]
Parameters: none
[info] Number of threads: 12

List of user-set options:

                                    Name   Value                used
                acceptable_compl_inf_tol = 0.01                  yes
              acceptable_constr_viol_tol = 0.01                  yes
                 acceptable_dual_inf_tol = 0.01                  yes
                          acceptable_tol = 0.01                  yes
                           compl_inf_tol = 0.01                  yes
                         constr_viol_tol = 0.01                  yes
                            dual_inf_tol = 0.01                  yes
                   hessian_approximation = limited-memory        yes
                      print_user_options = yes                   yes
                                     tol = 0.01                  yes
This is Ipopt version 3.12.8, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:   503210
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:        0

CasADi - 2023-11-02 17:47:58 WARNING("nlp:nlp_jac_g failed: NaN detected for output jac_g_x, at nonzero index 94 (row 94, col 0).") [D:\dev\tools\anaconda3\conda-bld\opensim-moco_1689351229736\work\dependencies\casadi\casadi\core\oracle_function.cpp:265]
Error evaluating Jacobian of equality constraints at user provided starting point.
  No scaling factors for equality constraints computed!
CasADi - 2023-11-02 17:47:59 WARNING("nlp:nlp_jac_g failed: NaN detected for output jac_g_x, at nonzero index 94 (row 94, col 0).") [D:\dev\tools\anaconda3\conda-bld\opensim-moco_1689351229736\work\dependencies\casadi\casadi\core\oracle_function.cpp:265]

Number of Iterations....: 0

Number of objective function evaluations             = 0
Number of objective gradient evaluations             = 0
Number of equality constraint evaluations            = 0
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 1
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 0
Total CPU secs in IPOPT (w/o function evaluations)   =      1.237
Total CPU secs in NLP function evaluations           =      0.997

EXIT: Invalid number in NLP function or derivative detected.
         nlp  :   t_proc      (avg)   t_wall      (avg)    n_eval
  nlp_grad_f  | 381.00ms (381.00ms) 380.73ms (380.73ms)         1
   nlp_jac_g  |   1.85 s (923.50ms)   1.85 s (923.59ms)         2
       total  |   2.24 s (  2.24 s)   2.24 s (  2.24 s)         1

Breakdown of objective (including weights):
  state_tracking: 693.475
  control_effort: 0

Active or violated continuous variable bounds
L and U indicate which bound is active; '*' indicates a bound is violated.
The case of lower==upper==value is ignored.

State bounds: no bounds active or violated

Control bounds: no bounds active or violated

Multiplier bounds: no bounds active or violated

Derivative bounds: no bounds active or violated

Active or violated parameter bounds
L and U indicate which bound is active; '*' indicates a bound is violated.
The case of lower==upper==value is ignored.

Time bounds: no bounds active or violated

Parameter bounds: no bounds active or violated

Total number of constraints: 4548.

Differential equation defects:
  L2 norm across mesh, max abs value (L1 norm), time of max abs
/jointset/ground_pelvis/pelvis_tilt/value        2.56e-02       1.05e-02       0.587500
/jointset/ground_pelvis/pelvis_list/value        1.73e-02       5.44e-03       0.710714
/jointset/ground_pelvis/pelvis_rotation/value        1.36e-02       7.58e-03       0.562857
/jointset/ground_pelvis/pelvis_tx/value        2.10e-02       5.01e-03       0.390357
/jointset/ground_pelvis/pelvis_ty/value        1.08e-02       4.22e-03       0.587500
/jointset/ground_pelvis/pelvis_tz/value        6.13e-03       3.36e-03       0.612143
/jointset/hip_r/hip_flexion_r/value        6.04e-02       2.86e-02       0.193214
/jointset/hip_r/hip_adduction_r/value        2.16e-02       9.91e-03       0.070000
/jointset/hip_r/hip_rotation_r/value        2.72e-02       1.35e-02       0.488929
/jointset/hip_l/hip_flexion_l/value        5.53e-02       2.10e-02       0.562857
/jointset/hip_l/hip_adduction_l/value        2.11e-02       8.08e-03       0.217857
/jointset/hip_l/hip_rotation_l/value        3.84e-02       1.40e-02       0.119286
/jointset/back/lumbar_extension/value        3.48e-02       1.21e-02       0.562857
/jointset/back/lumbar_bending/value        2.21e-02       9.36e-03       0.094643
/jointset/back/lumbar_rotation/value        1.69e-02       8.56e-03       0.094643
/jointset/walker_knee_r/knee_angle_r/value        1.55e-01       4.89e-02       0.439643
/jointset/patellofemoral_r/knee_angle_r_beta/value        1.55e-01       4.89e-02       0.439643
/jointset/walker_knee_l/knee_angle_l/value        1.55e-01       5.18e-02       0.587500
/jointset/patellofemoral_l/knee_angle_l_beta/value        1.55e-01       5.18e-02       0.587500
/jointset/acromial_r/arm_flex_r/value        3.67e-02       1.88e-02       0.686071
/jointset/acromial_r/arm_add_r/value        1.52e-02       6.00e-03       0.710714
/jointset/acromial_r/arm_rot_r/value        4.37e-02       2.05e-02       0.439643
/jointset/acromial_l/arm_flex_l/value        2.31e-02       1.63e-02       0.686071
/jointset/acromial_l/arm_add_l/value        1.05e-02       4.77e-03       0.439643
/jointset/acromial_l/arm_rot_l/value        2.28e-02       1.26e-02       0.143929
/jointset/ankle_r/ankle_angle_r/value        6.54e-02       2.47e-02       0.365714
/jointset/ankle_l/ankle_angle_l/value        1.65e-01       9.33e-02       0.217857
/jointset/elbow_r/elbow_flex_r/value        5.91e-02       2.87e-02       0.464286
/jointset/elbow_l/elbow_flex_l/value        3.95e-02       2.63e-02       0.217857
/jointset/radioulnar_r/pro_sup_r/value        3.90e-02       1.19e-02       0.168571
/jointset/radioulnar_l/pro_sup_l/value        2.35e-02       8.12e-03       0.513571
/jointset/ground_pelvis/pelvis_tilt/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ground_pelvis/pelvis_list/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ground_pelvis/pelvis_rotation/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ground_pelvis/pelvis_tx/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ground_pelvis/pelvis_ty/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ground_pelvis/pelvis_tz/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_r/hip_flexion_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_r/hip_adduction_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_r/hip_rotation_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_l/hip_flexion_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_l/hip_adduction_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/hip_l/hip_rotation_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/back/lumbar_extension/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/back/lumbar_bending/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/back/lumbar_rotation/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/walker_knee_r/knee_angle_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/patellofemoral_r/knee_angle_r_beta/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/walker_knee_l/knee_angle_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/patellofemoral_l/knee_angle_l_beta/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_r/arm_flex_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_r/arm_add_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_r/arm_rot_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_l/arm_flex_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_l/arm_add_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/acromial_l/arm_rot_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ankle_r/ankle_angle_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/ankle_l/ankle_angle_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/elbow_r/elbow_flex_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/elbow_l/elbow_flex_l/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/radioulnar_r/pro_sup_r/speed       -nan(ind)       -nan(ind)       0.070000
/jointset/radioulnar_l/pro_sup_l/speed       -nan(ind)       -nan(ind)       0.070000

Kinematic constraints:
  L2 norm across mesh, max abs value (L1 norm), time of max abs
00:  cid31_p0        3.51e-08       2.40e-08       0.070000
01:  cid32_p0        3.03e-08       2.75e-09       0.587500
02: cid31_p0d        2.18e-07       1.20e-07       0.070000
03: cid32_p0d        2.39e-07       8.89e-08       0.587500
04:cid31_p0dd       -nan(ind)       -nan(ind)       0.070000
05:cid32_p0dd       -nan(ind)       -nan(ind)       0.070000
Kinematic constraint values at each mesh point:
      time          0          1          2          3          4          5
000   0.070000   2.40e-08  -2.95e-08   1.20e-07  -1.84e-07  -nan(ind)  -nan(ind)
001   9.46e-02   6.30e-09   1.62e-09  -8.66e-09   1.30e-08  -nan(ind)  -nan(ind)
002   1.19e-01   1.07e-08  -1.24e-09  -1.73e-08   3.15e-08  -nan(ind)  -nan(ind)
003   1.44e-01   8.08e-09  -5.86e-10  -9.64e-09   9.13e-10  -nan(ind)  -nan(ind)
004   1.69e-01   8.48e-09  -6.15e-10   1.98e-08  -2.78e-08  -nan(ind)  -nan(ind)
005   1.93e-01   8.39e-09  -2.01e-10   1.37e-09   2.90e-08  -nan(ind)  -nan(ind)
006   2.18e-01   9.75e-09  -2.01e-10   1.60e-10  -3.38e-08  -nan(ind)  -nan(ind)
007   2.42e-01   4.86e-09  -3.93e-10  -1.23e-07   4.47e-08  -nan(ind)  -nan(ind)
008   2.67e-01  -6.53e-10   6.18e-10   3.28e-11  -3.05e-08  -nan(ind)  -nan(ind)
009   2.92e-01   6.97e-10  -1.39e-09   4.91e-09   2.72e-08  -nan(ind)  -nan(ind)
010   3.16e-01   5.29e-10   1.85e-10   1.97e-08  -2.63e-08  -nan(ind)  -nan(ind)
011   3.41e-01   1.04e-09   6.37e-11  -9.71e-09   5.57e-09  -nan(ind)  -nan(ind)
012   3.66e-01  -1.29e-09   2.46e-11  -1.71e-08   2.14e-08  -nan(ind)  -nan(ind)
013   3.90e-01   1.25e-09   1.03e-09   2.53e-08  -2.83e-08  -nan(ind)  -nan(ind)
014   4.15e-01  -8.02e-10  -8.87e-10   6.26e-09   5.42e-09  -nan(ind)  -nan(ind)
015   4.40e-01   1.29e-10   2.85e-10   2.41e-08   5.49e-09  -nan(ind)  -nan(ind)
016   4.64e-01  -1.57e-09   2.25e-10  -2.37e-08  -5.33e-09  -nan(ind)  -nan(ind)
017   4.89e-01  -9.72e-10  -1.16e-10   2.01e-08   2.25e-08  -nan(ind)  -nan(ind)
018   5.14e-01   9.43e-10  -5.39e-10   7.96e-09  -1.85e-08  -nan(ind)  -nan(ind)
019   5.38e-01   1.45e-09   1.63e-09   3.05e-10   9.33e-09  -nan(ind)  -nan(ind)
020   5.63e-01   4.50e-10   7.93e-10   1.24e-08  -3.47e-08  -nan(ind)  -nan(ind)
021   5.87e-01  -3.05e-09   2.75e-09  -1.04e-07   8.89e-08  -nan(ind)  -nan(ind)
022   6.12e-01  -5.98e-09  -1.96e-09   8.63e-09  -3.10e-08  -nan(ind)  -nan(ind)
023   6.37e-01  -7.03e-09  -1.96e-09   5.04e-10   1.30e-08  -nan(ind)  -nan(ind)
024   6.61e-01  -6.48e-09  -1.96e-09   7.86e-09  -2.29e-08  -nan(ind)  -nan(ind)
025   6.86e-01  -4.51e-09  -1.96e-09   1.93e-08   1.97e-08  -nan(ind)  -nan(ind)
026   7.11e-01   1.27e-09  -1.96e-09  -2.87e-08  -5.49e-09  -nan(ind)  -nan(ind)
027   7.35e-01   1.27e-09  -1.96e-09   3.24e-08   2.30e-09  -nan(ind)  -nan(ind)
028   7.60e-01   1.27e-09  -1.96e-09  -2.18e-08   3.81e-08  -nan(ind)  -nan(ind)

Path constraints: none

[info] Set log level to Info.
[info] ------------------------------------------------------------------------
[info] Elapsed real time: 6 second(s).
[info] Thu Nov  2 17:47:59 2023
[warning] MocoCasADiSolver did NOT succeed:
[warning]   Invalid_Number_Detected
[info] ========================================================================

There is probably something that I am doing wrong, but I can't put my finger on it.
I did replace my locked joints with Welds. Maybe my actuator bounds are not valid? Or something else?

Thank you in advance for your help!

P.S.: Subsidiary question:
How are mesh points split (see "Kinematic constraint values at each mesh point")? I track joint motion for 1.45-0.07=1.38s, with a meshInderval of 0.05. This leads to about 28 mesh points, which looks correct.
But then the points are separated by 0.025, and the time only goes up to 0.76 instead of 1.45
Last edited by Pagnon David on Thu Nov 02, 2023 10:49 am, edited 1 time in total.
Top
User avatar
Pagnon David
Posts: 82
Joined: Mon Jan 06, 2014 3:13 am

Re: NaN detected for output jac_g_x

Post by Pagnon David » Thu Nov 02, 2023 10:41 am

While I'm at it, I'll also post my code and data so that you can reproduce the problem if you wish.

The model is the stock Rajagopal2015 model converted for OpenSim v4 (too large to be attached).

Code: Select all

import os
import opensim as osim
import numpy as np


# inspired by https://github.com/stanfordnmbl/opencap-processing/blob/main/utilsProcessing.py
# and https://github.com/stanfordnmbl/opencap-processing/blob/main/Moco/exampleWalking/exampleWalking.m


###############
# PARAMETERS  #
###############

# Model and motion to track
input_model_file = 'Rajagopal2015_v4.osim'
input_mot_file = 'walking1_videoAndMocap_OpenCap.mot'
mocoTrackName = 'walking1_videoAndMocap_OpenCap'
output_model_file = os.path.splitext(input_model_file)[0] + '_mocowithcontacts.osim'
trackedStatesFile = mocoTrackName + '_tracked_states.sto'

# model
coordinateActuatorsOptimalForce = 250 # actuators # 250 instead of 10

stiffness = 1000000 # contacts
dissipation = 2.0
static_friction = 0.8
dynamic_friction = 0.8
viscous_friction = 0.5
transition_velocity = 0.2

# motion
lowPassFilterFreq = 6
fractionExtraBoundSize = 0.1 # State's bounds: +/- 10% of the range of the tracked data 

# times and intervals
initialTime = 0.07
finalTime = 1.45
meshInterval = 0.05 # 20 fps but Hermite-Simpson transcription -> 2n+1 grid points -> 2*20+1 = 41 fps

# weights
controlEffortWeight = 0.1
stateTrackingWeight = 1
# # For CMJ
# calcnPosTrackingWeight = 1000;
# calcnVelTrackingWeight = 100;
# calcnTargetTrackingHeight = 0.03;


###############
# MODEL       #
###############

# Remove actuators, muscles, and groups
model = osim.Model(input_model_file)

forces = model.updForceSet()
[forces.remove(i) for i in range(forces.getSize()-1, -1, -1)] # in reverse order or index out of bounds

group_names = osim.ArrayStr()
forces.getGroupNames(group_names)
[forces.removeGroup(group_names.get(i)) for i in range(forces.getNumGroups()-1,-1,-1)]

modelProcessor = osim.ModelProcessor(model)

# Add actuators and increase their optimal forces
modelProcessor.append(osim.ModOpAddReserves(coordinateActuatorsOptimalForce)) # float('inf'), True

# Weld locked joints (not 'radius_hand
jointsToWeld = osim.StdVectorString()
jointsToWeld.append('mtp_r')
jointsToWeld.append('mtp_l')
jointsToWeld.append('radius_hand_r')
jointsToWeld.append('radius_hand_l')
jointsToWeld.append('subtalar_r')
jointsToWeld.append('subtalar_l')
modelProcessor.append(osim.ModOpReplaceJointsWithWelds(jointsToWeld))

model = modelProcessor.process()

# Set patella mass to zero (reason here https://github.com/stanfordnmbl/opencap-processing/blob/main/utilsProcessing.py#L601)
bodySet = model.get_BodySet()
for i in range(bodySet.getSize()):        
    c_body = bodySet.get(i)
    c_body_name = c_body.getName()            
    if (c_body_name == 'patella_l' or c_body_name == 'patella_r'):
        c_body.set_mass(0.)
        c_body.set_inertia(osim.Vec6(0))


#################
# MODEL CONTACTS#
#################

# Define contact ground plane and add it to the model
reference_contact_half_space = {"name": "floor", "location": np.array([0, 0, 0]),"orientation": np.array([0, 0, -np.pi/2]), "frame": "ground"}
contact_half_space_frame = model.get_ground()
contactHalfSpace = osim.ContactHalfSpace(osim.Vec3(reference_contact_half_space["location"]),
                                        osim.Vec3(reference_contact_half_space["orientation"]),
                                        contact_half_space_frame, reference_contact_half_space["name"])
contactHalfSpace.connectSocket_frame(contact_half_space_frame)
model.addContactGeometry(contactHalfSpace)

# Define contact spheres
reference_contact_spheres = {
	"s1_r": {"radius": 0.032, "location": np.array([0.0019011578840796601,   -0.01,  -0.00382630379623308]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_r"},
	"s2_r": {"radius": 0.032, "location": np.array([0.14838639994206301,     -0.01,  -0.028713422052654002]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_r"},
	"s3_r": {"radius": 0.032, "location": np.array([0.13300117060705099,     -0.01,  0.051636247344956601]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_r"},
	"s4_r": {"radius": 0.032, "location": np.array([0.066234666199163503,    -0.01,  0.026364160674169801]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_r"},
	"s5_r": {"radius": 0.032, "location": np.array([0.059999999999999998,    -0.01,  -0.018760308461917698]), "orientation": np.array([0, 0, 0]), "socket_frame": "toes_r" },
	"s6_r": {"radius": 0.032, "location": np.array([0.044999999999999998,    -0.01,  0.061856956754965199]), "orientation": np.array([0, 0, 0]), "socket_frame": "toes_r" },
	"s1_l": {"radius": 0.032, "location": np.array([0.0019011578840796601,   -0.01,  0.00382630379623308]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_l"},
	"s2_l": {"radius": 0.032, "location": np.array([0.14838639994206301,     -0.01,  0.028713422052654002]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_l"},
	"s3_l": {"radius": 0.032, "location": np.array([0.13300117060705099,     -0.01,  -0.051636247344956601]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_l"},
	"s4_l": {"radius": 0.032, "location": np.array([0.066234666199163503,    -0.01,  -0.026364160674169801]), "orientation": np.array([0, 0, 0]), "socket_frame": "calcn_l"},
	"s5_l": {"radius": 0.032, "location": np.array([0.059999999999999998,    -0.01,  0.018760308461917698]), "orientation": np.array([0, 0, 0]), "socket_frame": "toes_l" },
	"s6_l": {"radius": 0.032, "location": np.array([0.044999999999999998,    -0.01,  -0.061856956754965199]), "orientation": np.array([0, 0, 0]), "socket_frame": "toes_l" }}      
reference_scale_factors = {"calcn_r": np.array([0.91392399999999996, 0.91392399999999996, 0.91392399999999996]),
						   "toes_r":  np.array([0.91392399999999996, 0.91392399999999996, 0.91392399999999996]),
						   "calcn_l": np.array([0.91392399999999996, 0.91392399999999996, 0.91392399999999996]),
						   "toes_l":  np.array([0.91392399999999996, 0.91392399999999996, 0.91392399999999996])}


# Add contact spheres to the model
for ref_contact_sphere in reference_contact_spheres:    
    body = bodySet.get(reference_contact_spheres[ref_contact_sphere]["socket_frame"])
    # Scale location based on attached_geometry scale_factors
    attached_geometry = body.get_attached_geometry(0)
    c_scale_factors = attached_geometry.get_scale_factors().to_numpy() 
    c_ref_scale_factors = reference_scale_factors[reference_contact_spheres[ref_contact_sphere]["socket_frame"]]
    scale_factors = c_ref_scale_factors / c_scale_factors        
    scale_factors[1] = 1 # y position not scaled
    scaled_location = reference_contact_spheres[ref_contact_sphere]["location"] / scale_factors
    c_contactSphere = osim.ContactSphere(reference_contact_spheres[ref_contact_sphere]["radius"], 
                                        osim.Vec3(scaled_location), body, ref_contact_sphere)
    c_contactSphere.connectSocket_frame(body)
    model.addContactGeometry(c_contactSphere)
    # SmoothSphereHalfSpaceForces
    SmoothSphereHalfSpaceForce = osim.SmoothSphereHalfSpaceForce("SmoothSphereHalfSpaceForce_" + ref_contact_sphere, 
                                                                c_contactSphere, contactHalfSpace)
    SmoothSphereHalfSpaceForce.set_stiffness(stiffness)
    SmoothSphereHalfSpaceForce.set_dissipation(dissipation)
    SmoothSphereHalfSpaceForce.set_static_friction(static_friction)
    SmoothSphereHalfSpaceForce.set_dynamic_friction(dynamic_friction)
    SmoothSphereHalfSpaceForce.set_viscous_friction(viscous_friction)
    SmoothSphereHalfSpaceForce.set_transition_velocity(transition_velocity)        
    SmoothSphereHalfSpaceForce.connectSocket_half_space(contactHalfSpace)
    SmoothSphereHalfSpaceForce.connectSocket_sphere(c_contactSphere)
    model.addForce(SmoothSphereHalfSpaceForce)

model.finalizeConnections()
model.initSystem()
model.printToXML(output_model_file)


###############
# MOTION      #
###############

# Set tracking settings and save sto
track = osim.MocoTrack()
track.setName(mocoTrackName)
tableProcessor = osim.TableProcessor(input_mot_file)
tableProcessor.append(osim.TabOpLowPassFilter(lowPassFilterFreq))
tableProcessor.append(osim.TabOpUseAbsoluteStateNames())

modelProcessor = osim.ModelProcessor(output_model_file)
track.setModel(modelProcessor)
track.setStatesReference(tableProcessor)
track.set_states_global_tracking_weight(stateTrackingWeight)
track.set_allow_unused_references(True)
track.set_track_reference_position_derivatives(True)
track.set_apply_tracked_states_to_guess(True)
track.set_initial_time(initialTime)
track.set_final_time(finalTime)
track.set_mesh_interval(meshInterval)
study = track.initialize() # save joint angles and speeds to mocoTrackName_tracked_states.sto

trackedStatesTable = osim.TimeSeriesTable(trackedStatesFile)
trackedStatesTable.trimFrom(initialTime)
trackedStatesTable.trimTo(finalTime)
# pd.DataFrame(trackedStatesTable.getMatrix().to_numpy())

problem = study.updProblem()


###############
# GOALS       #
###############

# Tracking goals. Other joints and states have value stateTrackingWeight
stateTrackingGoal = osim.MocoStateTrackingGoal.safeDownCast(problem.updGoal('state_tracking'))
stateTrackingGoal.setWeightForState('/jointset/ground_pelvis/pelvis_tilt/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/ground_pelvis/pelvis_tilt/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/back/lumbar_extension/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/back/lumbar_extension/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/hip_r/hip_flexion_r/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/hip_r/hip_flexion_r/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/hip_l/hip_flexion_l/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/hip_l/hip_flexion_l/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/walker_knee_r/knee_angle_r/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/walker_knee_r/knee_angle_r/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/walker_knee_l/knee_angle_l/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/walker_knee_l/knee_angle_l/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/ankle_r/ankle_angle_r/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/ankle_r/ankle_angle_r/value', 10.0)
stateTrackingGoal.setWeightForState('/jointset/ankle_l/ankle_angle_l/speed', 10.0)
stateTrackingGoal.setWeightForState('/jointset/ankle_l/ankle_angle_l/value', 10.0)


# Periodicity
periodicityGoal = osim.MocoPeriodicityGoal('periodicityGoal')
problem.addGoal(periodicityGoal)
model = modelProcessor.process()
model.initSystem()
# Periodic coordinates (except pelvis forward translation) and speeds
for i in range(model.getNumStateVariables()):
    currentStateName = str(model.getStateVariableNames().getitem(i))
    if currentStateName.startswith('/jointset'):
        if 'pelvis_tx/value' not in currentStateName:
            periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(currentStateName))
# Periodic coordinate actuator (lumbar and upper limb) controls
modelActuators = model.getActuators()
for i in range(modelActuators.getSize()):
    thisActuator = modelActuators.get(i)
    if thisActuator.getConcreteClassName() == 'CoordinateActuator':
        thisPair = osim.MocoPeriodicityGoalPair(thisActuator.getAbsolutePathString())
        periodicityGoal.addControlPair(thisPair)


# Effort goal
effortGoal = osim.MocoControlGoal.safeDownCast(problem.updGoal('control_effort'))
effortGoal.setWeight(controlEffortWeight)


###############
# BOUNDS      #
###############

# State's bounds based on a fraction of the range of the state's value from the tracked data (here, +/- 10% of the range):
# - Lower bound: (minimum value) - fractionExtraBoundSize * (range of value)
# - Upper bound: (maximum value) + fractionExtraBoundSize * (range of value)
statesLabels = trackedStatesTable.getColumnLabels()
for col in statesLabels:
    if 'jointset' in col: # Remove welded joints
        stateColumn = trackedStatesTable.getDependentColumn(col).to_numpy()
        colmin, colmax = min(stateColumn), max(stateColumn)
        colrange = colmax - colmin
        extraBoundSize = colrange * fractionExtraBoundSize
        extraBounds = [colmin - extraBoundSize, colmax + extraBoundSize]
        problem.setStateInfo(col, extraBounds, [], [])


###############
# Solve       #
###############

solver = osim.MocoCasADiSolver.safeDownCast(study.updSolver())
solver.set_minimize_implicit_auxiliary_derivatives(True)
solver.set_implicit_auxiliary_derivatives_weight(1e-6)
solution = study.solve()
solution.write(mocoTrackName + '_solution.sto')


# Add extraction of GRF
contact_r = osim.StdVectorString()
contact_l = osim.StdVectorString()
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s1_r')
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s2_r')
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s3_r')
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s4_r')
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s5_r')
contact_r.add('/forceset/SmoothSphereHalfSpaceForce_s6_r')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s1_l')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s2_l')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s3_l')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s4_l')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s5_l')
contact_l.add('/forceset/SmoothSphereHalfSpaceForce_s6_l')

externalForcesTableFlat = osim.createExternalLoadsTableForGait(model, solution.exportToStatesTrajectory(model), contact_r, contact_l)
osim.STOFileAdapter.write(externalForcesTableFlat, mocoTrackName+'_grfs.sto')
Attachments
walking1_videoAndMocap_OpenCap.mot
(95.3 KiB) Downloaded 3 times
Top
User avatar
Nicholas Bianco
Posts: 963
Joined: Thu Oct 04, 2012 8:09 pm

Re: NaN detected for output jac_g_x

Post by Nicholas Bianco » Thu Nov 02, 2023 12:09 pm

Hi David,

You're getting NaN's because you set the patella mass to zero. The OpenCap pipeline sets it to zero because it removes the patella entirely since muscle paths are approximated with functions. But if you include a zero mass body in Moco (that isn't an intermediate body or welded to another body with mass), it will produce NaN's when computing gradients in CasADi.

-Nick
Top
User avatar
Pagnon David
Posts: 82
Joined: Mon Jan 06, 2014 3:13 am

Re: NaN detected for output jac_g_x

Post by Pagnon David » Thu Nov 02, 2023 4:10 pm

Wow that was a quick fix, thanks!
I'm glad I asked, I was getting crazy and I could not know where to look for anymore.

Just for the record, why would a zero mass lead to nan speeds? Are speeds not calculated as d angle / d t, but as integral( sum(F) / m dt )?
If so, why?

And since you're there, do you know how mesh points are split? (see "Kinematic constraint values at each mesh point" on my last error message).
I track joint motion for 1.45-0.07=1.38s, with a meshInderval of 0.05. This leads to about 28 mesh points, which looks correct. But then the points are separated by 0.025, and the time only goes up to 0.76 instead of 1.45
Top
POST REPLY