simtk.org

Code: Select all

               State s=this->model.initSystem();
		Storage statesStore;
		statesStore.setDescription("initial pose sequence");
		Array<std::string> stateNames;
		stateNames=this->model.getStateVariableNames();
		stateNames.insert(0, "time");
		statesStore.setColumnLabels(stateNames);
		Array<double> stateVals;
		this->model.getStateValues(s,stateVals);
		//Fill state storage with a sequence of copies
		for(int i=0;i<this->hz;i++){
			double time=t0+i*this->dt;
			statesStore.append(time,stateVals.size(),stateVals.get());
		}
		statesStore.setInDegrees(false);
		//Static optimization 
		StaticOptimization so(&this->model);
		so.setStatesStore(statesStore);
		double ti, tf;
		statesStore.getTime(0,ti);
		statesStore.getTime(this->hz,tf);
		so.setStartTime(ti);
		so.setEndTime(tf);
		//run the analysis
		for(int i=0;i<this->hz;i++){	
			//copy states
			statesStore.getData(i,s.getNY(),&s.updY()[0]);
			double t=0.0;
			statesStore.getTime(i,t);
			s.setTime(t);
			this->model.assemble(s);
			this->model.getMultibodySystem().realize(s, SimTK::Stage::Velocity);
			if(i==0){
				so.begin(s);
			}else if(i==this->hz){
				so.end(s);
			}else{
				so.step(s,i);
			}
		}

		//write the results into control vector
		for(int i=0;i<this->hz;i++){
			this->u[i]=Vector(this->szU);
			so.getActivationStorage()->getData(i,this->u[i].size(),&this->u[i][0]);
		}