I am trying to compute the joint reaction analysis in C++ because I want to implement the results of this analysis as a constraint in the static optimization.
Briefly I calculate the muscle force from the static optimisation (by importing an xml file), then I use them to calculate the joint reaction force at a given joint.
So I do not use the xml file the computing of the joint reaction in my C++ program.
My problem is that when I calculate the joint reaction forces with the GUI, by using the same muscle forces than those obtained in my C++ program,
I do no find the same values of joint reaction Forces.
I have check many things such as be sure that.
-the states positions are the same
-the external forces are the same
-the muscle forces are the same
please find below my code which is written in the optimizationTarget.cpp File. In this case; the joint reaction forces are just calculated
but not included in the constraints.
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 ¶meters,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]);
}
can anyone tell me if something appears to be wrong ?
best regards
yoann