% Lever spring Muscle Model % make this into a function that take in something about input lever and % outputs something so that you can minimize the motion. function [maxForce,maxVel] = buildAndRunModel3(x0,ofl,mass,goalNfl) global count data %% %clear %x0 = 0.003; %ofl = 0.01; % mass=2; const = 1280*0.047; maxIsometricForce = const/ofl; %goalNfl = 1.4; momentArm = x0; simulationTime = 0.1; excitation = [0,simulationTime;1,1]; leverLength = 0.12; %distPivotFromCenter = x0;%is this negative to the right of center? is this not supposed to be just one number? leverWidth = 0.04; % and height distPivotFromCenter = leverLength/2; stepsize = 0.0005; outputLever = leverLength-distPivotFromCenter; pennatationAngle =0; if momentArmleverWidth mtuLever = sqrt((leverLength/2)^2+(-momentArm+(leverWidth/2))^2); mtuWrap = pi*momentArm + sqrt((momentArm-leverWidth/3)^2 + (1.1*momentArm)^2); %+ (momentArm*(acos((momentArm)/(leverWidth/2)))); else mtuLever = leverLength/2-momentArm*1.1; mtuWrap = pi*momentArm; end mtuStand = (heightFromGround-1.1*momentArm); %sqrt((momentArm^2)-(leverWidth/2)^2)); mtuLength = mtuLever+mtuWrap+mtuStand; %ofl = .005; optimalFiberLength = ofl; pennationAngle = 0; %in radians muscleLength = optimalFiberLength*cos(pennationAngle); % if count==1 tendonSlackLength = mtuLength-optimalFiberLength; % else % tendonSlackLength= data(count-1).tendonSlackLength; % end % % Pull in the modeling classes straight from the OpenSim distribution import org.opensim.modeling.* %/////////////////////////////////////////// %// DEFINE BODIES AND JOINTS OF THE MODEL // %/////////////////////////////////////////// % Create a blank model model = Model(); model.setName('LeverSpringMuscle') model.setUseVisualizer(false); % Convenience - create a zero value Vec3 zeroVec3 = ArrayDouble.createVec3(0); % Set gravity as 0 since there is no ground contact model in this version % of the example %model.setGravity(Vec3(0, -9.80665, 0)); model.setGravity(Vec3(0, 0, 0)); % GROUND BODY % Get a reference to the model's ground body ground = model.getGround(); % "BLOCK" BODY % Create a block Body with associate dimensions, mass properties, and DisplayGeometry Izz = 1/12*leverMass*((leverLength)^2+(leverWidth)^2); Iyy = 1/12*leverMass*(leverLength^2+leverWidth^2); Ixx = 1/12*leverMass*(2*leverWidth^2); leverInertia = Inertia(Vec3(Ixx,Iyy,Izz)); lever = Body('lever', leverMass, Vec3(0,0,0), leverInertia); % check here - bricks defined by 1/2? leverGeometry = Brick(Vec3(1/2*leverLength, 1/2*leverWidth,1/2*leverWidth)); leverGeometry.setColor(Vec3(0.8, 0.1, 0.1)); lever.attachGeometry(leverGeometry); model.addBody(lever) %create another body with different masses on the end of the lever massRadius = (mass/(997*pi))^(1/3); massIntertia = Inertia(Vec3(2/5*mass*massRadius^2,2/5*mass*massRadius^2,2/5*mass*massRadius^2)); addedMass = Body('addedMass',mass,Vec3(0,0,0),massIntertia); addedMassGeometry = Sphere(massRadius); addedMassGeometry.setColor(Vec3(0.5,0.5,0.1)); addedMass.attachGeometry(addedMassGeometry); model.addBody(addedMass); %Weld joint between added mass and lever massLeverOffset = Vec3(-leverLength/2,leverWidth/2+(massRadius),0); massOffsetCenter = Vec3(0,0,0); % position from the center of the lever to the joint weldMassOffset = PhysicalOffsetFrame('weld_mass_offset', addedMass, ... Transform(massOffsetCenter)); weldLeverOffset = PhysicalOffsetFrame('weld_lever_offset', lever, ... Transform(massLeverOffset)); weld = WeldJoint('weld', weldMassOffset, weldLeverOffset); weld.addComponent(weldMassOffset); weld.addComponent(weldLeverOffset); model.addJoint(weld); %add a stand for the lever for visualization standInertia = Inertia(Vec3(1,1,1)); standVertical = Body('standVertical',mass,Vec3(0,0,0),standInertia); standVerticalGeometry = Brick(Vec3(standWidth,(heightFromGround)/2,standWidth)); standVertical.attachGeometry(standVerticalGeometry); model.addBody(standVertical) %Weld joint between stand and ground standVOffsetCenter = Vec3(0,-heightFromGround/2,0); % position from the center of the lever to the joint standVGroundOffset = PhysicalOffsetFrame('standV_offset_center', standVertical, ... Transform(standVOffsetCenter)); weldStandV = WeldJoint('weldStandV', ground, standVGroundOffset); weldStandV.addComponent(standVGroundOffset); model.addJoint(weldStandV); %add a pin for the pin joint % pinInertia = Inertia(Vec3(1,1,1)); % half_length = 2*leverWidth; % half_width = 0.001; % standPin = Body('standPin',mass,Vec3(0,0,0),pinInertia); % pinGeometry = Brick(Vec3(half_width, half_width,half_length)); % standPin.attachGeometry(pinGeometry); % model.addBody(standPin) % % %Weld joint between pin and stand % pinOffsetCenter = Vec3(0,-((heightFromGround)/2)+0.01,-half_length); % position from the center of the stand to the joint % pinOffsetCenter = PhysicalOffsetFrame('pinOffsetCenter', standPin, ... % Transform(pinOffsetCenter)); % weldPinStand = WeldJoint('weldPinStand', standVertical,pinOffsetCenter); % weldPinStand.addComponent(pinOffsetCenter); % model.addJoint(weldPinStand); % Lever Pin Joint leverGroundOffset = Vec3(0,heightFromGround,0); jointOffsetCenter = Vec3(distPivotFromCenter,0,0); % position from the center of the lever to the joint pivotLeverOffset = PhysicalOffsetFrame('pivot_lever_offset', lever, ... Transform(jointOffsetCenter)); pivotGroundOffset = PhysicalOffsetFrame('pivot_ground_offset', model.getGround(), ... Transform(leverGroundOffset)); pivot = PinJoint('pivot', pivotGroundOffset, pivotLeverOffset); pivot.addComponent(pivotLeverOffset); pivot.addComponent(pivotGroundOffset); model.addJoint(pivot); pivotCoord = pivot.upd_coordinates(0); pivotCoord.setName('pivotFlexion'); pivotRange = [100., -70.]; pivotStiff = [20., 20.]; pivotDamping = 5.; pivotTransition = 2.5; pivotLimitForce = CoordinateLimitForce('pivotFlexion', pivotRange(1), ... pivotStiff(1), pivotRange(2), pivotStiff(2), pivotDamping, pivotTransition); pivotLimitForce.setName('pivotLigaments'); pivot.addComponent(pivotLimitForce); %add muscle muscle1 = Millard2012EquilibriumMuscle(); muscle1.setName('muscle1') muscle1.setMaxIsometricForce(maxIsometricForce) muscle1.setOptimalFiberLength(optimalFiberLength) muscle1.setTendonSlackLength(tendonSlackLength); muscle1.setPennationAngleAtOptimalFiberLength(pennationAngle); muscle1.set_ignore_tendon_compliance(true); muscle1.upd_ignore_tendon_compliance(); muscle1.addNewPathPoint('insertion', lever, Vec3(0,leverPoint,0)); muscle1.addNewPathPoint('pivot', standVertical, Vec3(momentArm,(heightFromGround/2)+(.0001), 0)); muscle1.addNewPathPoint('mid1', standVertical, Vec3(standWidth,(heightFromGround/2)-(1.5*momentArm), 0)); muscle1.addNewPathPoint('origin', standVertical, Vec3(standWidth,-heightFromGround/2, 0)); model.addForce(muscle1); %thelen Muscle %muscle1.set_FmaxTendonStrain(0.000001); %muscle1.upd_FmaxTendonStrain() %MillardMuscle % tendonFLCurve = muscle1.getTendonForceLengthCurve();%diff % tendonFLCurve.setStrainAtOneNormForce(0.00001)%diff % add a wraping surface patellaFrame = PhysicalOffsetFrame('patellaFrame', lever, ... Transform(jointOffsetCenter)); patella = WrapCylinder(); patella.setName('patella'); patella.set_radius(momentArm); patella.set_length(leverWidth*4); patella.set_quadrant('y'); patella.set_xyz_body_rotation(Vec3(0, 0, -1)) patellaFrame.addWrapObject(patella); lever.addComponent(patellaFrame); muscle1.updGeometryPath().addPathWrap(patella); %add a brain % Create a controller to excite the muscle. brain = PrescribedController(); brain.setActuators(model.updActuators()); controlFunction = PiecewiseLinearFunction(); %excitation (1,i) = time array; %excitation (2,i) = excitation values array for i = 1:size(excitation,2) controlFunction.addPoint(excitation(1,i), excitation(2,i)); end brain.prescribeControlForActuator('muscle1', controlFunction); model.addController(brain); % Set bounds on coordinates % maxAngle = asin(heightFromGround/outputLever); % if heightFromGroundheightFromGround - otherwise min=0; % angleRange = [minAngle,maxAngle]; % pivot.upd_coordinates(0).setRange(angleRange); pivotCoord.setDefaultValue((4/9)*pi); %///floor\\\ % floor = ContactHalfSpace(Vec3(0,heightFromGround-leverWidth/2,0), Vec3(0, 0, -pi/2), model.getGround(), ... % 'floor'); % footRadius = leverWidth/2; % contactLeverOffset = Vec3(-leverLength/2,0,0); % foot = ContactSphere(footRadius, contactLeverOffset, lever, 'foot'); % % stiffness = 1.e8; dissipation = .9; friction = [0.9, 0.9, 0.6]; % contactForce = HuntCrossleyForce(); % contactForce.setStiffness(stiffness); % contactForce.setDissipation(dissipation); % contactForce.setStaticFriction(friction(1)); % contactForce.setDynamicFriction(friction(2)); % contactForce.setViscousFriction(friction(3)); % contactForce.addGeometry('floor'); % contactForce.addGeometry('foot'); % model.addContactGeometry(floor); % model.addContactGeometry(foot); % model.addForce(contactForce); %\\\floor/// % Simulate % need to set the initial position % set reporters % This is what it ourputs!!!! % % massVel = TableReporterVec3(); massVel.setName('massVelocity'); massVel.set_report_time_interval(stepsize/10); % Report the position of the origin of the body. %massVel.addToReport(addedMass.getOutput('linear_velocity'),'mVelocity'); %massVel.addToReport(addedMass.getOutput('linear_acceleration'),'mAccel'); massVel.addToReport(addedMass.getOutput('angular_acceleration'),'lAngAccel'); massVel.addToReport(addedMass.getOutput('angular_velocity'),'lAngVelocity'); model.addComponent(massVel); % % muscleReporter= TableReporter(); muscleReporter.setName('muscleReporter'); muscleReporter.set_report_time_interval(stepsize/10); muscleReporter.addToReport(muscle1.getOutput('fiber_force_along_tendon'),'activeForce'); muscleReporter.addToReport(muscle1.getOutput('fiber_length'),'fiberLen'); %muscleReporter.addToReport(muscle1.getOutput('tendon_force'),'tendonForce'); muscleReporter.addToReport(muscle1.getOutput('normalized_fiber_length'),'nFiberLen'); muscleReporter.addToReport(muscle1.getOutput('muscle_power'),'mPower'); muscleReporter.addToReport(muscle1.getOutput('fiber_velocity_along_tendon'),'mVelocity'); %muscleReporter.addToReport(muscle1.getOutput('activation'),'activation'); muscleReporter.addToReport(pivot.getComponent('pivotFlexion').getOutput('value'),'pivotAngle'); model.addComponent(muscleReporter); state = model.initSystem(); pivotFlexCoord = model.updCoordinateSet().get('pivotFlexion'); angleRange = [-(7/18)*pi:pi/20:(4/9)*pi]; for ang = 1:length(angleRange) pivotFlexCoord.setValue(state,angleRange(ang)); ma(ang)=muscle1.computeMomentArm(state,pivotCoord); end maMean = mean(ma); maSd = std(ma); pivotFlexCoord.setValue(state,(4/9)*pi); try muscle1.computeInitialFiberEquilibrium(state); catch tendonSlackLength = tendonSlackLength-optimalFiberLength*0.5; end nOFL = muscle1.getNormalizedFiberLength(state); diffnFL = ofl*(goalNfl-nOFL); while abs(goalNfl-nOFL)>0.005 muscle1.setTendonSlackLength(tendonSlackLength-diffnFL); muscle1.computeInitialFiberEquilibrium(state); nOFL = muscle1.getNormalizedFiberLength(state); if nOFL>goalNfl+0.4 diffnFL = diffnFL - 0.01; elseif nOFL>goalNfl+0.2 diffnFL = diffnFL - 0.005; elseif nOFL>goalNfl+0.1 diffnFL = diffnFL -0.0005; elseif nOFL>goalNfl+0.025 diffnFL = diffnFL -0.0001; elseif nOFL>goalNfl-0.1 && nOFLgoalNfl-0.3 &&nOFL-70 if rad2deg(angle)<50 addTime = simulationTime*1.1; else addTime = 0.1; end dcount=1; a=0; while abs(a-angle)>deg2rad(5) a=angle; simulationTime = simulationTime+addTime; state = manager.integrate(simulationTime); coord = pivot.get_coordinates(0); concreteOutput = OutputDouble.safeDownCast(coord.getOutput('value')); angle = concreteOutput.getValue(state); dcount=dcount+1; end end %simulatedAtLeastOnce = true; % Process Data % Process reporter tables. % ------------------------ % This puts the output in a matlab struct VelTable = massVel.getTable(); % in radians VelStruct = osimTableToStruct(VelTable); ForceTable = muscleReporter.getTable(); % in radians ForceStruct = osimTableToStruct(ForceTable); model.disownAllComponents(); %% %kinematics of lever/mass leverAngVel = VelStruct.lAngVelocity(:,3); negLeverAngVel = -(leverAngVel); [~,idx]= max([negLeverAngVel]); idx70 = find([-ForceStruct.pivotAngle]>=1.22,1); if isempty(idx70) idx70 = length(ForceStruct.pivotAngle); end %Trim idx idx = min(idx,idx70); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %trimmed kinematic values - arrays cutVel = negLeverAngVel(1:idx,:); leverAngAccel = VelStruct.lAngAccel(1:idx,3); negLeverAngAccel = -leverAngAccel; cutLeverAngAccel = negLeverAngAccel(1:idx); angle = ForceStruct.pivotAngle(1:idx); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %Muscle info arrays - trimmed musclePower = ForceStruct.mPower(1:idx); mVelocity = -ForceStruct.mVelocity(1:idx); cutNFiberLen = ForceStruct.nFiberLen(1:idx,:); fiberLength = cutNFiberLen * ofl; cutActiveForce = ForceStruct.activeForce(1:idx,:); cutMusclePower = musclePower(1:idx,:); %calculate the torque acting on the mass d = sqrt(((leverLength/2)+distPivotFromCenter)^2 + ((leverWidth/2)+massRadius)^2); % can make more accurate Im = 2/5*mass*massRadius^2; %the inertia of the object about it's center of mass MassIpivot = Im+mass*d^2;%moment of inertia? % in radians torqueM = cutLeverAngAccel*MassIpivot; %force acting to accelerate the mass massForce = torqueM/0.12; %output lever length %force acting to rotate the lever LeverIpivot = Iyy + leverMass*distPivotFromCenter^2;%Lever moment of inertia torqueL = cutLeverAngAccel * LeverIpivot; leverForce = torqueL/(0.12/2);%Force to Accelerate the lever - acting at it's center of mass forceEndLever = massForce;%modify this to use torque-t=rF F = t/r outputPower = (torqueL+torqueM).* cutVel;%we need to change velocity so it is from torque I think %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Scaler values % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %kinematics maxVel = max(negLeverAngVel); timeToMaxVel = VelStruct.time(idx); maxAccel = max(cutLeverAngAccel); % forces maxLeverForce = max(leverForce); maxOutputForce = max(forceEndLever); %mass force maxOutputPower = max(outputPower); totalOutputPower = sum(outputPower); impulse = sum(massForce); %outputImpulse maxMassForce = max(massForce); %max values muscle muscImpulse = sum(cutActiveForce); muscVel = max(mVelocity); InormalizedFiberLen = ForceStruct.nFiberLen(1); changeInDist = fiberLength(1)-fiberLength(end); totalMuscleWork = sum(cutActiveForce) * changeInDist; totalMusclePower = sum(cutMusclePower); maxMusclePower=max(cutMusclePower); powerRatio = totalOutputPower/totalMusclePower; maxMuscleForce = max(cutActiveForce); %% %clearvars -except ForceStruct VelStruct momentArm count %model info data(count).maMean = maMean; data(count).maSd = maSd; data(count).tendonSlackLength = tendonSlackLength; data(count).momentArm = momentArm; data(count).mass = mass; data(count).ofl = optimalFiberLength; data(count).initFiberLength = InormalizedFiberLen; %output values data(count).maxVel = maxVel; data(count).timeToMaxVel = timeToMaxVel; data(count).maxAccel = maxAccel; data(count).maxLeverForce = maxLeverForce; data(count).maxOutputForce =maxOutputForce; data(count).maxOutputPower = maxOutputPower; data(count).totalOutputPower = totalOutputPower; data(count).outputImpulse =impulse; %outputImpulse data(count).maxMassForce =maxMassForce; data(count).maxKe = 0.5*mass*maxVel^2; data(count).angleMaxVel = rad2deg(-angle(end)); %max values muscle data(count).maxMuscleForce =maxMuscleForce; data(count).muscImpulse = muscImpulse; data(count).InormalizedFiberLen = InormalizedFiberLen; data(count).totalMuscleWork = totalMuscleWork; data(count).totalMusclePower = totalMusclePower; data(count).maxMusclePower=maxMusclePower; data(count).powerRatio = powerRatio; data(count).changeInDist = changeInDist; data(count).muscVel=muscVel; %using torque just for calculating force %this count=count+1; end %want to get one number % % You'll need to do some math here to get from angle to velocity or force % (probably giving the angle every certain amount of time or something) % I must get angle and time for this to be possible. % % % plot(angleStruct.time,angleStruct.angle) % massPositionTable = massPos.getTable(); % massPosition = osimTableToStruct(massPositionTable); % % mtuTable = MtuReporter.getTable(); % mtuStruct = osimTableToStruct(mtuTable); %end %want to get one number % % You'll need to do some math here to get from angle to velocity or force % (probably giving the angle every certain amount of time or something) % I must get angle and time for this to be possible. % % % plot(angleStruct.time,angleStruct.angle) % massPositionTable = massPos.getTable(); % massPosition = osimTableToStruct(massPositionTable); % % mtuTable = MtuReporter.getTable(); % mtuStruct = osimTableToStruct(mtuTable); %end % Access the value of a concrete Output.