Skip to content

Commit

Permalink
Throw if passed in name or index of marker is invalid (#3970)
Browse files Browse the repository at this point in the history 
* Throw if passed in name or index of marker is invalid (rather than crash quietly)

* Update CHANGELOG.md

* Make interface methods that use index private as error prone and leaks the internal container.

* More error checking for consistency in methods that query by name

* More error checking for consistency in methods that query by name

* Test newly introduced exceptions in test case

* Move orientation sensor names error check to function that has orientation tracking
  • Loading branch information
@aymanhab
aymanhab authored Nov 20, 2024
1 parent 94efbad commit db5dcd4
Show file tree
Hide file tree
Showing 4 changed files with 73 additions and 22 deletions.
3 changes: 3 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,9 @@ v4.6
- Added methods and `Output`s for calculating the angular momentum of a `Body`. (#3962)
- Updated `TabOpLowPassFilter` so that by default the processed table is trimmed to the original time range after padding and filtering.
The original behavior (no trimming) can be enabled via the new property `trim_to_original_time_range`. (#3969)
- Make `InverseKinematicsSolver` methods to query for specific marker or orientation-sensor errors more robust to invalid names or names not
in the intersection of names in the model and names in the provided referece/data. Remove methods that are index based from public interface.(#3951)



v4.5.1
Expand Down
25 changes: 25 additions & 0 deletions OpenSim/Simulation/InverseKinematicsSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,10 @@ void InverseKinematicsSolver::updateMarkerWeight(const std::string& markerName,
{
const Array_<std::string> &names = _markersReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), markerName);
if (p == names.end())
throw Exception(
"InverseKinematicsSolver::updateMarkerWeight: "
"invalid or unused marker name " + markerName + ".");
int index = (int)std::distance(names.begin(), p);
updateMarkerWeight(index, value);
}
Expand Down Expand Up @@ -175,6 +179,10 @@ void InverseKinematicsSolver::updateOrientationWeight(const std::string& orienta
{
const Array_<std::string> &names = _orientationsReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), orientationName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::updateOrientationWeight: "
"invalid or unused orientation sensor name " +
orientationName + ".");
int index = (int)std::distance(names.begin(), p);
updateOrientationWeight(index, value);
}
Expand Down Expand Up @@ -212,6 +220,10 @@ SimTK::Vec3 InverseKinematicsSolver::computeCurrentMarkerLocation(const std::str
{
const Array_<std::string> &names = _markersReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), markerName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::computeCurrentMarkerLocation: "
"invalid or unused marker name " +
markerName + ".");
int index = (int)std::distance(names.begin(), p);
return computeCurrentMarkerLocation(index);
}
Expand Down Expand Up @@ -239,6 +251,9 @@ double InverseKinematicsSolver::computeCurrentMarkerError(const std::string &mar
{
const Array_<std::string>& names = _markersReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), markerName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::computeCurrentMarkerError: "
"invalid or unused marker name "+markerName+".");
int index = (int)std::distance(names.begin(), p);
return computeCurrentMarkerError(index);
}
Expand Down Expand Up @@ -266,6 +281,10 @@ double InverseKinematicsSolver::computeCurrentSquaredMarkerError(const std::stri
{
const Array_<std::string>& names = _markersReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), markerName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::computeCurrentSquaredMarkerError: "
"invalid or unused marker name " +
markerName + ".");
int index = (int)std::distance(names.begin(), p);
return computeCurrentSquaredMarkerError(index);
}
Expand Down Expand Up @@ -301,6 +320,9 @@ SimTK::Rotation InverseKinematicsSolver::
{
const Array_<std::string>& names = _orientationsReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), osensorName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::computeCurrentSensorOrientation: "
"invalid or unused orientation sensor name " + osensorName + ".");
int index = (int)std::distance(names.begin(), p);
return computeCurrentSensorOrientation(index);
}
Expand Down Expand Up @@ -331,6 +353,9 @@ double InverseKinematicsSolver::
{
const Array_<std::string>& names = _orientationsReference->getNames();
SimTK::Array_<const std::string>::iterator p = std::find(names.begin(), names.end(), osensorName);
if (p == names.end())
throw Exception("InverseKinematicsSolver::computeCurrentOrientationError: "
"invalid or unused orientation sensor name " + osensorName + ".");
int index = (int)std::distance(names.begin(), p);
return computeCurrentOrientationError(index);
}
Expand Down
53 changes: 31 additions & 22 deletions OpenSim/Simulation/InverseKinematicsSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,9 +140,6 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
/** Change the weighting of a marker, given the marker's name. Takes effect
when assemble() or track() is called next. */
void updateMarkerWeight(const std::string &markerName, double value);
/** Change the weighting of a marker, given the marker's index. Takes effect
when assemble() or track() is called next. */
void updateMarkerWeight(int markerIndex, double value);
/** Change the weighting of all markers. Takes effect when assemble() or
track() is called next. Marker weights are specified in the same order
as they appear in the MarkersReference that was passed in when the
Expand All @@ -152,9 +149,6 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
/** Change the weighting of an orientation sensor, given its name. Takes
effect when assemble() or track() is called next. */
void updateOrientationWeight(const std::string& orientationName, double value);
/** Change the weighting of an orientation sensor, given its index. Takes
effect when assemble() or track() is called next. */
void updateOrientationWeight(int orientationIndex, double value);
/** Change the weighting of all orientation sensors. Takes effect when
assemble() or track() is called next. Orientation weights are specified
in the same order as they appear in the OrientationsReference that was
Expand All @@ -164,19 +158,13 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
/** Compute and return a marker's spatial location in the ground frame,
given the marker's name. */
SimTK::Vec3 computeCurrentMarkerLocation(const std::string &markerName);
/** Compute and return a marker's spatial location in the ground frame,
given the marker's index. */
SimTK::Vec3 computeCurrentMarkerLocation(int markerIndex);
/** Compute and return the spatial locations of all markers, expressed in
the ground frame. */
void computeCurrentMarkerLocations(SimTK::Array_<SimTK::Vec3> &markerLocations);

