simtk.org

Code: Select all

void StaticOptimizationTarget::
printPerformance(const SimTK::State& s, double *parameters)
{
	double p;
	setCurrentState( &s );
	objectiveFunc(SimTK::Vector(getNumParameters(),parameters,true),true,p);
	SimTK::Vector constraints(getNumConstraints());
    constraintFunc(SimTK::Vector(getNumParameters(),parameters,true),true,constraints);
	
//	run joint reaction in this function just to see the results

	SimTK::State s2 = s;
	_jointReaction = new JointReaction();
    Array<std::string> jointNames("preshoulder1",1);
    _jointReaction->setJointNames(jointNames);
    Array<std::string> onBody("parent",1);
    _jointReaction->setOnBody(onBody);
    _jointReaction->setInFrame(onBody);
    _jointReaction->setModel(*_model);
	_jointReaction->begin(s2);
	
	//compute the joint reaction analysis for the following steps
	computeConstraintVectorB(s2, SimTK::Vector(getNumParameters(),parameters,true),constraints);
	
	cout << endl;
	cout << "time = " << s.getTime() <<" Performance =" << p << 
	" Constraint violation = " << sqrt(~constraints*constraints) << endl;
	
}	


void StaticOptimizationTarget::
computeConstraintVectorB(SimTK::State& s, const Vector &parameters,Vector &constraints) const
{
	
	// Compute actual accelerations
    int nConstraints(getNumConstraints());

    Vector actualAcceleration(nConstraints);
	computeAcceleration(s, parameters, actualAcceleration);

	// CONSTRAINTS

    for(int i=0; i<nConstraints; i++) {

		Coordinate& coord = _model->getCoordinateSet().get(_accelerationIndices[i]);
		
	} 

    // force constraint
    unsigned int N_mus(parameters.size()); // Number of muscles
    double *dforces;
    dforces = new double[N_mus+1]; // time column + muscle number
    Array<std::string> muscles;
    muscles.append("TIME"); // Append to the time column (index -1)

    // Calculate forces fro; accelerations
    SimTK::Vector musleForces(N_mus);
	getActuation(s, parameters,musleForces);


    dforces[0] = 0; // time set at zero
	
    for (unsigned int i=0; i<N_mus; ++i){
        dforces[i+1] = musleForces[i]; // copy muscle forces
		muscles.append((&_model->getActuators())->get(i).getName()); // copy associated muscle name
	}

	//storage
    Storage forces;
    forces.append(StateVector(0,N_mus+1,dforces)); 
    forces.setColumnLabels(muscles); // 
	
	// run joint reaction analysis
    _jointReaction->step(s, forces);

    // get values
    Storage react(_jointReaction->getForces());

    // get forces values
    unsigned int nReact = 9; // get only force values
    SimTK::Vector dforcesReact(nReact);
    react.getDataAtTime(s.getTime(),nReact, dforcesReact);
    SimTK::Vec3 dforcesReact2(dforcesReact[0], dforcesReact[1], dforcesReact[2]);

	
}