/** Internal method to convert the MarkerReferences into additional goals of the
of the base assembly solver, that is going to do the assembly. */
void InverseKinematicsSolver::setupGoals(SimTK::State &s)
{
// Setup coordinates performed by the base class
AssemblySolver::setupGoals(s);
_markerAssemblyCondition = new SimTK::Markers();
// Setup markers goals
// Get lists of all markers by names and corresponding weights from the MarkersReference
const SimTK::Array_<SimTK::String> &markerNames = _markersReference.getNames();
SimTK::Array_<double> markerWeights;
_markersReference.getWeights(s, markerWeights);
// get markers defined by the model
const MarkerSet &modelMarkerSet = getModel().getMarkerSet();
// get markers with specified tasks/weights
const Set<MarkerWeight>& mwSet = _markersReference.updMarkerWeightSet();
int index = -1;
int wIndex = -1;
SimTK::Transform X_BF;
//Loop through all markers in the reference
for(unsigned int i=0; i < markerNames.size(); ++i){
// Check if we have this marker in the model, else ignore it
index = modelMarkerSet.getIndex(markerNames[i], index);
wIndex = mwSet.getIndex(markerNames[i],wIndex);
if((index >= 0) && (wIndex >=0)){
Marker &marker = modelMarkerSet[index];
const SimTK::MobilizedBody& mobod =
marker.getReferenceFrame().getMobilizedBody();
X_BF = marker.getReferenceFrame().findTransformInBaseFrame();
_markerAssemblyCondition->
addMarker(marker.getName(), mobod, X_BF*marker.get_location(),
markerWeights[i]);
//cout << "IKSolver Marker: " << markerNames[i] << " " << marker.getName() << " weight: " << markerWeights[i] << endl;
}
}
// Add marker goal to the ik objective
_assembler->adoptAssemblyGoal(_markerAssemblyCondition);
// lock-in the order that the observations (markers) are in and this cannot change from frame to frame
// and we can use an array of just the data for updating
_markerAssemblyCondition->defineObservationOrder(markerNames);
updateGoals(s);
}
/** Obtain a model configuration that meets the assembly conditions
(desired values and constraints) given a state that satisfies or
is close to satisfying the constraints. Note there can be no change
in the number of constraints or desired coordinates. Desired
coordinate values can and should be updated between repeated calls
to track a desired trajectory of coordinate values. */
void AssemblySolver::track(SimTK::State &s)
{
// move the target locations or angles, etc... just do not change number of goals
// and their type (constrained vs. weighted)
if(_assembler && _assembler->isInitialized()){
updateGoals(s);
}
else{
throw Exception(
"AssemblySolver::track() failed: assemble() must be called first.");
}
/* TODO: Useful to include through debug message/log in the future
printf("UNASSEMBLED(track) CONFIGURATION (normerr=%g, maxerr=%g, cost=%g)\n",
_assembler->calcCurrentErrorNorm(),
max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal() );
cout << "Model numQs: " << _assembler->getInternalState().getNQ()
<< " Assembler num freeQs: " << _assembler->getNumFreeQs() << endl;
*/
try{
// Now do the assembly and return the updated state.
_assembler->track(s.getTime());
// update the state from the result of the assembler
_assembler->updateFromInternalState(s);
/* TODO: Useful to include through debug message/log in the future
printf("Tracking: t= %f (acc=%g tol=%g normerr=%g, maxerr=%g, cost=%g)\n",
s.getTime(),
_assembler->getAccuracyInUse(), _assembler->getErrorToleranceInUse(),
_assembler->calcCurrentErrorNorm(), max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal());
*/
}
catch (const std::exception& ex)
{
std::cout << "AssemblySolver::track() attempt Failed: " << ex.what() << std::endl;
throw Exception("AssemblySolver::track() attempt failed.");
}
}