/** Compute and return the distance error between a model marker and its
observation, given the marker's name. */
double computeCurrentMarkerError(const std::string &markerName);
/** Compute and return the distance error between a model marker and its
observation, given the marker's index. */
double computeCurrentMarkerError(int markerIndex);
/** Compute and return the distance errors between all model markers and
their observations. */
void computeCurrentMarkerErrors(SimTK::Array_<double> &markerErrors);
Expand All @@ -185,10 +173,7 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
its observation, given the marker's name. This method is cheaper than
squaring the value returned by computeCurrentMarkerError(). */
double computeCurrentSquaredMarkerError(const std::string &markerName);
/** Compute and return the squared-distance error between a model marker and
its observation, given the marker's index. This method is cheaper than
squaring the value returned by computeCurrentMarkerError(). */
double computeCurrentSquaredMarkerError(int markerIndex);

/** Compute and return the squared-distance errors between all model markers
and their observations. This method is cheaper than squaring the values
returned by computeCurrentMarkerErrors(). */
Expand All @@ -203,9 +188,7 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
/** Compute and return an orientation sensor's spatial orientation in the
ground frame, given the o-sensor's name. */
SimTK::Rotation computeCurrentSensorOrientation(const std::string& osensorName);
/** Compute and return an orientation sensor's spatial orientation in the
ground frame, given the o-sensor's index. */
SimTK::Rotation computeCurrentSensorOrientation(int osensorIndex);

/** Compute and return the spatial orientations of all o-sensors, expressed in
the ground frame. */
void computeCurrentSensorOrientations(
Expand All @@ -214,9 +197,6 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
/** Compute and return the orientation error between the model orientation
sensor and its observation, given the o-sensor's name. */
double computeCurrentOrientationError(const std::string& osensorName);
/** Compute and return the orientation error between the model orientation
sensor and its observation, given the o-sensor's index. */
double computeCurrentOrientationError(int osensorIndex);
/** Compute all the orientation errors between the model orientation
sensors and their observations. */
void computeCurrentOrientationErrors(SimTK::Array_<double>& osensorErrors);
Expand Down Expand Up @@ -248,6 +228,35 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
assembly problem. */
void setupOrientationsGoal(SimTK::State &s);

/** Compute and return a marker's spatial location in the ground frame,
given the marker's index. */
SimTK::Vec3 computeCurrentMarkerLocation(int markerIndex);

/** Compute and return the distance error between a model marker and its
observation, given the marker's index. */
double computeCurrentMarkerError(int markerIndex);

/** Compute and return the squared-distance error between a model marker and
its observation, given the marker's index. This method is cheaper than
squaring the value returned by computeCurrentMarkerError(). */
double computeCurrentSquaredMarkerError(int markerIndex);

/** Change the weighting of a marker, given the marker's index. Takes effect
when assemble() or track() is called next. */
void updateMarkerWeight(int markerIndex, double value);

/** Compute and return an orientation sensor's spatial orientation in the
ground frame, given the o-sensor's index. */
SimTK::Rotation computeCurrentSensorOrientation(int osensorIndex);

/** Compute and return the orientation error between the model orientation
sensor and its observation, given the o-sensor's index. */
double computeCurrentOrientationError(int osensorIndex);

/** Change the weighting of an orientation sensor, given its index. Takes
effect when assemble() or track() is called next. */
void updateOrientationWeight(int orientationIndex, double value);

// The marker reference values and weightings
std::shared_ptr<MarkersReference> _markersReference;

Expand Down
14 changes: 14 additions & 0 deletions OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -685,6 +685,14 @@ void testNumberOfMarkersMismatch()
"InverseKinematicsSolver mangled marker order.");
}
}
SimTK_TEST_MUST_THROW_EXC(
ikSolver.computeCurrentMarkerError("junk"), Exception);
SimTK_TEST_MUST_THROW_EXC(
ikSolver.computeCurrentSquaredMarkerError("junk"), Exception);
SimTK_TEST_MUST_THROW_EXC(
ikSolver.computeCurrentMarkerLocation("junk"), Exception);
SimTK_TEST_MUST_THROW_EXC(
ikSolver.updateMarkerWeight("junk", 0.1), Exception);
cout << endl;
}
}
Expand Down Expand Up @@ -804,6 +812,12 @@ void testNumberOfOrientationsMismatch()
}
cout << endl;
}
SimTK_TEST_MUST_THROW_EXC(
ikSolver.computeCurrentOrientationError("junk"), Exception);
SimTK_TEST_MUST_THROW_EXC(
ikSolver.computeCurrentSensorOrientation("junk"), Exception);
SimTK_TEST_MUST_THROW_EXC(
ikSolver.updateOrientationWeight("junk", 0.1), Exception);
}

Model* constructPendulumWithMarkers()
Expand Down

0 comments on commit db5dcd4

Please sign in to comment.