From 2ce323a4fb7491ada5a2bf7f5e81cac348a8f144 Mon Sep 17 00:00:00 2001 From: Ajay Seth Date: Wed, 24 Jan 2018 13:22:30 -0800 Subject: [PATCH 1/4] Add methods to get the marker reference values from the InverseKinematicsSolver --- .../Simulation/InverseKinematicsSolver.cpp | 25 +++++++++++++++++++ OpenSim/Simulation/InverseKinematicsSolver.h | 8 ++++++ 2 files changed, 33 insertions(+) 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); From d1e8bff4022ff6709d5530cf89203f20ceed099e Mon Sep 17 00:00:00 2001 From: Ajay Seth Date: Wed, 24 Jan 2018 14:09:38 -0800 Subject: [PATCH 2/4] Add test conditions to verify the order of marker reference values returned by InverseKinematicsSolver is consistent with order of marker errors. --- .../Test/testInverseKinematicsSolver.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) 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; } From 335fda59a8a84e9897654e46f300a32c5ad2c16b Mon Sep 17 00:00:00 2001 From: Ajay Seth Date: Wed, 24 Jan 2018 14:50:34 -0800 Subject: [PATCH 3/4] Check for NaN marker references and account for them in computing RMS error values. --- OpenSim/Tools/InverseKinematicsTool.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index 289c3bdc3f..9b66f9e72a 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) { + val.isNaN() ? ignore_cnt++ : 0; + } 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)); From e73e1daaf2685747def4f2374ad44cb1b44f8adb Mon Sep 17 00:00:00 2001 From: Ajay Seth Date: Wed, 24 Jan 2018 16:52:45 -0800 Subject: [PATCH 4/4] Use if instead of ternary operator --- OpenSim/Tools/InverseKinematicsTool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Tools/InverseKinematicsTool.cpp b/OpenSim/Tools/InverseKinematicsTool.cpp index 9b66f9e72a..b4e123a714 100644 --- a/OpenSim/Tools/InverseKinematicsTool.cpp +++ b/OpenSim/Tools/InverseKinematicsTool.cpp @@ -341,7 +341,7 @@ bool InverseKinematicsTool::run() ikSolver.getMarkerReferenceValues(markerRefValues); for (const Vec3& val : markerRefValues) { - val.isNaN() ? ignore_cnt++ : 0; + if(val.isNaN()) ignore_cnt++; } ikSolver.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);