diff --git a/CHANGELOG.md b/CHANGELOG.md index 272415bd..31a71360 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,4 +4,6 @@ This is not a comprehensive list of changes but rather a hand-curated collection Changes ======= +- 06/29/2023: Add support for torque-driven models ([pull request](https://github.com/stanfordnmbl/opencap-processing/pull/66)) +- 06/28/2023: Add support for contacts on one side only ([pull request](https://github.com/stanfordnmbl/opencap-processing/pull/57)) - 12/16/2022: Install CMake using conda, no need to install manually anymore ([pull request](https://github.com/stanfordnmbl/opencap-processing/pull/45)). diff --git a/UtilsDynamicSimulations/OpenSimAD/boundsOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/boundsOpenSimAD.py index d743c36d..8c945729 100644 --- a/UtilsDynamicSimulations/OpenSimAD/boundsOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/boundsOpenSimAD.py @@ -277,73 +277,27 @@ def getBoundsForceDerivative(self): return (upperBoundsForceDerivative, lowerBoundsForceDerivative, scalingForceDerivative) - def getBoundsArmExcitation(self, armJoints): + def getBoundsCoordinateDynamics(self, coordinates, optimal_forces = {}, + default_optimal_force=500): lb = [-1] - lb_vec = lb * len(armJoints) + lb_vec = lb * len(coordinates) ub = [1] - ub_vec = ub * len(armJoints) - s = [150] - s_vec = s * len(armJoints) - upperBoundsArmExcitation = pd.DataFrame([ub_vec], - columns=armJoints) - lowerBoundsArmExcitation = pd.DataFrame([lb_vec], - columns=armJoints) - scalingArmExcitation = pd.DataFrame([s_vec], columns=armJoints) - - return (upperBoundsArmExcitation, lowerBoundsArmExcitation, - scalingArmExcitation) - - def getBoundsArmActivation(self, armJoints): - - lb = [-1] - lb_vec = lb * len(armJoints) - ub = [1] - ub_vec = ub * len(armJoints) - s = [150] - s_vec = s * len(armJoints) - upperBoundsArmActivation = pd.DataFrame([ub_vec], - columns=armJoints) - lowerBoundsArmActivation = pd.DataFrame([lb_vec], - columns=armJoints) - scalingArmActivation = pd.DataFrame([s_vec], columns=armJoints) - - return (upperBoundsArmActivation, lowerBoundsArmActivation, - scalingArmActivation) - - def getBoundsLumbarExcitation(self, lumbarJoints): - - lb = [-1] - lb_vec = lb * len(lumbarJoints) - ub = [1] - ub_vec = ub * len(lumbarJoints) - s = [300] - s_vec = s * len(lumbarJoints) - upperBoundsLumbarExcitation = pd.DataFrame([ub_vec], - columns=lumbarJoints) - lowerBoundsLumbarExcitation = pd.DataFrame([lb_vec], - columns=lumbarJoints) - scalingLumbarExcitation = pd.DataFrame([s_vec], columns=lumbarJoints) - - return (upperBoundsLumbarExcitation, lowerBoundsLumbarExcitation, - scalingLumbarExcitation) - - def getBoundsLumbarActivation(self, lumbarJoints): - - lb = [-1] - lb_vec = lb * len(lumbarJoints) - ub = [1] - ub_vec = ub * len(lumbarJoints) - s = [300] - s_vec = s * len(lumbarJoints) - upperBoundsLumbarActivation = pd.DataFrame([ub_vec], - columns=lumbarJoints) - lowerBoundsLumbarActivation = pd.DataFrame([lb_vec], - columns=lumbarJoints) - scalingLumbarActivation = pd.DataFrame([s_vec], columns=lumbarJoints) + ub_vec = ub * len(coordinates) + s_vec = [] + for coord in coordinates: + if coord in optimal_forces: + s_vec.append(optimal_forces[coord]) + else: + s_vec.append(default_optimal_force) + upperBoundsCoordinateExcitation = pd.DataFrame([ub_vec], + columns=coordinates) + lowerBoundsCoordinateExcitation = pd.DataFrame([lb_vec], + columns=coordinates) + scalingCoordinateExcitation = pd.DataFrame([s_vec], columns=coordinates) - return (upperBoundsLumbarActivation, lowerBoundsLumbarActivation, - scalingLumbarActivation) + return (upperBoundsCoordinateExcitation, lowerBoundsCoordinateExcitation, + scalingCoordinateExcitation) def getBoundsReserveActuators(self, joint, value): diff --git a/UtilsDynamicSimulations/OpenSimAD/mainOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/mainOpenSimAD.py index 22dea459..075728c3 100644 --- a/UtilsDynamicSimulations/OpenSimAD/mainOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/mainOpenSimAD.py @@ -45,13 +45,16 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', # Cost function weights. weights = { - 'activationTerm': settings['weights']['activationTerm'], 'jointAccelerationTerm': settings['weights']['jointAccelerationTerm'], 'armExcitationTerm': settings['weights']['armExcitationTerm'], - 'activationDtTerm': settings['weights']['activationDtTerm'], - 'forceDtTerm': settings['weights']['forceDtTerm'], 'positionTrackingTerm': settings['weights']['positionTrackingTerm'], - 'velocityTrackingTerm': settings['weights']['velocityTrackingTerm']} + 'velocityTrackingTerm': settings['weights']['velocityTrackingTerm']} + if 'activationTerm' in settings['weights']: + weights['activationTerm'] = settings['weights']['activationTerm'] + if 'forceDtTerm' in settings['weights']: + weights['forceDtTerm'] = settings['weights']['forceDtTerm'] + if 'activationDtTerm' in settings['weights']: + weights['activationDtTerm'] = settings['weights']['activationDtTerm'] # Model info. # Model name. @@ -67,6 +70,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', # Set withArms to True to include arm joints. withArms = True + coordinate_optimal_forces = {} if "withArms" in settings: withArms = settings['withArms'] @@ -302,6 +306,23 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', if 'ipopt_tolerance' in settings: ipopt_tolerance = settings['ipopt_tolerance'] + # Set torque_driven_model to True to replace the muscle actuators with + # ideal torque actuators. The default optimal forces of the ideal torque + # actuators are defined in the function + # get_coordinate_actuator_optimal_forces in muscleDataOpenSimAD. You can + # also specify the optimal forces as part of the problem settings. For + # example, to set the optimal force of the right hip flexion to 400, add + # coordinate_optimal_forces['hip_flexion_r'] = 400 in the settings dict. + # See example under running_torque_driven in settingsOpenSimAD. + torque_driven_model = False + if 'torque_driven_model' in settings: + torque_driven_model = settings['torque_driven_model'] + if torque_driven_model: + weights['coordinateExcitationTerm'] = 1 + if 'coordinateExcitationTerm' in settings['weights']: + weights['coordinateExcitationTerm'] = ( + settings['weights']['coordinateExcitationTerm']) + # %% Paths and dirs. pathOSData = os.path.join(dataDir, subject, 'OpenSimData') pathModelFolder = os.path.join(pathOSData, 'Model') @@ -409,6 +430,15 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', s_muscleVolume = np.ones((nMuscles,)) s_muscleVolume = np.reshape(s_muscleVolume, (nMuscles, 1)) + # Coordinate actuator optimal forces. + from muscleDataOpenSimAD import get_coordinate_actuator_optimal_forces + coordinate_optimal_forces = get_coordinate_actuator_optimal_forces() + # Adjust based on settings. + if 'coordinate_optimal_forces' in settings: + for coord in settings['coordinate_optimal_forces']: + coordinate_optimal_forces[coord] = ( + settings['coordinate_optimal_forces'][coord]) + # %% Coordinates. # This section specifies the coordinates of the model. This is # specific to the Rajagopal musculoskeletal model. @@ -467,6 +497,17 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', for joint in mtpJoints: passiveTorqueJoints.remove(joint) nPassiveTorqueJoints = len(passiveTorqueJoints) + + # Muscle-driven coordinates. + muscleDrivenJoints = [ + 'hip_flexion_l', 'hip_flexion_r', 'hip_adduction_l', + 'hip_adduction_r', 'hip_rotation_l', 'hip_rotation_r', 'knee_angle_l', + 'knee_angle_r', 'ankle_angle_l', 'ankle_angle_r', 'subtalar_angle_l', + 'subtalar_angle_r', 'lumbar_extension', 'lumbar_bending', + 'lumbar_rotation'] + for joint in lumbarJoints: + muscleDrivenJoints.remove(joint) + nMuscleDrivenJoints = len(muscleDrivenJoints) # Specify which states should have periodic constraints. # See settingsOpenSimAD for example. @@ -486,13 +527,22 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', for joint in periodicConstraints['Qds']: idxPeriodicQds.append(joints.index(joint)) if 'muscles' in periodicConstraints: - if 'all' in periodicConstraints['muscles']: - idxPeriodicMuscles = getIndices(bothSidesMuscles, - bothSidesMuscles) + if torque_driven_model: + if 'all' in periodicConstraints['muscles']: + idxPeriodicMuscles = getIndices(muscleDrivenJoints, + muscleDrivenJoints) + else: + idxPeriodicMuscles = [] + for c_m in periodicConstraints['muscles']: + idxPeriodicMuscles.append(muscleDrivenJoints.index(c_m)) else: - idxPeriodicMuscles = [] - for c_m in periodicConstraints['Qds']: - idxPeriodicMuscles.append(bothSidesMuscles.index(c_m)) + if 'all' in periodicConstraints['muscles']: + idxPeriodicMuscles = getIndices(bothSidesMuscles, + bothSidesMuscles) + else: + idxPeriodicMuscles = [] + for c_m in periodicConstraints['Qds']: + idxPeriodicMuscles.append(bothSidesMuscles.index(c_m)) if 'lumbar' in periodicConstraints: if 'all' in periodicConstraints['lumbar']: idxPeriodicLumbar = getIndices(lumbarJoints, @@ -502,15 +552,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', for c_m in periodicConstraints['lumbar']: idxPeriodicLumbar.append(lumbarJoints.index(c_m)) - # Muscle-driven coordinates. - muscleDrivenJoints = [ - 'hip_flexion_l', 'hip_flexion_r', 'hip_adduction_l', - 'hip_adduction_r', 'hip_rotation_l', 'hip_rotation_r', 'knee_angle_l', - 'knee_angle_r', 'ankle_angle_l', 'ankle_angle_r', 'subtalar_angle_l', - 'subtalar_angle_r', 'lumbar_extension', 'lumbar_bending', - 'lumbar_rotation'] - for joint in lumbarJoints: - muscleDrivenJoints.remove(joint) + # %% Coordinate actuator activation dynamics. if withArms or withLumbarCoordinateActuators: @@ -519,6 +561,8 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', f_armDynamics = coordinateActuatorDynamics(nArmJoints) if withLumbarCoordinateActuators: f_lumbarDynamics = coordinateActuatorDynamics(nLumbarJoints) + if torque_driven_model: + f_coordinateDynamics = coordinateActuatorDynamics(nMuscleDrivenJoints) # %% Passive/limit torques. from functionCasADiOpenSimAD import limitPassiveTorque, linarPassiveTorque @@ -633,70 +677,71 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', leftPolynomialJoints.remove('mtp_angle_l') rightPolynomialJoints.remove('mtp_angle_r') - # Load polynomials if computed already, compute otherwise. - loadPolynomialData = True - if (not os.path.exists(os.path.join( - pathModelFolder, model_full_name + '_polynomial_r_{}.npy'.format(type_bounds_polynomials))) - or not os.path.exists(os.path.join( - pathModelFolder, model_full_name + '_polynomial_l_{}.npy'.format(type_bounds_polynomials)))): - loadPolynomialData = False - from muscleDataOpenSimAD import getPolynomialData - polynomialData = {} - polynomialData['r'] = getPolynomialData( - loadPolynomialData, pathModelFolder, model_full_name, pathDummyMotion, - rightPolynomialJoints, rightSideMuscles, - type_bounds_polynomials=type_bounds_polynomials, side='r') - polynomialData['l'] = getPolynomialData( - loadPolynomialData, pathModelFolder, model_full_name, pathDummyMotion, - leftPolynomialJoints, leftSideMuscles, - type_bounds_polynomials=type_bounds_polynomials, side='l') - if loadPolynomialData: - polynomialData['r'] = polynomialData['r'].item() - polynomialData['l'] = polynomialData['l'].item() - # Coefficients should not be larger than 1. - sides = ['r', 'l'] - for side in sides: - for c_pol in polynomialData[side]: - assert (np.max(polynomialData[side][c_pol]['coefficients']) < 1), ( - "coeffs {}".format(side)) - - # The function f_polynomial takes as inputs joint positions and velocities - # from one side, and returns muscle-tendon lengths, velocities, and moment - # arms for the muscle of that side. - nPolynomials = len(leftPolynomialJoints) - f_polynomial = {} - f_polynomial['r'] = polynomialApproximation( - rightSideMuscles, polynomialData['r'], nPolynomials) - f_polynomial['l'] = polynomialApproximation( - leftSideMuscles, polynomialData['l'], nPolynomials) + if not torque_driven_model: + # Load polynomials if computed already, compute otherwise. + loadPolynomialData = True + if (not os.path.exists(os.path.join( + pathModelFolder, model_full_name + '_polynomial_r_{}.npy'.format(type_bounds_polynomials))) + or not os.path.exists(os.path.join( + pathModelFolder, model_full_name + '_polynomial_l_{}.npy'.format(type_bounds_polynomials)))): + loadPolynomialData = False + from muscleDataOpenSimAD import getPolynomialData + polynomialData = {} + polynomialData['r'] = getPolynomialData( + loadPolynomialData, pathModelFolder, model_full_name, pathDummyMotion, + rightPolynomialJoints, rightSideMuscles, + type_bounds_polynomials=type_bounds_polynomials, side='r') + polynomialData['l'] = getPolynomialData( + loadPolynomialData, pathModelFolder, model_full_name, pathDummyMotion, + leftPolynomialJoints, leftSideMuscles, + type_bounds_polynomials=type_bounds_polynomials, side='l') + if loadPolynomialData: + polynomialData['r'] = polynomialData['r'].item() + polynomialData['l'] = polynomialData['l'].item() + # Coefficients should not be larger than 1. + sides = ['r', 'l'] + for side in sides: + for c_pol in polynomialData[side]: + assert (np.max(polynomialData[side][c_pol]['coefficients']) < 1), ( + "coeffs {}".format(side)) + + # The function f_polynomial takes as inputs joint positions and velocities + # from one side, and returns muscle-tendon lengths, velocities, and moment + # arms for the muscle of that side. + nPolynomials = len(leftPolynomialJoints) + f_polynomial = {} + f_polynomial['r'] = polynomialApproximation( + rightSideMuscles, polynomialData['r'], nPolynomials) + f_polynomial['l'] = polynomialApproximation( + leftSideMuscles, polynomialData['l'], nPolynomials) + + # Helper indices. + leftPolynomialJointIndices = getIndices(joints, leftPolynomialJoints) + rightPolynomialJointIndices = getIndices(joints, rightPolynomialJoints) + leftPolynomialMuscleIndices = ( + list(range(nSideMuscles)) + + list(range(nSideMuscles, nSideMuscles))) + rightPolynomialMuscleIndices = list(range(nSideMuscles)) + from utilsOpenSimAD import getMomentArmIndices + momentArmIndices = getMomentArmIndices( + rightSideMuscles, leftPolynomialJoints,rightPolynomialJoints, + polynomialData['r']) - # Helper indices. - leftPolynomialJointIndices = getIndices(joints, leftPolynomialJoints) - rightPolynomialJointIndices = getIndices(joints, rightPolynomialJoints) - leftPolynomialMuscleIndices = ( - list(range(nSideMuscles)) + - list(range(nSideMuscles, nSideMuscles))) - rightPolynomialMuscleIndices = list(range(nSideMuscles)) - from utilsOpenSimAD import getMomentArmIndices - momentArmIndices = getMomentArmIndices( - rightSideMuscles, leftPolynomialJoints,rightPolynomialJoints, - polynomialData['r']) - - # Plot polynomial approximations (when possible) for sanity check. - plotPolynomials = False - if plotPolynomials: - from polynomialsOpenSimAD import testPolynomials - path_data4PolynomialFitting = os.path.join( - pathModelFolder, - 'data4PolynomialFitting_{}.npy'.format(model_full_name)) - data4PolynomialFitting = np.load(path_data4PolynomialFitting, - allow_pickle=True).item() - testPolynomials( - data4PolynomialFitting, rightPolynomialJoints, rightSideMuscles, - f_polynomial['r'], polynomialData['r'], momentArmIndices) - testPolynomials( - data4PolynomialFitting, leftPolynomialJoints, leftSideMuscles, - f_polynomial['l'], polynomialData['l'], momentArmIndices) + # Plot polynomial approximations (when possible) for sanity check. + plotPolynomials = False + if plotPolynomials: + from polynomialsOpenSimAD import testPolynomials + path_data4PolynomialFitting = os.path.join( + pathModelFolder, + 'data4PolynomialFitting_{}.npy'.format(model_full_name)) + data4PolynomialFitting = np.load(path_data4PolynomialFitting, + allow_pickle=True).item() + testPolynomials( + data4PolynomialFitting, rightPolynomialJoints, rightSideMuscles, + f_polynomial['r'], polynomialData['r'], momentArmIndices) + testPolynomials( + data4PolynomialFitting, leftPolynomialJoints, leftSideMuscles, + f_polynomial['l'], polynomialData['l'], momentArmIndices) # %% External functions. # The external function is written in C++ and compiled as a library, which @@ -785,7 +830,9 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', if withArms: f_nArmJointsSum2 = normSumSqr(nArmJoints) if withLumbarCoordinateActuators: - f_nLumbarJointsSum2 = normSumSqr(nLumbarJoints) + f_nLumbarJointsSum2 = normSumSqr(nLumbarJoints) + if torque_driven_model: + f_nCoordinatesSum2 = normSumSqr(nMuscleDrivenJoints) f_diffTorques = diffTorques() # %% OPTIMAL CONTROL PROBLEM FORMULATION @@ -806,18 +853,26 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', uw, lw, scaling = {}, {}, {} bounds = bounds_tracking(Qs_toTrack, joints, rightSideMuscles) # States. - # Muscle activations. - uw['A'], lw['A'], scaling['A'] = bounds.getBoundsActivation(lb_activation=lb_activation) - uw['Ak'] = ca.vec(uw['A'].to_numpy().T * np.ones((1, N+1))).full() - lw['Ak'] = ca.vec(lw['A'].to_numpy().T * np.ones((1, N+1))).full() - uw['Aj'] = ca.vec(uw['A'].to_numpy().T * np.ones((1, d*N))).full() - lw['Aj'] = ca.vec(lw['A'].to_numpy().T * np.ones((1, d*N))).full() - # Muscle forces. - uw['F'], lw['F'], scaling['F'] = bounds.getBoundsForce() - uw['Fk'] = ca.vec(uw['F'].to_numpy().T * np.ones((1, N+1))).full() - lw['Fk'] = ca.vec(lw['F'].to_numpy().T * np.ones((1, N+1))).full() - uw['Fj'] = ca.vec(uw['F'].to_numpy().T * np.ones((1, d*N))).full() - lw['Fj'] = ca.vec(lw['F'].to_numpy().T * np.ones((1, d*N))).full() + if torque_driven_model: + # Coordinate activations. + uw['CoordA'], lw['CoordA'], scaling['CoordA'] = bounds.getBoundsCoordinateDynamics(muscleDrivenJoints, coordinate_optimal_forces) + uw['CoordAk'] = ca.vec(uw['CoordA'].to_numpy().T * np.ones((1, N+1))).full() + lw['CoordAk'] = ca.vec(lw['CoordA'].to_numpy().T * np.ones((1, N+1))).full() + uw['CoordAj'] = ca.vec(uw['CoordA'].to_numpy().T * np.ones((1, d*N))).full() + lw['CoordAj'] = ca.vec(lw['CoordA'].to_numpy().T * np.ones((1, d*N))).full() + else: + # Muscle activations. + uw['A'], lw['A'], scaling['A'] = bounds.getBoundsActivation(lb_activation=lb_activation) + uw['Ak'] = ca.vec(uw['A'].to_numpy().T * np.ones((1, N+1))).full() + lw['Ak'] = ca.vec(lw['A'].to_numpy().T * np.ones((1, N+1))).full() + uw['Aj'] = ca.vec(uw['A'].to_numpy().T * np.ones((1, d*N))).full() + lw['Aj'] = ca.vec(lw['A'].to_numpy().T * np.ones((1, d*N))).full() + # Muscle forces. + uw['F'], lw['F'], scaling['F'] = bounds.getBoundsForce() + uw['Fk'] = ca.vec(uw['F'].to_numpy().T * np.ones((1, N+1))).full() + lw['Fk'] = ca.vec(lw['F'].to_numpy().T * np.ones((1, N+1))).full() + uw['Fj'] = ca.vec(uw['F'].to_numpy().T * np.ones((1, d*N))).full() + lw['Fj'] = ca.vec(lw['F'].to_numpy().T * np.ones((1, d*N))).full() # Joint positions. uw['Qs'], lw['Qs'], scaling['Qs'] = bounds.getBoundsPosition(polynomial_bounds) uw['Qsk'] = ca.vec(uw['Qs'].to_numpy().T * np.ones((1, N+1))).full() @@ -832,39 +887,45 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', lw['Qdsj'] = ca.vec(lw['Qds'].to_numpy().T*np.ones((1, d*N))).full() if withArms: # Arm activations. - uw['ArmA'], lw['ArmA'], scaling['ArmA'] = bounds.getBoundsArmActivation(armJoints) + uw['ArmA'], lw['ArmA'], scaling['ArmA'] = bounds.getBoundsCoordinateDynamics(armJoints, coordinate_optimal_forces) uw['ArmAk'] = ca.vec(uw['ArmA'].to_numpy().T * np.ones((1, N+1))).full() lw['ArmAk'] = ca.vec(lw['ArmA'].to_numpy().T * np.ones((1, N+1))).full() uw['ArmAj'] = ca.vec(uw['ArmA'].to_numpy().T * np.ones((1, d*N))).full() lw['ArmAj'] = ca.vec(lw['ArmA'].to_numpy().T * np.ones((1, d*N))).full() if withLumbarCoordinateActuators: # Lumbar activations. - uw['LumbarA'], lw['LumbarA'], scaling['LumbarA'] = bounds.getBoundsLumbarActivation(lumbarJoints) + uw['LumbarA'], lw['LumbarA'], scaling['LumbarA'] = bounds.getBoundsCoordinateDynamics(lumbarJoints, coordinate_optimal_forces) uw['LumbarAk'] = ca.vec(uw['LumbarA'].to_numpy().T * np.ones((1, N+1))).full() lw['LumbarAk'] = ca.vec(lw['LumbarA'].to_numpy().T * np.ones((1, N+1))).full() uw['LumbarAj'] = ca.vec(uw['LumbarA'].to_numpy().T * np.ones((1, d*N))).full() lw['LumbarAj'] = ca.vec(lw['LumbarA'].to_numpy().T * np.ones((1, d*N))).full() # Controls. - # Muscle activation derivatives. - uw['ADt'], lw['ADt'], scaling['ADt'] = bounds.getBoundsActivationDerivative( - activationTimeConstant=activationTimeConstant, - deactivationTimeConstant=deactivationTimeConstant) - uw['ADtk'] = ca.vec(uw['ADt'].to_numpy().T * np.ones((1, N))).full() - lw['ADtk'] = ca.vec(lw['ADt'].to_numpy().T * np.ones((1, N))).full() + if torque_driven_model: + # Coordinate excitations. + uw['CoordE'], lw['CoordE'], scaling['CoordE'] = bounds.getBoundsCoordinateDynamics(muscleDrivenJoints, coordinate_optimal_forces) + uw['CoordEk'] = ca.vec(uw['CoordE'].to_numpy().T * np.ones((1, N))).full() + lw['CoordEk'] = ca.vec(lw['CoordE'].to_numpy().T * np.ones((1, N))).full() + else: + # Muscle activation derivatives. + uw['ADt'], lw['ADt'], scaling['ADt'] = bounds.getBoundsActivationDerivative( + activationTimeConstant=activationTimeConstant, + deactivationTimeConstant=deactivationTimeConstant) + uw['ADtk'] = ca.vec(uw['ADt'].to_numpy().T * np.ones((1, N))).full() + lw['ADtk'] = ca.vec(lw['ADt'].to_numpy().T * np.ones((1, N))).full() + # Muscle force derivatives. + uw['FDt'], lw['FDt'], scaling['FDt'] = bounds.getBoundsForceDerivative() + uw['FDtk'] = ca.vec(uw['FDt'].to_numpy().T * np.ones((1, N))).full() + lw['FDtk'] = ca.vec(lw['FDt'].to_numpy().T * np.ones((1, N))).full() if withArms: # Arm excitations. - uw['ArmE'], lw['ArmE'], scaling['ArmE'] = bounds.getBoundsArmExcitation(armJoints) + uw['ArmE'], lw['ArmE'], scaling['ArmE'] = bounds.getBoundsCoordinateDynamics(armJoints, coordinate_optimal_forces) uw['ArmEk'] = ca.vec(uw['ArmE'].to_numpy().T * np.ones((1, N))).full() lw['ArmEk'] = ca.vec(lw['ArmE'].to_numpy().T * np.ones((1, N))).full() if withLumbarCoordinateActuators: # Lumbar excitations. - uw['LumbarE'], lw['LumbarE'], scaling['LumbarE'] = bounds.getBoundsLumbarExcitation(lumbarJoints) + uw['LumbarE'], lw['LumbarE'], scaling['LumbarE'] = bounds.getBoundsCoordinateDynamics(lumbarJoints, coordinate_optimal_forces) uw['LumbarEk'] = ca.vec(uw['LumbarE'].to_numpy().T * np.ones((1, N))).full() - lw['LumbarEk'] = ca.vec(lw['LumbarE'].to_numpy().T * np.ones((1, N))).full() - # Muscle force derivatives. - uw['FDt'], lw['FDt'], scaling['FDt'] = bounds.getBoundsForceDerivative() - uw['FDtk'] = ca.vec(uw['FDt'].to_numpy().T * np.ones((1, N))).full() - lw['FDtk'] = ca.vec(lw['FDt'].to_numpy().T * np.ones((1, N))).full() + lw['LumbarEk'] = ca.vec(lw['LumbarE'].to_numpy().T * np.ones((1, N))).full() # Joint velocity derivatives (accelerations). uw['Qdds'], lw['Qdds'], scaling['Qdds'] = bounds.getBoundsAcceleration() uw['Qddsk'] = ca.vec(uw['Qdds'].to_numpy().T * np.ones((1, N))).full() @@ -892,12 +953,17 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', guess = dataDrivenGuess_tracking( Qs_toTrack, N, d, joints, bothSidesMuscles) # States. - # Muscle activations. - w0['A'] = guess.getGuessActivation(scaling['A']) - w0['Aj'] = guess.getGuessActivationCol() - # Muscle forces. - w0['F'] = guess.getGuessForce(scaling['F']) - w0['Fj'] = guess.getGuessForceCol() + if torque_driven_model: + # Coordinate activations. + w0['CoordA'] = guess.getGuessTorqueActuatorActivation(muscleDrivenJoints) + w0['CoordAj'] = guess.getGuessTorqueActuatorActivationCol(muscleDrivenJoints) + else: + # Muscle activations. + w0['A'] = guess.getGuessActivation(scaling['A']) + w0['Aj'] = guess.getGuessActivationCol() + # Muscle forces. + w0['F'] = guess.getGuessForce(scaling['F']) + w0['Fj'] = guess.getGuessForceCol() # Joint positions. w0['Qs'] = guess.getGuessPosition(scaling['Qs']) w0['Qsj'] = guess.getGuessPositionCol() @@ -913,16 +979,20 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', w0['LumbarA'] = guess.getGuessTorqueActuatorActivation(lumbarJoints) w0['LumbarAj'] = guess.getGuessTorqueActuatorActivationCol(lumbarJoints) # Controls - # Muscle activation derivatives. - w0['ADt'] = guess.getGuessActivationDerivative(scaling['ADt']) + if torque_driven_model: + # Coordinate excitations. + w0['CoordE'] = guess.getGuessTorqueActuatorExcitation(muscleDrivenJoints) + else: + # Muscle activation derivatives. + w0['ADt'] = guess.getGuessActivationDerivative(scaling['ADt']) + # Muscle force derivatives. + w0['FDt'] = guess.getGuessForceDerivative(scaling['FDt']) if withArms: - # Arm activations. + # Arm excitations. w0['ArmE'] = guess.getGuessTorqueActuatorExcitation(armJoints) if withLumbarCoordinateActuators: - # Lumbar activations. - w0['LumbarE'] = guess.getGuessTorqueActuatorExcitation(lumbarJoints) - # Muscle force derivatives. - w0['FDt'] = guess.getGuessForceDerivative(scaling['FDt']) + # Lumbar excitations. + w0['LumbarE'] = guess.getGuessTorqueActuatorExcitation(lumbarJoints) # Joint velocity derivatives (accelerations). w0['Qdds'] = guess.getGuessAcceleration(scaling['Qdds']) # Reserve actuators. @@ -1039,30 +1109,44 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', # Time step. h = timeElapsed / N # States. - # Muscle activation at mesh points. - a = opti.variable(nMuscles, N+1) - opti.subject_to(opti.bounded(lw['Ak'], ca.vec(a), uw['Ak'])) - opti.set_initial(a, w0['A'].to_numpy().T) - assert np.alltrue(lw['Ak'] <= ca.vec(w0['A'].to_numpy().T).full()), "Issue with lower bound muscle activations" - assert np.alltrue(uw['Ak'] >= ca.vec(w0['A'].to_numpy().T).full()), "Issue with upper bound muscle activations" - # Muscle activation at collocation points. - a_col = opti.variable(nMuscles, d*N) - opti.subject_to(opti.bounded(lw['Aj'], ca.vec(a_col), uw['Aj'])) - opti.set_initial(a_col, w0['Aj'].to_numpy().T) - assert np.alltrue(lw['Aj'] <= ca.vec(w0['Aj'].to_numpy().T).full()), "Issue with lower bound muscle activations (collocation points)" - assert np.alltrue(uw['Aj'] >= ca.vec(w0['Aj'].to_numpy().T).full()), "Issue with upper bound muscle activations (collocation points)" - # Muscle force at mesh points. - nF = opti.variable(nMuscles, N+1) - opti.subject_to(opti.bounded(lw['Fk'], ca.vec(nF), uw['Fk'])) - opti.set_initial(nF, w0['F'].to_numpy().T) - assert np.alltrue(lw['Fk'] <= ca.vec(w0['F'].to_numpy().T).full()), "Issue with lower bound muscle forces" - assert np.alltrue(uw['Fk'] >= ca.vec(w0['F'].to_numpy().T).full()), "Issue with upper bound muscle forces" - # Muscle force at collocation points. - nF_col = opti.variable(nMuscles, d*N) - opti.subject_to(opti.bounded(lw['Fj'], ca.vec(nF_col), uw['Fj'])) - opti.set_initial(nF_col, w0['Fj'].to_numpy().T) - assert np.alltrue(lw['Fj'] <= ca.vec(w0['Fj'].to_numpy().T).full()), "Issue with lower bound muscle forces (collocation points)" - assert np.alltrue(uw['Fj'] >= ca.vec(w0['Fj'].to_numpy().T).full()), "Issue with upper bound muscle forces (collocation points)" + if torque_driven_model: + # Coordinate activation at mesh points. + aCoord = opti.variable(nMuscleDrivenJoints, N+1) + opti.subject_to(opti.bounded(lw['CoordAk'], ca.vec(aCoord), uw['CoordAk'])) + opti.set_initial(aCoord, w0['CoordA'].to_numpy().T) + assert np.alltrue(lw['CoordAk'] <= ca.vec(w0['CoordA'].to_numpy().T).full()), "Issue with lower bound coordinate activations" + assert np.alltrue(uw['CoordAk'] >= ca.vec(w0['CoordA'].to_numpy().T).full()), "Issue with upper bound coordinate activations" + # Coordinate activation at collocation points. + aCoord_col = opti.variable(nMuscleDrivenJoints, d*N) + opti.subject_to(opti.bounded(lw['CoordAj'], ca.vec(aCoord_col), uw['CoordAj'])) + opti.set_initial(aCoord_col, w0['CoordAj'].to_numpy().T) + assert np.alltrue(lw['CoordAj'] <= ca.vec(w0['CoordAj'].to_numpy().T).full()), "Issue with lower bound coordinate activations (collocation points)" + assert np.alltrue(uw['CoordAj'] >= ca.vec(w0['CoordAj'].to_numpy().T).full()), "Issue with upper bound coordinate activations (collocation points)" + else: + # Muscle activation at mesh points. + a = opti.variable(nMuscles, N+1) + opti.subject_to(opti.bounded(lw['Ak'], ca.vec(a), uw['Ak'])) + opti.set_initial(a, w0['A'].to_numpy().T) + assert np.alltrue(lw['Ak'] <= ca.vec(w0['A'].to_numpy().T).full()), "Issue with lower bound muscle activations" + assert np.alltrue(uw['Ak'] >= ca.vec(w0['A'].to_numpy().T).full()), "Issue with upper bound muscle activations" + # Muscle activation at collocation points. + a_col = opti.variable(nMuscles, d*N) + opti.subject_to(opti.bounded(lw['Aj'], ca.vec(a_col), uw['Aj'])) + opti.set_initial(a_col, w0['Aj'].to_numpy().T) + assert np.alltrue(lw['Aj'] <= ca.vec(w0['Aj'].to_numpy().T).full()), "Issue with lower bound muscle activations (collocation points)" + assert np.alltrue(uw['Aj'] >= ca.vec(w0['Aj'].to_numpy().T).full()), "Issue with upper bound muscle activations (collocation points)" + # Muscle force at mesh points. + nF = opti.variable(nMuscles, N+1) + opti.subject_to(opti.bounded(lw['Fk'], ca.vec(nF), uw['Fk'])) + opti.set_initial(nF, w0['F'].to_numpy().T) + assert np.alltrue(lw['Fk'] <= ca.vec(w0['F'].to_numpy().T).full()), "Issue with lower bound muscle forces" + assert np.alltrue(uw['Fk'] >= ca.vec(w0['F'].to_numpy().T).full()), "Issue with upper bound muscle forces" + # Muscle force at collocation points. + nF_col = opti.variable(nMuscles, d*N) + opti.subject_to(opti.bounded(lw['Fj'], ca.vec(nF_col), uw['Fj'])) + opti.set_initial(nF_col, w0['Fj'].to_numpy().T) + assert np.alltrue(lw['Fj'] <= ca.vec(w0['Fj'].to_numpy().T).full()), "Issue with lower bound muscle forces (collocation points)" + assert np.alltrue(uw['Fj'] >= ca.vec(w0['Fj'].to_numpy().T).full()), "Issue with upper bound muscle forces (collocation points)" # Joint position at mesh points. Qs = opti.variable(nJoints, N+1) opti.subject_to(opti.bounded(lw['Qsk'], ca.vec(Qs), uw['Qsk'])) @@ -1124,12 +1208,20 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', assert np.alltrue(lw['LumbarAj'] <= ca.vec(w0['LumbarAj'].to_numpy().T).full()), "Issue with lower bound lumbar activations (collocation points)" assert np.alltrue(uw['LumbarAj'] >= ca.vec(w0['LumbarAj'].to_numpy().T).full()), "Issue with upper bound lumbar activations (collocation points)" # Controls. - # Muscle activation derivative at mesh points. - aDt = opti.variable(nMuscles, N) - opti.subject_to(opti.bounded(lw['ADtk'], ca.vec(aDt), uw['ADtk'])) - opti.set_initial(aDt, w0['ADt'].to_numpy().T) - assert np.alltrue(lw['ADtk'] <= ca.vec(w0['ADt'].to_numpy().T).full()), "Issue with lower bound muscle activation derivatives" - assert np.alltrue(uw['ADtk'] >= ca.vec(w0['ADt'].to_numpy().T).full()), "Issue with upper bound muscle activation derivatives" + if torque_driven_model: + # Coordinate excitation at mesh points. + eCoord = opti.variable(nMuscleDrivenJoints, N) + opti.subject_to(opti.bounded(lw['CoordEk'], ca.vec(eCoord), uw['CoordEk'])) + opti.set_initial(eCoord, w0['CoordE'].to_numpy().T) + assert np.alltrue(lw['CoordEk'] <= ca.vec(w0['CoordE'].to_numpy().T).full()), "Issue with lower bound coordinate excitations" + assert np.alltrue(uw['CoordEk'] >= ca.vec(w0['CoordE'].to_numpy().T).full()), "Issue with upper bound coordinate excitations" + else: + # Muscle activation derivative at mesh points. + aDt = opti.variable(nMuscles, N) + opti.subject_to(opti.bounded(lw['ADtk'], ca.vec(aDt), uw['ADtk'])) + opti.set_initial(aDt, w0['ADt'].to_numpy().T) + assert np.alltrue(lw['ADtk'] <= ca.vec(w0['ADt'].to_numpy().T).full()), "Issue with lower bound muscle activation derivatives" + assert np.alltrue(uw['ADtk'] >= ca.vec(w0['ADt'].to_numpy().T).full()), "Issue with upper bound muscle activation derivatives" if withArms: # Arm excitation at mesh points. eArm = opti.variable(nArmJoints, N) @@ -1144,12 +1236,13 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', opti.set_initial(eLumbar, w0['LumbarE'].to_numpy().T) assert np.alltrue(lw['LumbarEk'] <= ca.vec(w0['LumbarE'].to_numpy().T).full()), "Issue with lower bound lumbar excitations" assert np.alltrue(uw['LumbarEk'] >= ca.vec(w0['LumbarE'].to_numpy().T).full()), "Issue with upper bound lumbar excitations" - # Muscle force derivative at mesh points. - nFDt = opti.variable(nMuscles, N) - opti.subject_to(opti.bounded(lw['FDtk'], ca.vec(nFDt), uw['FDtk'])) - opti.set_initial(nFDt, w0['FDt'].to_numpy().T) - assert np.alltrue(lw['FDtk'] <= ca.vec(w0['FDt'].to_numpy().T).full()), "Issue with lower bound muscle force derivatives" - assert np.alltrue(uw['FDtk'] >= ca.vec(w0['FDt'].to_numpy().T).full()), "Issue with upper bound muscle force derivatives" + if not torque_driven_model: + # Muscle force derivative at mesh points. + nFDt = opti.variable(nMuscles, N) + opti.subject_to(opti.bounded(lw['FDtk'], ca.vec(nFDt), uw['FDtk'])) + opti.set_initial(nFDt, w0['FDt'].to_numpy().T) + assert np.alltrue(lw['FDtk'] <= ca.vec(w0['FDt'].to_numpy().T).full()), "Issue with lower bound muscle force derivatives" + assert np.alltrue(uw['FDtk'] >= ca.vec(w0['FDt'].to_numpy().T).full()), "Issue with upper bound muscle force derivatives" # Joint velocity derivative (acceleration) at mesh points. Qdds = opti.variable(nJoints, N) opti.subject_to(opti.bounded(lw['Qddsk'], ca.vec(Qdds), uw['Qddsk'])) @@ -1173,18 +1266,20 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', plotGuessVSBounds(lw, uw, w0, nJoints, N, d, guessQsEnd, guessQdsEnd, withArms=withArms, withLumbarCoordinateActuators= - withLumbarCoordinateActuators) + withLumbarCoordinateActuators, + torque_driven_model=torque_driven_model) # %% Unscale design variables. - nF_nsc = nF * (scaling['F'].to_numpy().T * np.ones((1, N+1))) - nF_col_nsc = nF_col * (scaling['F'].to_numpy().T * np.ones((1, d*N))) + if not torque_driven_model: + nF_nsc = nF * (scaling['F'].to_numpy().T * np.ones((1, N+1))) + nF_col_nsc = nF_col * (scaling['F'].to_numpy().T * np.ones((1, d*N))) + aDt_nsc = aDt * (scaling['ADt'].to_numpy().T * np.ones((1, N))) + nFDt_nsc = nFDt * (scaling['FDt'].to_numpy().T * np.ones((1, N))) Qs_nsc = Qs * (scaling['Qs'].to_numpy().T * np.ones((1, N+1))) Qs_col_nsc = Qs_col * (scaling['Qs'].to_numpy().T * np.ones((1, d*N))) Qds_nsc = Qds * (scaling['Qds'].to_numpy().T * np.ones((1, N+1))) - Qds_col_nsc = Qds_col * (scaling['Qds'].to_numpy().T * np.ones((1, d*N))) - aDt_nsc = aDt * (scaling['ADt'].to_numpy().T * np.ones((1, N))) - Qdds_nsc = Qdds * (scaling['Qdds'].to_numpy().T * np.ones((1, N))) - nFDt_nsc = nFDt * (scaling['FDt'].to_numpy().T * np.ones((1, N))) + Qds_col_nsc = Qds_col * (scaling['Qds'].to_numpy().T * np.ones((1, d*N))) + Qdds_nsc = Qdds * (scaling['Qdds'].to_numpy().T * np.ones((1, N))) if withReserveActuators: rAct_nsc = {} for c_j in reserveActuatorCoordinates: @@ -1208,9 +1303,12 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', for k in range(N): # Variables within current mesh. # States. - akj = (ca.horzcat(a[:, k], a_col[:, k*d:(k+1)*d])) - nFkj = (ca.horzcat(nF[:, k], nF_col[:, k*d:(k+1)*d])) - nFkj_nsc = (ca.horzcat(nF_nsc[:, k], nF_col_nsc[:, k*d:(k+1)*d])) + if torque_driven_model: + aCoordkj = (ca.horzcat(aCoord[:, k], aCoord_col[:, k*d:(k+1)*d])) + else: + akj = (ca.horzcat(a[:, k], a_col[:, k*d:(k+1)*d])) + nFkj = (ca.horzcat(nF[:, k], nF_col[:, k*d:(k+1)*d])) + nFkj_nsc = (ca.horzcat(nF_nsc[:, k], nF_col_nsc[:, k*d:(k+1)*d])) Qskj = (ca.horzcat(Qs[:, k], Qs_col[:, k*d:(k+1)*d])) Qskj_nsc = (ca.horzcat(Qs_nsc[:, k], Qs_col_nsc[:, k*d:(k+1)*d])) Qdskj = (ca.horzcat(Qds[:, k], Qds_col[:, k*d:(k+1)*d])) @@ -1220,10 +1318,13 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', if withLumbarCoordinateActuators: aLumbarkj = (ca.horzcat(aLumbar[:, k], aLumbar_col[:, k*d:(k+1)*d])) # Controls. - aDtk = aDt[:, k] - aDtk_nsc = aDt_nsc[:, k] - nFDtk = nFDt[:, k] - nFDtk_nsc = nFDt_nsc[:, k] + if torque_driven_model: + eCoordk = eCoord[:, k] + else: + aDtk = aDt[:, k] + aDtk_nsc = aDt_nsc[:, k] + nFDtk = nFDt[:, k] + nFDtk_nsc = nFDt_nsc[:, k] Qddsk = Qdds[:, k] Qddsk_nsc = Qdds_nsc[:, k] if withArms: @@ -1241,50 +1342,51 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', QsQdskj_nsc[::2, :] = Qskj_nsc[idxJoints4F, :] QsQdskj_nsc[1::2, :] = Qdskj_nsc[idxJoints4F, :] - # Polynomial approximations - # Left side. - Qsink_l = Qskj_nsc[leftPolynomialJointIndices, 0] - Qdsink_l = Qdskj_nsc[leftPolynomialJointIndices, 0] - [lMTk_l, vMTk_l, dMk_l] = f_polynomial['l'](Qsink_l, Qdsink_l) - # Right side. - Qsink_r = Qskj_nsc[rightPolynomialJointIndices, 0] - Qdsink_r = Qdskj_nsc[rightPolynomialJointIndices, 0] - [lMTk_r, vMTk_r, dMk_r] = f_polynomial['r'](Qsink_r, Qdsink_r) - # Muscle-tendon lengths and velocities. - lMTk_lr = ca.vertcat(lMTk_l[leftPolynomialMuscleIndices], - lMTk_r[rightPolynomialMuscleIndices]) - vMTk_lr = ca.vertcat(vMTk_l[leftPolynomialMuscleIndices], - vMTk_r[rightPolynomialMuscleIndices]) - # Moment arms. - dMk = {} - # Left side. - for joint in leftPolynomialJoints: - if ((joint != 'mtp_angle_l') and - (joint != 'lumbar_extension') and - (joint != 'lumbar_bending') and - (joint != 'lumbar_rotation')): - dMk[joint] = dMk_l[ - momentArmIndices[joint], - leftPolynomialJoints.index(joint)] - # Right side. - for joint in rightPolynomialJoints: - if ((joint != 'mtp_angle_r') and - (joint != 'lumbar_extension') and - (joint != 'lumbar_bending') and - (joint != 'lumbar_rotation')): - # We need to adjust momentArmIndices for the right - # side since polynomial indices are 'one-sided'. - # We subtract by the number of side muscles. - c_ma = [i - nSideMuscles for - i in momentArmIndices[joint]] - dMk[joint] = dMk_r[ - c_ma, rightPolynomialJoints.index(joint)] - - # Hill-equilibrium. - [hillEquilibriumk, Fk, activeFiberForcek, passiveFiberForcek, - normActiveFiberLengthForcek, nFiberLengthk, - fiberVelocityk, _, _] = (f_hillEquilibrium( - akj[:, 0], lMTk_lr, vMTk_lr, nFkj_nsc[:, 0], nFDtk_nsc)) + if not torque_driven_model: + # Polynomial approximations + # Left side. + Qsink_l = Qskj_nsc[leftPolynomialJointIndices, 0] + Qdsink_l = Qdskj_nsc[leftPolynomialJointIndices, 0] + [lMTk_l, vMTk_l, dMk_l] = f_polynomial['l'](Qsink_l, Qdsink_l) + # Right side. + Qsink_r = Qskj_nsc[rightPolynomialJointIndices, 0] + Qdsink_r = Qdskj_nsc[rightPolynomialJointIndices, 0] + [lMTk_r, vMTk_r, dMk_r] = f_polynomial['r'](Qsink_r, Qdsink_r) + # Muscle-tendon lengths and velocities. + lMTk_lr = ca.vertcat(lMTk_l[leftPolynomialMuscleIndices], + lMTk_r[rightPolynomialMuscleIndices]) + vMTk_lr = ca.vertcat(vMTk_l[leftPolynomialMuscleIndices], + vMTk_r[rightPolynomialMuscleIndices]) + # Moment arms. + dMk = {} + # Left side. + for joint in leftPolynomialJoints: + if ((joint != 'mtp_angle_l') and + (joint != 'lumbar_extension') and + (joint != 'lumbar_bending') and + (joint != 'lumbar_rotation')): + dMk[joint] = dMk_l[ + momentArmIndices[joint], + leftPolynomialJoints.index(joint)] + # Right side. + for joint in rightPolynomialJoints: + if ((joint != 'mtp_angle_r') and + (joint != 'lumbar_extension') and + (joint != 'lumbar_bending') and + (joint != 'lumbar_rotation')): + # We need to adjust momentArmIndices for the right + # side since polynomial indices are 'one-sided'. + # We subtract by the number of side muscles. + c_ma = [i - nSideMuscles for + i in momentArmIndices[joint]] + dMk[joint] = dMk_r[ + c_ma, rightPolynomialJoints.index(joint)] + + # Hill-equilibrium. + [hillEquilibriumk, Fk, activeFiberForcek, passiveFiberForcek, + normActiveFiberLengthForcek, nFiberLengthk, + fiberVelocityk, _, _] = (f_hillEquilibrium( + akj[:, 0], lMTk_lr, vMTk_lr, nFkj_nsc[:, 0], nFDtk_nsc)) # Limit torques. passiveTorque_k = {} @@ -1326,8 +1428,11 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', # Loop over collocation points. for j in range(d): # Expression for the state derivatives. - ap = ca.mtimes(akj, C[j+1]) - nFp_nsc = ca.mtimes(nFkj_nsc, C[j+1]) + if torque_driven_model: + aCoordp = ca.mtimes(aCoordkj, C[j+1]) + else: + ap = ca.mtimes(akj, C[j+1]) + nFp_nsc = ca.mtimes(nFkj_nsc, C[j+1]) Qsp_nsc = ca.mtimes(Qskj_nsc, C[j+1]) Qdsp_nsc = ca.mtimes(Qdskj_nsc, C[j+1]) if withArms: @@ -1336,11 +1441,17 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', aLumbarp = ca.mtimes(aLumbarkj, C[j+1]) # Append collocation equations. - # Muscle activation dynamics. - opti.subject_to((h*aDtk_nsc - ap) == 0) - # Muscle contraction dynamics. - opti.subject_to((h*nFDtk_nsc - nFp_nsc) / - scaling['F'].to_numpy().T == 0) + if torque_driven_model: + # Coordinate activation dynamics. + aCoordDtj = f_coordinateDynamics( + eCoordk, aCoordkj[:, j+1]) + opti.subject_to(h*aCoordDtj - aCoordp == 0) + else: + # Muscle activation dynamics. + opti.subject_to((h*aDtk_nsc - ap) == 0) + # Muscle contraction dynamics. + opti.subject_to((h*nFDtk_nsc - nFp_nsc) / + scaling['F'].to_numpy().T == 0) # Skeleton dynamics. # Position derivative. opti.subject_to((h*Qdskj_nsc[:, j+1] - Qsp_nsc) / @@ -1360,26 +1471,30 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', opti.subject_to(h*aLumbarDtj - aLumbarp == 0) # Cost function - activationTerm = f_NMusclesSumWeightedPow( - akj[:, j+1], s_muscleVolume * w_muscles) - jointAccelerationTerm = f_nJointsSum2(Qddsk) - activationDtTerm = f_NMusclesSum2(aDtk) - forceDtTerm = f_NMusclesSum2(nFDtk) + jointAccelerationTerm = f_nJointsSum2(Qddsk) positionTrackingTerm = f_NQsToTrackWSum2( Qskj[idx_coordinates_toTrack, 0], dataToTrack_Qs_sc_offset[:, k], w_dataToTrack) velocityTrackingTerm = f_NQsToTrackWSum2( Qdskj[idx_coordinates_toTrack, 0], - dataToTrack_Qds_sc[:, k], w_dataToTrack) - + dataToTrack_Qds_sc[:, k], w_dataToTrack) J += (( weights['positionTrackingTerm'] * positionTrackingTerm + weights['velocityTrackingTerm'] * velocityTrackingTerm + - weights['activationTerm'] * activationTerm + - weights['jointAccelerationTerm'] * jointAccelerationTerm + - weights['activationDtTerm'] * activationDtTerm + - weights['forceDtTerm'] * forceDtTerm) * h * B[j + 1]) - + weights['jointAccelerationTerm'] * jointAccelerationTerm) * h * B[j + 1]) + if torque_driven_model: + coordinateExcitationTerm = f_nCoordinatesSum2(eCoordk) + J += (weights['coordinateExcitationTerm'] * + coordinateExcitationTerm * h * B[j + 1]) + else: + activationTerm = f_NMusclesSumWeightedPow( + akj[:, j+1], s_muscleVolume * w_muscles) + activationDtTerm = f_NMusclesSum2(aDtk) + forceDtTerm = f_NMusclesSum2(nFDtk) + J += (( + weights['activationTerm'] * activationTerm + + weights['activationDtTerm'] * activationDtTerm + + weights['forceDtTerm'] * forceDtTerm) * h * B[j + 1]) if withArms: armExcitationTerm = f_nArmJointsSum2(eArmk) J += (weights['armExcitationTerm'] * @@ -1419,16 +1534,27 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', opti.subject_to(Tk[idxGroundPelvisJointsinF, 0] == 0) # Skeleton dynamics. - # Muscle-driven joint torques. - for joint in muscleDrivenJoints: - Fk_joint = Fk[momentArmIndices[joint]] - mTk_joint = ca.sum1(dMk[joint]*Fk_joint) - if withReserveActuators and joint in reserveActuatorCoordinates: - mTk_joint += rActk_nsc[joint] - diffTk_joint = f_diffTorques( - Tk[F_map['residuals'][joint] ], mTk_joint, - passiveTorque_k[joint]) - opti.subject_to(diffTk_joint == 0) + if torque_driven_model: + for cj, joint in enumerate(muscleDrivenJoints): + coordAct_joint = ( + scaling['CoordE'].iloc[0][joint] * + aCoordkj[cj, 0]) + diffTk_joint = f_diffTorques( + Tk[F_map['residuals'][joint]], + coordAct_joint, + passiveTorque_k[joint]) + opti.subject_to(diffTk_joint == 0) + else: + # Muscle-driven joint torques. + for joint in muscleDrivenJoints: + Fk_joint = Fk[momentArmIndices[joint]] + mTk_joint = ca.sum1(dMk[joint]*Fk_joint) + if withReserveActuators and joint in reserveActuatorCoordinates: + mTk_joint += rActk_nsc[joint] + diffTk_joint = f_diffTorques( + Tk[F_map['residuals'][joint] ], mTk_joint, + passiveTorque_k[joint]) + opti.subject_to(diffTk_joint == 0) # Torque-driven joint torques. # Lumbar joints. @@ -1438,7 +1564,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', scaling['LumbarE'].iloc[0][joint] * aLumbarkj[cj, 0]) diffTk_lumbar = f_diffTorques( - Tk[F_map['residuals'][joint] ], + Tk[F_map['residuals'][joint]], coordAct_lumbar, passiveTorque_k[joint]) opti.subject_to(diffTk_lumbar == 0) @@ -1462,26 +1588,28 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', linearPassiveTorqueMtp_k[joint])) opti.subject_to(diffTk_joint == 0) - # Activation dynamics. - act1 = aDtk_nsc + akj[:, 0] / deactivationTimeConstant - act2 = aDtk_nsc + akj[:, 0] / activationTimeConstant - opti.subject_to(act1 >= 0) - opti.subject_to(act2 <= 1 / activationTimeConstant) - - # Contraction dynamics. - opti.subject_to(hillEquilibriumk == 0) + if not torque_driven_model: + # Activation dynamics. + act1 = aDtk_nsc + akj[:, 0] / deactivationTimeConstant + act2 = aDtk_nsc + akj[:, 0] / activationTimeConstant + opti.subject_to(act1 >= 0) + opti.subject_to(act2 <= 1 / activationTimeConstant) + + # Contraction dynamics. + opti.subject_to(hillEquilibriumk == 0) # Equality / continuity constraints. - opti.subject_to(a[:, k+1] == ca.mtimes(akj, D)) - opti.subject_to(nF[:, k+1] == ca.mtimes(nFkj, D)) + if torque_driven_model: + opti.subject_to(aCoord[:, k+1] == ca.mtimes(aCoordkj, D)) + else: + opti.subject_to(a[:, k+1] == ca.mtimes(akj, D)) + opti.subject_to(nF[:, k+1] == ca.mtimes(nFkj, D)) opti.subject_to(Qs[:, k+1] == ca.mtimes(Qskj, D)) opti.subject_to(Qds[:, k+1] == ca.mtimes(Qdskj, D)) if withArms: - opti.subject_to(aArm[:, k+1] == - ca.mtimes(aArmkj, D)) + opti.subject_to(aArm[:, k+1] == ca.mtimes(aArmkj, D)) if withLumbarCoordinateActuators: - opti.subject_to(aLumbar[:, k+1] == - ca.mtimes(aLumbarkj, D)) + opti.subject_to(aLumbar[:, k+1] == ca.mtimes(aLumbarkj, D)) # Other constraints. # We might want the model's heels to remain in contact with the @@ -1512,10 +1640,14 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', Qds[idxPeriodicQds, 0] == 0) # Muscle activations and forces. if 'muscles' in periodicConstraints: - opti.subject_to(a[idxPeriodicMuscles, -1] - - a[idxPeriodicMuscles, 0] == 0) - opti.subject_to(nF[idxPeriodicMuscles, -1] - - nF[idxPeriodicMuscles, 0] == 0) + if torque_driven_model: + opti.subject_to(aCoord[idxPeriodicMuscles, -1] - + aCoord[idxPeriodicMuscles, 0] == 0) + else: + opti.subject_to(a[idxPeriodicMuscles, -1] - + a[idxPeriodicMuscles, 0] == 0) + opti.subject_to(nF[idxPeriodicMuscles, -1] - + nF[idxPeriodicMuscles, 0] == 0) if 'lumbar' in periodicConstraints: # Lumbar activations opti.subject_to(aLumbar[idxPeriodicLumbar, -1] - @@ -1561,18 +1693,28 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', if offset_ty: offset_opt = w_opt[starti:starti+1] starti += 1 - a_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*(N+1)], (N+1, nMuscles))).T - starti = starti + nMuscles*(N+1) - a_col_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*(d*N)], (d*N, nMuscles))).T - starti = starti + nMuscles*(d*N) - nF_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*(N+1)], (N+1, nMuscles))).T - starti = starti + nMuscles*(N+1) - nF_col_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*(d*N)], (d*N, nMuscles))).T - starti = starti + nMuscles*(d*N) + if torque_driven_model: + aCoord_opt = ( + np.reshape(w_opt[starti:starti+nMuscleDrivenJoints*(N+1)], + (N+1, nMuscleDrivenJoints))).T + starti = starti + nMuscleDrivenJoints*(N+1) + aCoord_col_opt = ( + np.reshape(w_opt[starti:starti+nMuscleDrivenJoints*(d*N)], + (d*N, nMuscleDrivenJoints))).T + starti = starti + nMuscleDrivenJoints*(d*N) + else: + a_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*(N+1)], (N+1, nMuscles))).T + starti = starti + nMuscles*(N+1) + a_col_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*(d*N)], (d*N, nMuscles))).T + starti = starti + nMuscles*(d*N) + nF_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*(N+1)], (N+1, nMuscles))).T + starti = starti + nMuscles*(N+1) + nF_col_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*(d*N)], (d*N, nMuscles))).T + starti = starti + nMuscles*(d*N) Qs_opt = ( np.reshape(w_opt[starti:starti+nJoints*(N+1)], (N+1, nJoints)) ).T starti = starti + nJoints*(N+1) @@ -1603,9 +1745,15 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', np.reshape(w_opt[starti:starti+nLumbarJoints*(d*N)], (d*N, nLumbarJoints))).T starti = starti + nLumbarJoints*(d*N) - aDt_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*N], (N, nMuscles))).T - starti = starti + nMuscles*N + if torque_driven_model: + eCoord_opt = ( + np.reshape(w_opt[starti:starti+nMuscleDrivenJoints*N], + (N, nMuscleDrivenJoints))).T + starti = starti + nMuscleDrivenJoints*N + else: + aDt_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*N], (N, nMuscles))).T + starti = starti + nMuscles*N if withArms: eArm_opt = ( np.reshape(w_opt[starti:starti+nArmJoints*N], @@ -1616,10 +1764,11 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', np.reshape(w_opt[starti:starti+nLumbarJoints*N], (N, nLumbarJoints))).T starti = starti + nLumbarJoints*N - nFDt_col_opt = ( - np.reshape(w_opt[starti:starti+nMuscles*(N)], (N, nMuscles))).T - starti = starti + nMuscles*(N) - Qdds_col_opt = ( + if not torque_driven_model: + nFDt_opt = ( + np.reshape(w_opt[starti:starti+nMuscles*(N)], (N, nMuscles))).T + starti = starti + nMuscles*(N) + Qdds_opt = ( np.reshape(w_opt[starti:starti+nJoints*(N)],(N, nJoints))).T starti = starti + nJoints*(N) if withReserveActuators: @@ -1634,24 +1783,36 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', visualizeResultsBounds = False if visualizeResultsBounds: from plotsOpenSimAD import plotOptimalSolutionVSBounds - c_wopt = { - 'a_opt': a_opt, 'a_col_opt': a_col_opt, - 'nF_opt': nF_opt, 'nF_col_opt': nF_col_opt, - 'Qs_opt': Qs_opt, 'Qs_col_opt': Qs_col_opt, - 'Qds_opt': Qds_opt, 'Qds_col_opt': Qds_col_opt, - 'aDt_opt': aDt_opt, 'nFDt_col_opt': nFDt_col_opt, - 'Qdds_col_opt': Qdds_col_opt} - plotOptimalSolutionVSBounds(lw, uw, c_wopt) + c_wopt = {'Qs_opt': Qs_opt, 'Qs_col_opt': Qs_col_opt, + 'Qds_opt': Qds_opt, 'Qds_col_opt': Qds_col_opt, + 'Qdds_opt': Qdds_opt} + if torque_driven_model: + c_wopt['aCoord_opt'] = aCoord_opt + c_wopt['aCoord_col_opt'] = aCoord_col_opt + c_wopt['eCoord_opt'] = eCoord_opt + else: + c_wopt['a_opt'] = a_opt + c_wopt['a_col_opt'] = a_col_opt + c_wopt['nF_opt'] = nF_opt + c_wopt['nF_col_opt'] = nF_col_opt + c_wopt['aDt_opt'] = aDt_opt + c_wopt['nFDt_opt'] = nFDt_opt + plotOptimalSolutionVSBounds(lw, uw, c_wopt, + torque_driven_model=torque_driven_model) # %% Unscale results. Qs_opt_nsc = Qs_opt * ( scaling['Qs'].to_numpy().T * np.ones((1, N+1))) Qds_opt_nsc = Qds_opt * ( scaling['Qds'].to_numpy().T * np.ones((1, N+1))) - Qdds_col_opt_nsc = Qdds_col_opt * ( + Qdds_opt_nsc = Qdds_opt * ( scaling['Qdds'].to_numpy().T * np.ones((1, N))) - nFDt_col_opt_nsc = nFDt_col_opt * ( - scaling['FDt'].to_numpy().T * np.ones((1, N))) + if torque_driven_model: + aCoord_opt_nsc = aCoord_opt * ( + scaling['CoordA'].to_numpy().T * np.ones((1, N+1))) + else: + nFDt_opt_nsc = nFDt_opt * ( + scaling['FDt'].to_numpy().T * np.ones((1, N))) if withReserveActuators: rAct_opt_nsc = {} for c_j in reserveActuatorCoordinates: @@ -1702,7 +1863,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', QsQds_opt_nsc = np.zeros((nJoints*2, N+1)) QsQds_opt_nsc[::2, :] = Qs_opt_nsc[idxJoints4F, :] QsQds_opt_nsc[1::2, :] = Qds_opt_nsc[idxJoints4F, :] - Qdds_opt_nsc = Qdds_col_opt_nsc[idxJoints4F, :] + Qdds_opt_nsc = Qdds_opt_nsc[idxJoints4F, :] if treadmill: Tj_temp = F(ca.vertcat( ca.vertcat(QsQds_opt_nsc[:, 0], Qdds_opt_nsc[:, 0]), @@ -1796,14 +1957,23 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', if writeGUI: # Kinematics and activations. - muscleLabels = ([bothSidesMuscle + '/activation' - for bothSidesMuscle in bothSidesMuscles]) - labels = ['time'] + joints - labels_w_muscles = labels + muscleLabels from utils import numpy_to_storage - data = np.concatenate((tgridf.T, Qs_opt_nsc_deg.T, - a_opt.T), axis=1) - numpy_to_storage(labels_w_muscles, data, os.path.join( + labels = ['time'] + joints + if torque_driven_model: + coordLabels = ([joint + '/activation' + for joint in muscleDrivenJoints]) + labels += coordLabels + + data = np.concatenate((tgridf.T, Qs_opt_nsc_deg.T, + aCoord_opt_nsc.T), axis=1) + else: + muscleLabels = ([bothSidesMuscle + '/activation' + for bothSidesMuscle in bothSidesMuscles]) + labels += muscleLabels + + data = np.concatenate((tgridf.T, Qs_opt_nsc_deg.T, + a_opt.T), axis=1) + numpy_to_storage(labels, data, os.path.join( pathResults, 'kinematics_activations_{}_{}.mot'.format( trialName, case)), datatype='IK') # Torques @@ -1881,33 +2051,38 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', # %% Contribution different terms to the cost function. # This also serves as a sanity check. - activationTerm_opt_all = 0 + if torque_driven_model: + coordExcitationTerm_opt_all = 0 + else: + activationTerm_opt_all = 0 + activationDtTerm_opt_all = 0 + forceDtTerm_opt_all = 0 if withArms: armExcitationTerm_opt_all = 0 if withLumbarCoordinateActuators: lumbarExcitationTerm_opt_all = 0 if trackQdds: accelerationTrackingTerm_opt_all = 0 - jointAccelerationTerm_opt_all = 0 - activationDtTerm_opt_all = 0 - forceDtTerm_opt_all = 0 + jointAccelerationTerm_opt_all = 0 positionTrackingTerm_opt_all = 0 velocityTrackingTerm_opt_all = 0 if withReserveActuators: reserveActuatorTerm_opt_all = 0 if min_ratio_vGRF and weights['vGRFRatioTerm'] > 0: - vGRFRatioTerm_opt_all = 0 - pMT_opt = np.zeros((len(muscleDrivenJoints), N)) - aMT_opt = np.zeros((len(muscleDrivenJoints), N)) - pT_opt = np.zeros((nPassiveTorqueJoints, N)) - Ft_opt = np.zeros((nMuscles, N)) + vGRFRatioTerm_opt_all = 0 + if not torque_driven_model: + pMT_opt = np.zeros((len(muscleDrivenJoints), N)) + aMT_opt = np.zeros((len(muscleDrivenJoints), N)) + Ft_opt = np.zeros((nMuscles, N)) + pT_opt = np.zeros((nPassiveTorqueJoints, N)) h = timeElapsed / N for k in range(N): # States. - akj_opt = ca.horzcat(a_opt[:, k], a_col_opt[:, k*d:(k+1)*d]) - nFkj_opt = ca.horzcat(nF_opt[:, k], nF_col_opt[:, k*d:(k+1)*d]) - nFkj_opt_nsc = nFkj_opt * ( - scaling['F'].to_numpy().T * np.ones((1, d+1))) + if not torque_driven_model: + akj_opt = ca.horzcat(a_opt[:, k], a_col_opt[:, k*d:(k+1)*d]) + nFkj_opt = ca.horzcat(nF_opt[:, k], nF_col_opt[:, k*d:(k+1)*d]) + nFkj_opt_nsc = nFkj_opt * ( + scaling['F'].to_numpy().T * np.ones((1, d+1))) Qskj_opt = (ca.horzcat(Qs_opt[:, k], Qs_col_opt[:, k*d:(k+1)*d])) Qskj_opt_nsc = Qskj_opt * ( scaling['Qs'].to_numpy().T * np.ones((1, d+1))) @@ -1915,7 +2090,12 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', Qdskj_opt_nsc = Qdskj_opt * ( scaling['Qds'].to_numpy().T * np.ones((1, d+1))) # Controls. - aDtk_opt = aDt_opt[:, k] + if torque_driven_model: + eCoordk_opt = eCoord_opt[:, k] + else: + aDtk_opt = aDt_opt[:, k] + nFDtk_opt = nFDt_opt[:, k] + nFDtk_opt_nsc = nFDt_opt_nsc[:, k] if withArms: eArmk_opt = eArm_opt[:, k] if withLumbarCoordinateActuators: @@ -1924,69 +2104,68 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', rActk_opt = {} for c_j in reserveActuatorCoordinates: rActk_opt[c_j] = rAct_opt[c_j][:, k] - Qddsk_opt = Qdds_col_opt[:, k] - nFDtk_opt = nFDt_col_opt[:, k] - nFDtk_opt_nsc = nFDt_col_opt_nsc[:, k] + Qddsk_opt = Qdds_opt[:, k] # Joint positions and velocities are intertwined. QsQdskj_opt_nsc = ca.DM(nJoints*2, d+1) QsQdskj_opt_nsc[::2, :] = Qskj_opt_nsc QsQdskj_opt_nsc[1::2, :] = Qdskj_opt_nsc - # Polynomial approximations. - # Left side. - Qsink_opt_l = Qskj_opt_nsc[leftPolynomialJointIndices, 0] - Qdsink_opt_l = Qdskj_opt_nsc[leftPolynomialJointIndices, 0] - [lMTk_opt_l, vMTk_opt_l, dMk_opt_l] = f_polynomial['l']( - Qsink_opt_l, Qdsink_opt_l) - # Right side. - Qsink_opt_r = Qskj_opt_nsc[rightPolynomialJointIndices, 0] - Qdsink_opt_r = Qdskj_opt_nsc[rightPolynomialJointIndices, 0] - [lMTk_opt_r, vMTk_opt_r, dMk_opt_r] = f_polynomial['r']( - Qsink_opt_r, Qdsink_opt_r) - # Muscle-tendon lengths and velocities. - lMTk_opt_lr = ca.vertcat( - lMTk_opt_l[leftPolynomialMuscleIndices], - lMTk_opt_r[rightPolynomialMuscleIndices]) - vMTk_opt_lr = ca.vertcat( - vMTk_opt_l[leftPolynomialMuscleIndices], - vMTk_opt_r[rightPolynomialMuscleIndices]) - # Moment arms. - dMk_opt = {} - # Left side. - for joint in leftPolynomialJoints: - if ((joint != 'mtp_angle_l') and - (joint != 'lumbar_extension') and - (joint != 'lumbar_bending') and - (joint != 'lumbar_rotation')): - dMk_opt[joint] = dMk_opt_l[ - momentArmIndices[joint], - leftPolynomialJoints.index(joint)] - # Right side. - for joint in rightPolynomialJoints: - if ((joint != 'mtp_angle_r') and - (joint != 'lumbar_extension') and - (joint != 'lumbar_bending') and - (joint != 'lumbar_rotation')): - # We need to adjust momentArmIndices for the right - # side since polynomial indices are 'one-sided'. - # We subtract by the number of side muscles. - c_ma = [i - nSideMuscles for - i in momentArmIndices[joint]] - dMk_opt[joint] = dMk_opt_r[ - c_ma, rightPolynomialJoints.index(joint)] - # Hill-equilibrium. - [hillEqk_opt, Fk_opt, _, _,_, _, _, aFPk_opt, pFPk_opt] = ( - f_hillEquilibrium(akj_opt[:, 0], lMTk_opt_lr, vMTk_opt_lr, - nFkj_opt_nsc[:, 0], nFDtk_opt_nsc)) - Ft_opt[:,k] = Fk_opt.full().flatten() - # Passive muscle moments. - for c_j, joint in enumerate(muscleDrivenJoints): - pFk_opt_joint = pFPk_opt[momentArmIndices[joint]] - pMT_opt[c_j, k] = ca.sum1(dMk_opt[joint]*pFk_opt_joint) - # Active muscle moments. - for c_j, joint in enumerate(muscleDrivenJoints): - aFk_opt_joint = aFPk_opt[momentArmIndices[joint]] - aMT_opt[c_j, k] = ca.sum1(dMk_opt[joint]*aFk_opt_joint) + if not torque_driven_model: + # Polynomial approximations. + # Left side. + Qsink_opt_l = Qskj_opt_nsc[leftPolynomialJointIndices, 0] + Qdsink_opt_l = Qdskj_opt_nsc[leftPolynomialJointIndices, 0] + [lMTk_opt_l, vMTk_opt_l, dMk_opt_l] = f_polynomial['l']( + Qsink_opt_l, Qdsink_opt_l) + # Right side. + Qsink_opt_r = Qskj_opt_nsc[rightPolynomialJointIndices, 0] + Qdsink_opt_r = Qdskj_opt_nsc[rightPolynomialJointIndices, 0] + [lMTk_opt_r, vMTk_opt_r, dMk_opt_r] = f_polynomial['r']( + Qsink_opt_r, Qdsink_opt_r) + # Muscle-tendon lengths and velocities. + lMTk_opt_lr = ca.vertcat( + lMTk_opt_l[leftPolynomialMuscleIndices], + lMTk_opt_r[rightPolynomialMuscleIndices]) + vMTk_opt_lr = ca.vertcat( + vMTk_opt_l[leftPolynomialMuscleIndices], + vMTk_opt_r[rightPolynomialMuscleIndices]) + # Moment arms. + dMk_opt = {} + # Left side. + for joint in leftPolynomialJoints: + if ((joint != 'mtp_angle_l') and + (joint != 'lumbar_extension') and + (joint != 'lumbar_bending') and + (joint != 'lumbar_rotation')): + dMk_opt[joint] = dMk_opt_l[ + momentArmIndices[joint], + leftPolynomialJoints.index(joint)] + # Right side. + for joint in rightPolynomialJoints: + if ((joint != 'mtp_angle_r') and + (joint != 'lumbar_extension') and + (joint != 'lumbar_bending') and + (joint != 'lumbar_rotation')): + # We need to adjust momentArmIndices for the right + # side since polynomial indices are 'one-sided'. + # We subtract by the number of side muscles. + c_ma = [i - nSideMuscles for + i in momentArmIndices[joint]] + dMk_opt[joint] = dMk_opt_r[ + c_ma, rightPolynomialJoints.index(joint)] + # Hill-equilibrium. + [hillEqk_opt, Fk_opt, _, _,_, _, _, aFPk_opt, pFPk_opt] = ( + f_hillEquilibrium(akj_opt[:, 0], lMTk_opt_lr, vMTk_opt_lr, + nFkj_opt_nsc[:, 0], nFDtk_opt_nsc)) + Ft_opt[:,k] = Fk_opt.full().flatten() + # Passive muscle moments. + for c_j, joint in enumerate(muscleDrivenJoints): + pFk_opt_joint = pFPk_opt[momentArmIndices[joint]] + pMT_opt[c_j, k] = ca.sum1(dMk_opt[joint]*pFk_opt_joint) + # Active muscle moments. + for c_j, joint in enumerate(muscleDrivenJoints): + aFk_opt_joint = aFPk_opt[momentArmIndices[joint]] + aMT_opt[c_j, k] = ca.sum1(dMk_opt[joint]*aFk_opt_joint) # Passive limit moments. if enableLimitTorques: for c_j, joint in enumerate(passiveTorqueJoints): @@ -1994,18 +2173,22 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', Qskj_opt_nsc[joints.index(joint), 0], Qdskj_opt_nsc[joints.index(joint), 0]) for j in range(d): - activationTerm_opt = f_NMusclesSumWeightedPow(akj_opt[:, j+1], s_muscleVolume * w_muscles) - jointAccelerationTerm_opt = f_nJointsSum2(Qddsk_opt) - activationDtTerm_opt = f_NMusclesSum2(aDtk_opt) - forceDtTerm_opt = f_NMusclesSum2(nFDtk_opt) + if torque_driven_model: + coordExcitationTerm_opt = f_nCoordinatesSum2(eCoordk_opt) + coordExcitationTerm_opt_all += weights['coordinateExcitationTerm'] * coordExcitationTerm_opt * h * B[j + 1] + else: + activationTerm_opt = f_NMusclesSumWeightedPow(akj_opt[:, j+1], s_muscleVolume * w_muscles) + activationDtTerm_opt = f_NMusclesSum2(aDtk_opt) + forceDtTerm_opt = f_NMusclesSum2(nFDtk_opt) + activationTerm_opt_all += weights['activationTerm'] * activationTerm_opt * h * B[j + 1] + activationDtTerm_opt_all += weights['activationDtTerm'] * activationDtTerm_opt * h * B[j + 1] + forceDtTerm_opt_all += weights['forceDtTerm'] * forceDtTerm_opt * h * B[j + 1] + jointAccelerationTerm_opt = f_nJointsSum2(Qddsk_opt) positionTrackingTerm_opt = f_NQsToTrackWSum2(Qskj_opt[idx_coordinates_toTrack, 0], dataToTrack_Qs_sc_offset_opt[:, k], w_dataToTrack) velocityTrackingTerm_opt = f_NQsToTrackWSum2(Qdskj_opt[idx_coordinates_toTrack, 0], dataToTrack_Qds_sc[:, k], w_dataToTrack) positionTrackingTerm_opt_all += weights['positionTrackingTerm'] * positionTrackingTerm_opt * h * B[j + 1] velocityTrackingTerm_opt_all += weights['velocityTrackingTerm'] * velocityTrackingTerm_opt * h * B[j + 1] - activationTerm_opt_all += weights['activationTerm'] * activationTerm_opt * h * B[j + 1] - jointAccelerationTerm_opt_all += weights['jointAccelerationTerm'] * jointAccelerationTerm_opt * h * B[j + 1] - activationDtTerm_opt_all += weights['activationDtTerm'] * activationDtTerm_opt * h * B[j + 1] - forceDtTerm_opt_all += weights['forceDtTerm'] * forceDtTerm_opt * h * B[j + 1] + jointAccelerationTerm_opt_all += weights['jointAccelerationTerm'] * jointAccelerationTerm_opt * h * B[j + 1] if withArms: armExcitationTerm_opt = f_nArmJointsSum2(eArmk_opt) armExcitationTerm_opt_all += weights['armExcitationTerm'] * armExcitationTerm_opt * h * B[j + 1] @@ -2033,10 +2216,13 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', vGRFRatioTerm_opt_all += (weights['vGRFRatioTerm'] * vGRF_ratio_opt * h * B[j + 1]) # "Motor control" terms. - JMotor_opt = (activationTerm_opt_all.full() + - jointAccelerationTerm_opt_all.full() + - activationDtTerm_opt_all.full() + - forceDtTerm_opt_all.full()) + if torque_driven_model: + JMotor_opt = coordExcitationTerm_opt_all.full() + else: + JMotor_opt = (activationTerm_opt_all.full() + + activationDtTerm_opt_all.full() + + forceDtTerm_opt_all.full()) + JMotor_opt += jointAccelerationTerm_opt_all.full() if withArms: JMotor_opt += armExcitationTerm_opt_all.full() if withLumbarCoordinateActuators: @@ -2057,40 +2243,50 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', np.abs(JAll_opt[0][0] - stats['iterations']['obj'][-1]) <= 1e-5), "Error reconstructing optimal cost value" JTerms = {} - JTerms["activationTerm"] = activationTerm_opt_all.full()[0][0] + if torque_driven_model: + JTerms["coordinateExcitationTerm"] = coordExcitationTerm_opt_all.full()[0][0] + else: + JTerms["activationTerm"] = activationTerm_opt_all.full()[0][0] + JTerms["activationDtTerm"] = activationDtTerm_opt_all.full()[0][0] + JTerms["forceDtTerm"] = forceDtTerm_opt_all.full()[0][0] if withArms: JTerms["armExcitationTerm"] = armExcitationTerm_opt_all.full()[0][0] if withLumbarCoordinateActuators: JTerms["lumbarExcitationTerm"] = lumbarExcitationTerm_opt_all.full()[0][0] - JTerms["jointAccelerationTerm"] = jointAccelerationTerm_opt_all.full()[0][0] - JTerms["activationDtTerm"] = activationDtTerm_opt_all.full()[0][0] - JTerms["forceDtTerm"] = forceDtTerm_opt_all.full()[0][0] + JTerms["jointAccelerationTerm"] = jointAccelerationTerm_opt_all.full()[0][0] JTerms["positionTerm"] = positionTrackingTerm_opt_all.full()[0][0] JTerms["velocityTerm"] = velocityTrackingTerm_opt_all.full()[0][0] if trackQdds: - JTerms["accelerationTerm"] = accelerationTrackingTerm_opt_all.full()[0][0] - JTerms["activationTerm_sc"] = JTerms["activationTerm"] / JAll_opt[0][0] + JTerms["accelerationTerm"] = accelerationTrackingTerm_opt_all.full()[0][0] + if torque_driven_model: + JTerms["coordinateExcitationTerm_sc"] = JTerms["coordinateExcitationTerm"] / JAll_opt[0][0] + else: + JTerms["activationTerm_sc"] = JTerms["activationTerm"] / JAll_opt[0][0] + JTerms["activationDtTerm_sc"] = JTerms["activationDtTerm"] / JAll_opt[0][0] + JTerms["forceDtTerm_sc"] = JTerms["forceDtTerm"] / JAll_opt[0][0] if withArms: JTerms["armExcitationTerm_sc"] = JTerms["armExcitationTerm"] / JAll_opt[0][0] if withLumbarCoordinateActuators: JTerms["lumbarExcitationTerm_sc"] = JTerms["lumbarExcitationTerm"] / JAll_opt[0][0] JTerms["jointAccelerationTerm_sc"] = JTerms["jointAccelerationTerm"] / JAll_opt[0][0] - JTerms["activationDtTerm_sc"] = JTerms["activationDtTerm"] / JAll_opt[0][0] - JTerms["forceDtTerm_sc"] = JTerms["forceDtTerm"] / JAll_opt[0][0] + JTerms["positionTerm_sc"] = JTerms["positionTerm"] / JAll_opt[0][0] JTerms["velocityTerm_sc"] = JTerms["velocityTerm"] / JAll_opt[0][0] if trackQdds: JTerms["accelerationTerm_sc"] = JTerms["accelerationTerm"] / JAll_opt[0][0] # Print out contributions to the cost function. print("\nCONTRIBUTION TO THE COST FUNCTION") - print("Muscle activations: {}%".format(np.round(JTerms["activationTerm_sc"] * 100, 2))) + if torque_driven_model: + print("Coordinate excitations: {}%".format(np.round(JTerms["coordinateExcitationTerm_sc"] * 100, 2))) + else: + print("Muscle activations: {}%".format(np.round(JTerms["activationTerm_sc"] * 100, 2))) + print("Muscle activation derivatives: {}%".format(np.round(JTerms["activationDtTerm_sc"] * 100, 2))) + print("Muscle-tendon force derivatives: {}%".format(np.round(JTerms["forceDtTerm_sc"] * 100, 2))) if withArms: - print("Arm activations: {}%".format(np.round(JTerms["armExcitationTerm_sc"] * 100, 2))) + print("Arm excitations: {}%".format(np.round(JTerms["armExcitationTerm_sc"] * 100, 2))) if withLumbarCoordinateActuators: print("Lumbar excitations: {}%".format(np.round(JTerms["lumbarExcitationTerm_sc"] * 100, 2))) - print("Joint accelerations: {}%".format(np.round(JTerms["jointAccelerationTerm_sc"] * 100, 2))) - print("Muscle activation derivatives: {}%".format(np.round(JTerms["activationDtTerm_sc"] * 100, 2))) - print("Muscle-tendon force derivatives: {}%".format(np.round(JTerms["forceDtTerm_sc"] * 100, 2))) + print("Joint accelerations: {}%".format(np.round(JTerms["jointAccelerationTerm_sc"] * 100, 2))) print("Position tracking: {}%".format(np.round(JTerms["positionTerm_sc"] * 100, 2))) print("Velocity tracking: {}%".format(np.round(JTerms["velocityTerm_sc"] * 100, 2))) if trackQdds: @@ -2121,6 +2317,9 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', np.expand_dims(c_KAM['KAM_l'], axis=1)), axis=1).T # %% Compute medial knee contact forces. + if torque_driven_model: + computeMCF = False + print("To compute contact forces, use a muscle-driven model.") if computeMCF: # Export muscle forces and non muscle-driven torques (if existing). import pandas as pd @@ -2248,7 +2447,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', 'coordinate_speeds_toTrack': refData_Qds_nsc, 'coordinate_speeds': Qds_opt_nsc, 'coordinate_accelerations_toTrack': refData_Qdds_nsc, - 'coordinate_accelerations': Qdds_col_opt_nsc, + 'coordinate_accelerations': Qdds_opt_nsc, 'torques': torques_opt, 'torques_BWht': torques_BWht_opt, 'GRF': GRF_all_opt['all'], @@ -2259,10 +2458,7 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', 'rotationalCoordinates': rotationalJoints, 'GRF_labels': GR_labels_fig, 'time': tgridf, - 'muscle_activations': a_opt, 'muscles': bothSidesMuscles, - 'passive_muscle_torques': pMT_opt, - 'active_muscle_torques': aMT_opt, 'passive_limit_torques': pT_opt, 'muscle_driven_joints': muscleDrivenJoints, 'limit_torques_joints': passiveTorqueJoints} @@ -2275,6 +2471,13 @@ def run_tracking(baseDir, dataDir, subject, settings, case='0', optimaltrajectories[case]['MCF_BW'] = MCF_BW optimaltrajectories[case]['MCF_labels'] = MCF_labels optimaltrajectories[case]['iter'] = stats['iter_count'] + + if torque_driven_model: + optimaltrajectories[case]['coordinate_activations'] = aCoord_opt_nsc + else: + optimaltrajectories[case]['muscle_activations'] = a_opt + optimaltrajectories[case]['passive_muscle_torques'] = pMT_opt + optimaltrajectories[case]['passive_muscle_torques'] = aMT_opt np.save(os.path.join(pathResults, 'optimaltrajectories.npy'), - optimaltrajectories) \ No newline at end of file + optimaltrajectories) diff --git a/UtilsDynamicSimulations/OpenSimAD/muscleDataOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/muscleDataOpenSimAD.py index 7a1a9932..208afbda 100644 --- a/UtilsDynamicSimulations/OpenSimAD/muscleDataOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/muscleDataOpenSimAD.py @@ -330,4 +330,37 @@ def passiveJointTorqueData(joint, model_type='rajagopal2016'): k = kAll[joint] theta = thetaAll[joint] - return k, theta \ No newline at end of file + return k, theta + +# %% Coordinate actuator optimal forces. +# Values inspired from Fig. S3 from https://www.ncbi.nlm.nih.gov/pmc/articles/PMC5507211/ +def get_coordinate_actuator_optimal_forces(): + + coordinate_optimal_forces = { + 'hip_flexion_l': 500, + 'hip_flexion_r': 500, + 'hip_adduction_l': 400, + 'hip_adduction_r': 400, + 'hip_rotation_l': 400, + 'hip_rotation_r': 400, + 'knee_angle_l': 400, + 'knee_angle_r': 400, + 'ankle_angle_l': 400, + 'ankle_angle_r': 400, + 'subtalar_angle_l': 400, + 'subtalar_angle_r': 400, + 'lumbar_extension': 300, + 'lumbar_bending': 300, + 'lumbar_rotation': 300, + 'arm_flex_l': 150, + 'arm_add_l': 150, + 'arm_rot_l': 150, + 'arm_flex_r': 150, + 'arm_add_r': 150, + 'arm_rot_r': 150, + 'elbow_flex_l': 150, + 'elbow_flex_r': 150, + 'pro_sup_l': 150, + 'pro_sup_r': 150} + + return coordinate_optimal_forces diff --git a/UtilsDynamicSimulations/OpenSimAD/plotsOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/plotsOpenSimAD.py index 7dc9ee05..27dfa193 100644 --- a/UtilsDynamicSimulations/OpenSimAD/plotsOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/plotsOpenSimAD.py @@ -26,33 +26,48 @@ def plotGuessVSBounds(lw, uw, w0, nJoints, N, d, guessQsEnd, guessQdsEnd, withArms=True, - withLumbarCoordinateActuators=True): + withLumbarCoordinateActuators=True, + torque_driven_model=False): # States. - # Muscle activation at mesh points. - lwp = lw['A'].to_numpy().T - uwp = uw['A'].to_numpy().T - y = w0['A'].to_numpy().T - title='Muscle activation at mesh points' - plotVSBounds(y,lwp,uwp,title) - # Muscle activation at collocation points. - lwp = lw['A'].to_numpy().T - uwp = uw['A'].to_numpy().T - y = w0['Aj'].to_numpy().T - title='Muscle activation at collocation points' - plotVSBounds(y,lwp,uwp,title) - # Muscle force at mesh points. - lwp = lw['F'].to_numpy().T - uwp = uw['F'].to_numpy().T - y = w0['F'].to_numpy().T - title='Muscle force at mesh points' - plotVSBounds(y,lwp,uwp,title) - # Muscle force at collocation points. - lwp = lw['F'].to_numpy().T - uwp = uw['F'].to_numpy().T - y = w0['Fj'].to_numpy().T - title='Muscle force at collocation points' - plotVSBounds(y,lwp,uwp,title) + if torque_driven_model: + # Coordinate activation at mesh points. + lwp = lw['CoordA'].to_numpy().T + uwp = uw['CoordA'].to_numpy().T + y = w0['CoordA'].to_numpy().T + title='Coordinate activation at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Coordinate activation at collocation points. + lwp = lw['CoordA'].to_numpy().T + uwp = uw['CoordA'].to_numpy().T + y = w0['CoordAj'].to_numpy().T + title='Coordinate activation at collocation points' + plotVSBounds(y,lwp,uwp,title) + else: + # Muscle activation at mesh points. + lwp = lw['A'].to_numpy().T + uwp = uw['A'].to_numpy().T + y = w0['A'].to_numpy().T + title='Muscle activation at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Muscle activation at collocation points. + lwp = lw['A'].to_numpy().T + uwp = uw['A'].to_numpy().T + y = w0['Aj'].to_numpy().T + title='Muscle activation at collocation points' + plotVSBounds(y,lwp,uwp,title) + # Muscle force at mesh points. + lwp = lw['F'].to_numpy().T + uwp = uw['F'].to_numpy().T + y = w0['F'].to_numpy().T + title='Muscle force at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Muscle force at collocation points. + lwp = lw['F'].to_numpy().T + uwp = uw['F'].to_numpy().T + y = w0['Fj'].to_numpy().T + title='Muscle force at collocation points' + plotVSBounds(y,lwp,uwp,title) # Joint position at mesh points. lwp = np.reshape( lw['Qsk'], (nJoints, N+1), order='F') @@ -108,12 +123,20 @@ def plotGuessVSBounds(lw, uw, w0, nJoints, N, d, guessQsEnd, title='Lumbar activation at collocation points' plotVSBounds(y,lwp,uwp,title) # Controls. - # Muscle activation derivative at mesh points. - lwp = lw['ADt'].to_numpy().T - uwp = uw['ADt'].to_numpy().T - y = w0['ADt'].to_numpy().T - title='Muscle activation derivative at mesh points' - plotVSBounds(y,lwp,uwp,title) + if torque_driven_model: + # Coordinate excitation at mesh points. + lwp = lw['CoordE'].to_numpy().T + uwp = uw['CoordE'].to_numpy().T + y = w0['CoordE'].to_numpy().T + title='Coordinate excitation at mesh points' + plotVSBounds(y,lwp,uwp,title) + else: + # Muscle activation derivative at mesh points. + lwp = lw['ADt'].to_numpy().T + uwp = uw['ADt'].to_numpy().T + y = w0['ADt'].to_numpy().T + title='Muscle activation derivative at mesh points' + plotVSBounds(y,lwp,uwp,title) if withArms: # Arm excitation at mesh points. lwp = lw['ArmE'].to_numpy().T @@ -127,13 +150,14 @@ def plotGuessVSBounds(lw, uw, w0, nJoints, N, d, guessQsEnd, uwp = uw['LumbarE'].to_numpy().T y = w0['LumbarE'].to_numpy().T title='Lumbar excitation at mesh points' - plotVSBounds(y,lwp,uwp,title) - # Muscle force derivative at mesh points. - lwp = lw['FDt'].to_numpy().T - uwp = uw['FDt'].to_numpy().T - y = w0['FDt'].to_numpy().T - title='Muscle force derivative at mesh points' - plotVSBounds(y,lwp,uwp,title) + plotVSBounds(y,lwp,uwp,title) + if not torque_driven_model: + # Muscle force derivative at mesh points. + lwp = lw['FDt'].to_numpy().T + uwp = uw['FDt'].to_numpy().T + y = w0['FDt'].to_numpy().T + title='Muscle force derivative at mesh points' + plotVSBounds(y,lwp,uwp,title) # Joint velocity derivative (acceleration) at mesh points. lwp = lw['Qdds'].to_numpy().T uwp = uw['Qdds'].to_numpy().T @@ -141,32 +165,46 @@ def plotGuessVSBounds(lw, uw, w0, nJoints, N, d, guessQsEnd, title='Joint velocity derivative (acceleration) at mesh points' plotVSBounds(y,lwp,uwp,title) -def plotOptimalSolutionVSBounds(lw, uw, c_wopt): +def plotOptimalSolutionVSBounds(lw, uw, c_wopt, torque_driven_model=False): # States - # Muscle activation at mesh points - lwp = lw['A'].to_numpy().T - uwp = uw['A'].to_numpy().T - y = c_wopt['a_opt'] - title='Muscle activation at mesh points' - plotVSBounds(y,lwp,uwp,title) - # Muscle activation at collocation points - lwp = lw['A'].to_numpy().T - uwp = uw['A'].to_numpy().T - y = c_wopt['a_col_opt'] - title='Muscle activation at collocation points' - plotVSBounds(y,lwp,uwp,title) - # Muscle force at mesh points - lwp = lw['F'].to_numpy().T - uwp = uw['F'].to_numpy().T - y = c_wopt['nF_opt'] - title='Muscle force at mesh points' - plotVSBounds(y,lwp,uwp,title) - # Muscle force at collocation points - lwp = lw['F'].to_numpy().T - uwp = uw['F'].to_numpy().T - y = c_wopt['nF_col_opt'] - title='Muscle force at collocation points' - plotVSBounds(y,lwp,uwp,title) + if torque_driven_model: + # Coordinate activation at mesh points + lwp = lw['CoordA'].to_numpy().T + uwp = uw['CoordA'].to_numpy().T + y = c_wopt['aCoord_opt'] + title='Coordinate activation at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Coordinate activation at collocation points + lwp = lw['CoordA'].to_numpy().T + uwp = uw['CoordA'].to_numpy().T + y = c_wopt['aCoord_col_opt'] + title='Coordinate activation at collocation points' + plotVSBounds(y,lwp,uwp,title) + else: + # Muscle activation at mesh points + lwp = lw['A'].to_numpy().T + uwp = uw['A'].to_numpy().T + y = c_wopt['a_opt'] + title='Muscle activation at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Muscle activation at collocation points + lwp = lw['A'].to_numpy().T + uwp = uw['A'].to_numpy().T + y = c_wopt['a_col_opt'] + title='Muscle activation at collocation points' + plotVSBounds(y,lwp,uwp,title) + # Muscle force at mesh points + lwp = lw['F'].to_numpy().T + uwp = uw['F'].to_numpy().T + y = c_wopt['nF_opt'] + title='Muscle force at mesh points' + plotVSBounds(y,lwp,uwp,title) + # Muscle force at collocation points + lwp = lw['F'].to_numpy().T + uwp = uw['F'].to_numpy().T + y = c_wopt['nF_col_opt'] + title='Muscle force at collocation points' + plotVSBounds(y,lwp,uwp,title) # Joint position at mesh points lwp = lw['Qs'].to_numpy().T uwp = uw['Qs'].to_numpy().T @@ -192,22 +230,31 @@ def plotOptimalSolutionVSBounds(lw, uw, c_wopt): title='Joint velocity at collocation points' plotVSBounds(y,lwp,uwp,title) # Controls - # Muscle activation derivative at mesh points - lwp = lw['ADt'].to_numpy().T - uwp = uw['ADt'].to_numpy().T - y = c_wopt['aDt_opt'] - title='Muscle activation derivative at mesh points' - plotVSBounds(y,lwp,uwp,title) + if torque_driven_model: + # Muscle activation derivative at mesh points + lwp = lw['CoordE'].to_numpy().T + uwp = uw['CoordE'].to_numpy().T + y = c_wopt['eCoord_opt'] + title='Coordinate excitation at mesh points' + plotVSBounds(y,lwp,uwp,title) + else: + # Muscle activation derivative at mesh points + lwp = lw['ADt'].to_numpy().T + uwp = uw['ADt'].to_numpy().T + y = c_wopt['aDt_opt'] + title='Muscle activation derivative at mesh points' + plotVSBounds(y,lwp,uwp,title) # Slack controls - # Muscle force derivative at collocation points - lwp = lw['FDt'].to_numpy().T - uwp = uw['FDt'].to_numpy().T - y = c_wopt['nFDt_col_opt'] - title='Muscle force derivative at collocation points' - plotVSBounds(y,lwp,uwp,title) + if not torque_driven_model: + # Muscle force derivative at collocation points + lwp = lw['FDt'].to_numpy().T + uwp = uw['FDt'].to_numpy().T + y = c_wopt['nFDt_opt'] + title='Muscle force derivative at collocation points' + plotVSBounds(y,lwp,uwp,title) # Joint velocity derivative (acceleration) at collocation points lwp = lw['Qdds'].to_numpy().T uwp = uw['Qdds'].to_numpy().T - y = c_wopt['Qdds_col_opt'] + y = c_wopt['Qdds_opt'] title='Joint velocity derivative (acceleration) at collocation points' plotVSBounds(y,lwp,uwp,title) \ No newline at end of file diff --git a/UtilsDynamicSimulations/OpenSimAD/settingsOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/settingsOpenSimAD.py index f963b865..df21bd17 100644 --- a/UtilsDynamicSimulations/OpenSimAD/settingsOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/settingsOpenSimAD.py @@ -139,9 +139,66 @@ def get_setup(motion_type): 'cutoff_freq_Qdds': 12, 'splineQds': True, 'meshDensity': 100, + 'yCalcnToes': True} + + setups['running_torque_driven'] = { + 'ipopt_tolerance': 3, + 'weights': { + 'positionTrackingTerm': 100, + 'velocityTrackingTerm': 10, + 'accelerationTrackingTerm': 50, + 'armExcitationTerm': 0.001, + 'lumbarExcitationTerm': 0.001, + 'jointAccelerationTerm': 0.001, + 'coordinateExcitationTerm': 10}, + 'coordinates_toTrack': { + 'pelvis_tilt': {"weight": 10}, + 'pelvis_list': {"weight": 10}, + 'pelvis_rotation': {"weight": 10}, + 'pelvis_tx': {"weight": 10}, + 'pelvis_ty': {"weight": 10}, + 'pelvis_tz': {"weight": 10}, + 'hip_flexion_l': {"weight": 20}, + 'hip_adduction_l': {"weight": 10}, + 'hip_rotation_l': {"weight": 1}, + 'hip_flexion_r': {"weight": 20}, + 'hip_adduction_r': {"weight": 10}, + 'hip_rotation_r': {"weight": 1}, + 'knee_angle_l': {"weight": 10}, + 'knee_angle_r': {"weight": 10}, + 'ankle_angle_l': {"weight": 10}, + 'ankle_angle_r': {"weight": 10}, + 'subtalar_angle_l': {"weight": 10}, + 'subtalar_angle_r': {"weight": 10}, + 'lumbar_extension': {"weight": 10}, + 'lumbar_bending': {"weight": 10}, + 'lumbar_rotation': {"weight": 10}, + 'arm_flex_l': {"weight": 10}, + 'arm_add_l': {"weight": 10}, + 'arm_rot_l': {"weight": 10}, + 'arm_flex_r': {"weight": 10}, + 'arm_add_r': {"weight": 10}, + 'arm_rot_r': {"weight": 10}, + 'elbow_flex_l': {"weight": 10}, + 'elbow_flex_r': {"weight": 10}, + 'pro_sup_l': {"weight": 10}, + 'pro_sup_r': {"weight": 10}}, + 'coordinate_constraints': { + 'pelvis_tx': {"env_bound": 0.1}}, + 'ignorePassiveFiberForce': True, + 'filter_Qs_toTrack': True, + 'cutoff_freq_Qs': 12, + 'filter_Qds_toTrack': True, + 'cutoff_freq_Qds': 12, + 'filter_Qdds_toTrack': True, + 'cutoff_freq_Qdds': 12, + 'splineQds': True, + 'meshDensity': 100, 'yCalcnToes': True, - - } + 'torque_driven_model': True, + 'coordinate_optimal_forces': { + 'hip_flexion_r': 400, + 'hip_flexion_l': 400}} setups['walking'] = { 'ipopt_tolerance': 3, @@ -195,7 +252,6 @@ def get_setup(motion_type): 'cutoff_freq_Qs': 6, 'meshDensity': 100} - setups['drop_jump'] = { 'weights': { 'positionTrackingTerm': 50, diff --git a/UtilsDynamicSimulations/OpenSimAD/utilsOpenSimAD.py b/UtilsDynamicSimulations/OpenSimAD/utilsOpenSimAD.py index 7a581062..09c9d0c9 100644 --- a/UtilsDynamicSimulations/OpenSimAD/utilsOpenSimAD.py +++ b/UtilsDynamicSimulations/OpenSimAD/utilsOpenSimAD.py @@ -1950,8 +1950,9 @@ def plotResultsDC(dataDir, subject, motion_filename, settings, if i < NMuscles: color=iter(plt.cm.rainbow(np.linspace(0,1,len(cases)))) for c, case in enumerate(cases): - ax.plot(optimaltrajectories[case]['time'][0,:-1].T, - optimaltrajectories[case]['muscle_activations'][i:i+1,:-1].T, c=next(color), label='video-based DC ' + cases[c]) + if 'muscle_activations' in optimaltrajectories[case]: + ax.plot(optimaltrajectories[case]['time'][0,:-1].T, + optimaltrajectories[case]['muscle_activations'][i:i+1,:-1].T, c=next(color), label='video-based DC ' + cases[c]) ax.set_title(muscles[i]) ax.set_ylim((0,1)) handles, labels = ax.get_legend_handles_labels() diff --git a/example_kinetics.py b/example_kinetics.py index 43ae8168..427df7e8 100644 --- a/example_kinetics.py +++ b/example_kinetics.py @@ -142,12 +142,16 @@ # Options are 'walk_1_25ms', 'run_2_5ms', and 'run_4ms'. elif session_type == 'treadmill': trial_name = 'walk_1_25ms' + torque_driven_model = False # Example with torque-driven model. if trial_name == 'walk_1_25ms': # Walking, 1.25 m/s motion_type = 'walking' time_window = [1.0, 2.5] treadmill_speed = 1.25 elif trial_name == 'run_2_5ms': # Running, 2.5 m/s - motion_type = 'running' + if torque_driven_model: + motion_type = 'running_torque_driven' + else: + motion_type = 'running' time_window = [1.4, 2.6] treadmill_speed = 2.5 elif trial_name == 'run_4ms': # Running with periodic constraints, 4.0 m/s