Hello Christopher,
I checked the simulation performance line by line.
chrisdembia wrote:You must call `initSystem()` before constructing the Manager:
Code: Select all
osimModel.initSystem();
simulationManager = Manager(osimModel);
I realized the call 'osimModel.initSystem();' made all state variables back to defaults. Even I updated state variables using osimModel.updCoordinateSet, initSystem changed them back to defaults defined in preexisting osim file.
When I call Manager() in C++ or MATLAB with OpenSim 3.3, I have never called secondary "initSystem". Why do we need to call it with OpenSim 4.0?
If we need 'InitSystem' before calling Manager(), can we call them in the beginning or earlier than any modification or updating variables?
my current MATLAB codes are attached below.
Thank you,
Hide
main_20171026.m
Code: Select all
% ----------------------------------------------------------------------- %
% main_YYYYMMDD.m
%
% Author: Hideyuki Kimpara, Worcester Polytechnic Institute
%
% This code uses the Matlab interface to the OpenSim API to run a trial
% to connect simulink simulator and OpenSim analysis.
% This example borrows from other Matlab and C++ examples avaiable from
% the OpenSim project written by several people including, but not limited
% to: Brian Umberger and OpenSim team members at Stanford University.
% ----------------------------------------------------------------------- %
clearvars
clear
% Node and timing information
System_StartTim = 0.00;
System_End_Time = 000.30;
System_Frequency = fix(1000.);
kp = 10;
kv = 1;
if System_Frequency < 1
System_Frequency = 1.;
end
System_timestep = 1./System_Frequency;
System_Num_step = fix((System_End_Time-System_StartTim)*System_Frequency);
% Import the OpenSim modeling classes
import org.opensim.modeling.*
% Create an OpenSim model from an OSIM file
Model_In = ['TwoLinkArmModel.osim'];
osimModel = Model(Model_In);
output_Dir = 'Result';
% Prepare temporary export model file for debug
fileoutpath = [Model_In(1:end-5),'_Monitor_01.osim'];
% Add Analyses to the model: ForceReporter and BodyKinematics
aActuation = Actuation(osimModel);
aBodyKinematics = BodyKinematics(osimModel);
aForceReporter = ForceReporter(osimModel);
aJointReaction = JointReaction(osimModel);
aKinematics = Kinematics(osimModel);
aStatesReporter = StatesReporter(osimModel);
osimModel.addAnalysis(aActuation);
osimModel.addAnalysis(aBodyKinematics);
osimModel.addAnalysis(aForceReporter);
osimModel.addAnalysis(aJointReaction);
osimModel.addAnalysis(aKinematics);
osimModel.addAnalysis(aStatesReporter);
% Initialize the model (build the system and intialize the state)
osimState = osimModel.initSystem();
% Get the number of states from the model;
% in this case the number of controls equals the number of muscles
Num_Cent_Cnt = 2;
CentStruct = struct();
CentStruct.Number = Num_Cent_Cnt;
CentStruct.Cent_val_name = cell(1, Num_Cent_Cnt);
CentStruct.Cent_cor_name = cell(1, Num_Cent_Cnt);
CentStruct.Cent_act_name = cell(1, Num_Cent_Cnt);
CentStruct.ValAd = zeros(1, Num_Cent_Cnt);
CentStruct.Q_des = zeros(1, Num_Cent_Cnt);
CentStruct.U_des = zeros(1, Num_Cent_Cnt);
CentStruct.Q_ini = zeros(1, Num_Cent_Cnt);
CentStruct.U_ini = zeros(1, Num_Cent_Cnt);
CentStruct.ContV = zeros(1, Num_Cent_Cnt);
CentStruct.kp = kp * ones(1, Num_Cent_Cnt);
CentStruct.kv = kv * ones(1, Num_Cent_Cnt);
CentStruct.Cent_cor_name(1,1) = cell({'joint_1'});
CentStruct.Cent_cor_name(1,2) = cell({'joint_2'});
CentStruct.Cent_act_name(1,1) = cell({'joint_1_actuator'});
CentStruct.Cent_act_name(1,2) = cell({'joint_2_actuator'});
CentStruct.Q_des(1,1) = 80. * pi / 180.;
CentStruct.U_des(1,1) = 0.0 * pi / 180.;
CentStruct.Q_des(1,2) = -80. * pi / 180.;
CentStruct.U_des(1,2) = 0.0 * pi / 180.;
Control_Func.time = zeros(1, System_Num_step*2+1);
Control_Func.data = zeros(Num_Cent_Cnt, System_Num_step*2+1);
Control_Func.name = CentStruct.Cent_act_name;
Control_Func.time(1) = System_StartTim;
for i=1:System_Num_step
Control_Func.time(i*2) = System_StartTim + System_timestep * (i-1) + System_timestep*0.01;
Control_Func.time(i*2+1) = System_StartTim + System_timestep * (i);
end
for i=1:Num_Cent_Cnt
Coord = osimModel.getCoordinateSet().get(CentStruct.Cent_cor_name(1,i));
CentStruct.Q_ini(1,i) = Coord.getValue(osimState);
CentStruct.U_ini(1,i) = Coord.getSpeedValue(osimState);
Coord.setValue(osimState, 0.0);
Coord.setSpeedValue(osimState, 0.0);
end
% If I need to change initial joint angles, input values here.
CentStruct.Q_ini(1,1) = 90. * pi / 180.;
CentStruct.Q_ini(1,2) = 60. * pi / 180.;
% Solve difference of variable orders between osimModel and osimState.
osimModel.realizeVelocity(osimState);
for i=1:Num_Cent_Cnt
Coord = osimModel.getCoordinateSet().get(CentStruct.Cent_cor_name(1,i));
Coord.setValue(osimState, 1.);
Coord.setSpeedValue(osimState, -8888.8);
osimModel.realizeVelocity(osimState);
tempQ = osimState.getQ;
tempU = osimState.getU;
for j=1:size(tempQ)
if tempU.get(j-1) == -8888.8
CentStruct.ValAd(1,i) = j;
break;
end
end
Coord.setValue(osimState, 0.0);
Coord.setSpeedValue(osimState, 0.0);
osimState.getQ
end
% Return initial state variables into osimModel.
for i=1:Num_Cent_Cnt
Coord = osimModel.getCoordinateSet().get(CentStruct.Cent_cor_name(1,i));
Coord.setValue ( osimState, CentStruct.Q_ini(1,i) );
Coord.setSpeedValue ( osimState, CentStruct.U_ini(1,i) );
end
%# Set the initial states of the model
editableCoordSet = osimModel.updCoordinateSet();
% Arrange the initial guess by nodes and states
for i=1:Num_Cent_Cnt
editableCoordSet.get(CentStruct.Cent_cor_name(1,i)).setValue(osimState, CentStruct.Q_ini(1,i));
editableCoordSet.get(CentStruct.Cent_cor_name(1,i)).setSpeedValue(osimState, CentStruct.U_ini(1,i));
end
JointSet = osimModel.getJointSet();
CoordSet = osimModel.getCoordinateSet();
StateValNames = osimModel.getStateVariableNames();
InitStruct = struct();
InitStruct.JointSet = cell(JointSet.getSize,1);
InitStruct.CoordSet = cell(CoordSet.getSize,1);
InitStruct.StateLabl = cell(StateValNames.getSize,1);
InitStruct.StateVals = zeros(StateValNames.getSize,1);
for i = 1:StateValNames.getSize
InitStruct.StateLabl(i,1) = cell(StateValNames.getitem(i-1));
SpString = split(InitStruct.StateLabl(i,1),'/');
JointName = char(SpString(1));
CoordName = char(SpString(2));
ValueName = char(SpString(3));
Coord = osimModel.getCoordinateSet().get(CoordName);
if strcmp(ValueName, 'value')
InitStruct.StateVals(i,1) = Coord.getValue(osimState);
else
InitStruct.StateVals(i,1) = Coord.getSpeedValue(osimState);
end
end
for i = 1:CoordSet.getSize
InitStruct.CoordSet(i,1) = cell(CoordSet.get(i-1));
end
for i = 1:JointSet.getSize
InitStruct.JointSet(i,1) = cell(JointSet.get(i-1));
end
% Parameters to be passed in to the forward function
params.model = osimModel;
params.state = osimState;
params.CentSt = CentStruct;
params.Control = Control_Func;
stateStorage = aStatesReporter.getStatesStorage();
% start a timer
tic;
for i = 1:System_Num_step
time_start = System_StartTim + System_timestep * (i-1);
time_end = System_StartTim + System_timestep * (i);
disp(['Cycle [' int2str(i) ']/[' int2str(System_Num_step) '] : Calculating from ' num2str(time_start) ' to ' num2str(time_end)]);
ForwardOsimFunction_171026(time_start,time_end,params,InitStruct);
% Use last state variables for next step initial states
AA = stateStorage.getColumnLabels;
AB = stateStorage.getLastStateVector().getData;
for k=1:AB.getSize
for j = 1:size(InitStruct.StateLabl,1)
if (strcmp(InitStruct.StateLabl(j),AA.getitem(k)))
InitStruct.StateVals(j) = AB.getitem(k-1);
end
end
end
disp(['state val [' num2str(AB.get(0)/pi*180.) '], [' num2str(AB.get(2)/pi*180.) '] ' ]);
disp('');
clear AA AB;
end
% stop the timer
runtime = toc;
disp(['The runtime was ' num2str(runtime)]);
% Write out storage files containing the states and excitations
if isdir (output_Dir) == 0
mkdir (output_Dir);
end
outputpath = ['.\' char(output_Dir) '\']; % use current directory
bufferstamp = char(datetime,'yy_MM_dd-HH_mm');
osimModel.printControlStorage(['SimOut_',bufferstamp,'_excitations.sto']);
% Also write out the results of the analyses to storage files
aActuation.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aBodyKinematics.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aForceReporter.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aJointReaction.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aKinematics.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aStatesReporter.printResults(['SimOut_',bufferstamp],outputpath,0.01,'.sto');
aStatesReporter.printResults(['SimOut_',bufferstamp],'.\.',-1,'.sto');
% Reset the analyses so everything starts fresh for the next
% call to this function
aActuation.getForceStorage.reset(0);
aActuation.getPowerStorage.reset(0);
aActuation.getSpeedStorage.reset(0);
aBodyKinematics.getPositionStorage.reset(0);
aBodyKinematics.getVelocityStorage.reset(0);
aBodyKinematics.getAccelerationStorage.reset(0);
aForceReporter.getForceStorage.reset(0);
aKinematics.getAccelerationStorage.reset(0);
aKinematics.getPositionStorage.reset(0);
aKinematics.getVelocityStorage.reset(0);
aStatesReporter.getStatesStorage.reset(0);
ForwardOsimFunction_171026.m:
Code: Select all
function ForwardOsimFunction_171026(time_start,time_end,ExtraPar,InitStruct)
% ----------------------------------------------------------------------- %
% ForwardOsimFunction_YYMMDD.m
%
% Author: Hideyuki Kimpara, Worcester Polytechnic Institute
%
% This code uses the Matlab interface to the OpenSim API to run a trial
% to connect simulink simulator and OpenSim analysis.
% This example borrows from other Matlab and C++ examples avaiable from
% the OpenSim project written by several people including, but not limited
% to: Brian Umberger and OpenSim team members at Stanford University.
% ----------------------------------------------------------------------- %
% Import the OpenSim modeling classes
import org.opensim.modeling.*
% Get reference to the model
osimModel = ExtraPar.model;
% s = ExtraPar.state;
CentStruct = ExtraPar.CentSt;
Control_Func = ExtraPar.Control;
% Check to see if model state is initialized by checking size
if(osimModel.getWorkingState().getNY() == 0)
s = osimModel.initSystem();
else
s = osimModel.updWorkingState();
end
%# Set the initial states of the model
editableCoordSet = osimModel.updCoordinateSet();
% Arrange the initial guess by nodes and states
for ii = 1:size(InitStruct.StateVals,1)
VariableName = char(InitStruct.StateLabl(ii));
SpString = split(InitStruct.StateLabl(ii),'/');
JointName = char(SpString(1));
CoordName = char(SpString(2));
ValueName = char(SpString(3));
if strcmp(ValueName,'speed')
temp_str=char(InitStruct.StateLabl(ii));
editableCoordSet.get(CoordName).setSpeedValue(s, InitStruct.StateVals(ii));
else
editableCoordSet.get(CoordName).setValue(s, InitStruct.StateVals(ii));
end
end
clear editableCoordSet temp_str str_addr;
% for ii=1:CentStruct.Number
% end
joint_1 = osimModel.getCoordinateSet().get(CentStruct.Cent_cor_name(1,1));
joint_2 = osimModel.getCoordinateSet().get(CentStruct.Cent_cor_name(1,2));
q1 = joint_1.getValue(s);
dq1 = joint_1.getSpeedValue(s);
q2 = joint_2.getValue(s);
dq2 = joint_2.getSpeedValue(s);
q1_des = CentStruct.Q_des(1,1);
dq1_des = CentStruct.U_des(1,1);
q2_des = CentStruct.Q_des(1,2);
dq2_des = CentStruct.U_des(1,2);
disp(['joint1 vl: [' num2str(q1/pi*180.) ']/desired[' num2str(q1_des/pi*180.) '] ']);
disp(['joint1 sp: [' num2str(dq1/pi*180.) ']/desired[' num2str(dq1_des/pi*180.) '] ']);
disp(['joint2 vl: [' num2str(q2/pi*180.) ']/desired[' num2str(q2_des/pi*180.) '] ']);
disp(['joint2 sp: [' num2str(dq2/pi*180.) ']/desired[' num2str(dq2_des/pi*180.) '] ']);
% controlLaw = Vector( 2, 0.0 );
q = zeros(1,2); % q = Vector( 2, 0.0 );
q(1) = q1; q(2) = q2; % q.set(0, q1); q.set(1, q2);
dq = zeros(1,2); % dq = Vector( 2, 0.0 );
dq(1) = dq1; dq(2) = dq2; % dq.set(0, dq1); dq.set(1, dq2);
q_des = zeros(1,2); % q_des = Vector( 2, 0.0 );
q_des(1) = q1_des; q_des(2) = q2_des; % q_des.set(0, q1_des); q_des.set(1, q2_des);
dq_des = zeros(1,2); % dq_des = Vector( 2, 0.0 );
dq_des(1) = dq1_des; dq_des(2) = dq2_des; % dq_des.set(0, dq1_des); dq_des.set(1, dq2_des);
KP = diag(CentStruct.kp(1,1:2));
KV = diag(CentStruct.kv(1,1:2)); % KP = Matrix( 2, 2, 0.0 ); KV = Matrix( 2, 2, 0.0 );
% KP.set( 0, 0, kp ); KP.set( 1, 1, kp );
% KV.set( 0, 0, kv ); KV.set( 1, 1, kv );
controlLaw = zeros(1,CentStruct.Number);
controlLaw = (-1. * KP*(q - q_des)' - KV*(dq - dq_des)')';
controlLaw_Full = zeros(1,s.getNU);
controlLaw_Vector = Vector(s.getNU(), 0.0);
for ii=1:CentStruct.Number
controlLaw_Full(1,CentStruct.ValAd(1,ii)) = controlLaw(1,ii);
controlLaw_Vector.set(CentStruct.ValAd(1,ii)-1, controlLaw(1,ii));
end
smss = osimModel.getMatterSubsystem();
osimModel.realizeVelocity(s);
% Calculate MUDot = M * Udot
MUDot = Vector();
smss.multiplyByM(s, controlLaw_Vector, MUDot);
M = Matrix(s.getNU(),s.getNU());
smss.calcM(s,M);
M_array = osimMatrixToArray(M);
MUDot_array = (M_array * controlLaw_Full')';
% Calculate G_vector (Gravity force vector)
G_vector = Vector(s.getNU(),0.0);
GrvForce = osimModel.getGravityForce();
% % ****[ POINT 1 ]****
% % ** This method is not wrapped for MATLAB **
% % G_vector = GrvForce.getBodyForces(s);
% Calculate Cq_vector (Velocity vector)
Cq_vector = Vector();
knownUdot = Vector(s.getNU(), 0.0);
appliedMobilityForces = Vector();
appliedBodyForces = VectorOfSpatialVec();
smss.calcResidualForceIgnoringConstraints(s, appliedMobilityForces, appliedBodyForces, ...
knownUdot, Cq_vector);
% Test SimbodyMatterSubsystem.calcResidualForce().
% ------------------------------------------------
% For the given inputs, we will actually be computing the first column
% of the mass matrix. We accomplish this by setting all inputs to 0
% except for the acceleration of the first coordinate.
% f_residual = M udot + f_inertial + f_applied
% = M ~[1, 0, ...] + 0 + 0
knownLambda = Vector();
residualMobilityForces = Vector();
smss.calcResidualForce(s, Cq_vector, appliedBodyForces, ...
controlLaw_Vector, knownLambda, residualMobilityForces);
% Calculate Joint torques
controlTorque = Vector();
G_array = osimVectorToArray(G_vector);
Cq_array = osimVectorToArray(Cq_vector);
assert (size(MUDot_array,2) == size(Cq_array,2));
controlTorque_array = MUDot_array + Cq_array - G_array;
controlTorque = osimVectorFromArray(controlTorque_array);
disp(['controlTorque: [' num2str(controlTorque_array) ']' ]);
for jj=1:CentStruct.Number
CentStruct.ContV(jj) = controlTorque.get(CentStruct.ValAd(1,jj)-1);
end
assert(controlTorque.size() == s.getNU());
for ii=1:size(Control_Func.time,2)
if Control_Func.time(ii)>time_start && Control_Func.time(ii)<=time_end
for jj=1:CentStruct.Number
assert(strcmp (Control_Func.name(jj), CentStruct.Cent_act_name(jj)));
Control_Func.data(jj, ii) = CentStruct.ContV(jj);
end
end
end
% Delete old ControllerSet from the model
N_olderCont = osimModel.getControllerSet().getSize();
for ii = 1: N_olderCont
olderController = osimModel.getControllerSet().get(ii-1);
osimModel.removeController(olderController);
clear olderController
end
clear N_olderCont
q1m = joint_1.getValue(s);
dq1m = joint_1.getSpeedValue(s);
q2m = joint_2.getValue(s);
dq2m = joint_2.getSpeedValue(s);
disp(['Check Point A']);
disp(['\njoint1 vl: [' num2str(q1m/pi*180.) ']/desired[' num2str(q1_des/pi*180.) '] ']);
disp(['joint1 sp: [' num2str(dq1m/pi*180.) ']/desired[' num2str(dq1_des/pi*180.) '] ']);
disp(['joint2 vl: [' num2str(q2m/pi*180.) ']/desired[' num2str(q2_des/pi*180.) '] ']);
disp(['joint2 sp: [' num2str(dq2m/pi*180.) ']/desired[' num2str(dq2_des/pi*180.) '] ']);
% Define a controller and add it to the model
ActuatorController = PrescribedController();
ActuatorController.setName('PiecewiseLinear Controller')
ActuatorController.setActuators(osimModel.updActuators())
% get actuator set from model, and count the number of actuators
model_act = osimModel.getActuators();
Nacts = model_act.getSize();
for ii = 1:Nacts
PLF = PiecewiseLinearFunction();
% Get the coordinate set from the model
CurrentActs = model_act.get(ii-1);
for jj = 1:size(Control_Func.name,2)
if strcmp(CurrentActs.getName(), Control_Func.name(jj))
for kk=1:size(Control_Func.time,2)
PLF.addPoint(Control_Func.time(kk),Control_Func.data(jj,kk));
end
break;
end
end
ActuatorController.prescribeControlForActuator(ii-1,PLF);
end
clear PLF CurrentActs model_act Nacts;
% % ****[ POINT 2 ]****
% % Add controller to osimModel
osimModel.addController(ActuatorController);
clear muscleController;
% % Save the Modified Model to a file
% fileoutpath = ['Monitor_Model.osim'];
% osimModel.print(fileoutpath);
% disp(['The new model has been saved at ' fileoutpath]);
% %
% % ****[ POINT 3 ]****
% % Call the function that runs the simulation
% osimModel.setPropertiesFromState(s);
% osimModel.equilibrateMuscles(s);
osimModel.initSystem();
% Create a manager to run the simulation
simulationManager = Manager(osimModel);
simulationManager.setWriteToStorage(true);
simulationManager.setPerformAnalyses(true);
simulationManager.setInitialTime(time_start);
simulationManager.setFinalTime(time_end);
simulationManager.setIntegratorAccuracy(1e-04);
simulationManager.integrate(s);
q1 = joint_1.getValue(s);
dq1 = joint_1.getSpeedValue(s);
q2 = joint_2.getValue(s);
dq2 = joint_2.getSpeedValue(s);
q1_des = CentStruct.Q_des(1,1);
dq1_des = CentStruct.U_des(1,1);
q2_des = CentStruct.Q_des(1,2);
dq2_des = CentStruct.U_des(1,2);
disp(['2\njoint1 vl: [' num2str(q1/pi*180.) ']/desired[' num2str(q1_des/pi*180.) '] ']);
disp(['joint1 sp: [' num2str(dq1/pi*180.) ']/desired[' num2str(dq1_des/pi*180.) '] ']);
disp(['joint2 vl: [' num2str(q2/pi*180.) ']/desired[' num2str(q2_des/pi*180.) '] ']);
disp(['joint2 sp: [' num2str(dq2/pi*180.) ']/desired[' num2str(dq2_des/pi*180.) '] ']);
simulationManager.delete;
clear simulationManager;
end