diff --git a/OpenSim/Simulation/InverseKinematicsSolver.cpp b/OpenSim/Simulation/InverseKinematicsSolver.cpp index 12bd5bd01f..3ad2cdb91a 100644 --- a/OpenSim/Simulation/InverseKinematicsSolver.cpp +++ b/OpenSim/Simulation/InverseKinematicsSolver.cpp @@ -118,6 +118,31 @@ void InverseKinematicsSolver::updateMarkerWeights(const SimTK::Array_ &w throw Exception("InverseKinematicsSolver::updateMarkerWeights: invalid size of weights."); } +/* Get the current reference values for markers*/ +const SimTK::Vec3& InverseKinematicsSolver::getMarkerReferenceValue(int markerIndex) +{ + SimTK::Markers::ObservationIx oix( + _markerAssemblyCondition->getObservationIxForMarker( + SimTK::Markers::MarkerIx(markerIndex)) ); + return _markerAssemblyCondition->getObservation(oix); +} +const SimTK::Vec3& InverseKinematicsSolver::getMarkerReferenceValue(const std::string &markerName) +{ + SimTK::Markers::ObservationIx oix( + _markerAssemblyCondition->getObservationIxForMarker( + _markerAssemblyCondition->getMarkerIx(markerName)) ); + return _markerAssemblyCondition->getObservation(oix); +} +void InverseKinematicsSolver::getMarkerReferenceValues(SimTK::Array_ &markerRefs) +{ + int nm = getNumMarkersInUse(); + markerRefs.resize(nm); + + for (int i = 0; i < nm; ++i) { + markerRefs[i] = getMarkerReferenceValue(i); + } +} + /* Compute and return the spatial location of a marker in ground. */ SimTK::Vec3 InverseKinematicsSolver::computeCurrentMarkerLocation(const std::string &markerName) { diff --git a/OpenSim/Simulation/InverseKinematicsSolver.h b/OpenSim/Simulation/InverseKinematicsSolver.h index f357abc03c..c726381077 100644 --- a/OpenSim/Simulation/InverseKinematicsSolver.h +++ b/OpenSim/Simulation/InverseKinematicsSolver.h @@ -112,6 +112,14 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver solver was constructed. */ void updateMarkerWeights(const SimTK::Array_ &weights); + /** Get the current reference value for the marker specified by index */ + const SimTK::Vec3& getMarkerReferenceValue(int markerIndex); + /** Get the current reference value for the marker specified by name */ + const SimTK::Vec3& getMarkerReferenceValue(const std::string &markerName); + /** Get the current reference values for all markers in use. Missing + reference values at the current instant are denoted by NaN. */ + void getMarkerReferenceValues(SimTK::Array_ &markerRefs); + /** Compute and return a marker's spatial location in the ground frame, given the marker's name. */ SimTK::Vec3 computeCurrentMarkerLocation(const std::string &markerName); diff --git a/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp b/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp index c215cf96e5..9df11cea9e 100644 --- a/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp +++ b/OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp @@ -550,14 +550,21 @@ void testNumberOfMarkersMismatch() int nm = ikSolver.getNumMarkersInUse(); + SimTK::Array_ markerRefs(nm); SimTK::Array_ markerErrors(nm); + for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) { state.updTime() = i*dt; ikSolver.track(state); + ikSolver.getMarkerReferenceValues(markerRefs); + int nmr = markerRefs.size(); + SimTK_ASSERT_ALWAYS(nmr == nm, + "InverseKinematicsSolver number of reference values " + "failed to match the number of markers in use."); + //get the marker errors ikSolver.computeCurrentMarkerErrors(markerErrors); - int nme = markerErrors.size(); SimTK_ASSERT_ALWAYS(nme == nm, @@ -582,6 +589,15 @@ void testNumberOfMarkersMismatch() SimTK_ASSERT_ALWAYS(markerErrors[j] <= tol, "InverseKinematicsSolver mangled marker order."); } + + if (markerRefs[j].isNaN()) { + SimTK_ASSERT_ALWAYS(markerName == "mR", + "InverseKinematicsSolver order of marker reference " + "values does not match order of markers."); + SimTK_ASSERT_ALWAYS(markerErrors[j] == 0, + "InverseKinematicsSolver failed to assign 0 for the " + "error associated with marker reference with NaN value."); + } } cout << endl; } diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index 289c3bdc3f..b4e123a714 100644 --- a/OpenSim/Tools/InverseKinematicsTool.cpp +++ b/OpenSim/Tools/InverseKinematicsTool.cpp @@ -332,10 +332,17 @@ bool InverseKinematicsTool::run() ikSolver.track(s); if(_reportErrors){ - Array markerErrors(0.0, 3); + Array_ markerRefValues(nm); + Array markerErrors(0.0, nm); double totalSquaredMarkerError = 0.0; double maxSquaredMarkerError = 0.0; int worst = -1; + int ignore_cnt = 0; + + ikSolver.getMarkerReferenceValues(markerRefValues); + for (const Vec3& val : markerRefValues) { + if(val.isNaN()) ignore_cnt++; + } ikSolver.computeCurrentSquaredMarkerErrors(squaredMarkerErrors); for(int j=0; j 0 ? sqrt(totalSquaredMarkerError / nm) : 0; + double rms = (nm-ignore_cnt) > 0 ? + sqrt(totalSquaredMarkerError/(nm-ignore_cnt)) : NaN; markerErrors.set(0, totalSquaredMarkerError); markerErrors.set(1, rms); markerErrors.set(2, sqrt(maxSquaredMarkerError));