Skip to content

Upgrade InverseKinematicsTool to new Property system #2685

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 13 commits into from
Mar 25, 2020
Merged

Upgrade InverseKinematicsTool to new Property system #2685

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
30c5d90
Remove _verbose level as it doesn't belong to Tool
aymanhab Feb 27, 2020
15681e2
Remove unused _inputDir property
aymanhab Feb 27, 2020
23f84b8
Upgrade Tool to use new property system
aymanhab Mar 3, 2020
c9d70c0
Use ReferencePtr instead of naked model pointer
aymanhab Mar 3, 2020
c9d3989
Upgrade InverseKinematicsTool to use new property system, work in pro…
aymanhab Mar 5, 2020
eb87734
Reorder, initialize all Properties
aymanhab Mar 5, 2020
b898d8c
Remove registerTypes method and explicit call, should happen similar …
aymanhab Mar 6, 2020
ca8ccdc
Add orientation_file property and lock translational coords if only t…
aymanhab Mar 10, 2020
c333c2a
Revert "Add orientation_file property and lock translational coords i…
aymanhab Mar 16, 2020
af28d78
Address feedback on PR, in particular property comments, variable names
aymanhab Mar 20, 2020
8ab69b2
Fix quotes
aymanhab Mar 20, 2020
aef12b3
Address feedback on PR, formatting and headers
aymanhab Mar 25, 2020
c722623
Move include line later to fix compilation errors
aymanhab Mar 25, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions Applications/IK/ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,6 @@ int main(int argc,char **argv)
)";
std::cout << deprecationNotice << std::endl;

// REGISTER TYPES
InverseKinematicsTool::registerTypes();

// PARSE COMMAND LINE
string option = "";
string setupFileName;
Expand Down
1 change: 0 additions & 1 deletion OpenSim/Tools/InverseDynamicsTool.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -274,7 +274,6 @@ bool InverseDynamicsTool::run()
<< _lowpassCutoffFrequency << "..." << endl << endl;
_coordinateValues->pad(_coordinateValues->getSize()/2);
_coordinateValues->lowpassIIR(_lowpassCutoffFrequency);
if (getVerboseLevel()==Debug) _coordinateValues->print("coordinateDataFiltered.sto");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this a loss of functionality for someone trying to debug?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

True but rarely used AFAIK, we're moving in the direction of having a logger that does logging across all objects/tools so a private verbosity level flag is very ad-hoc and can be removed.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds good. Is there an issue open about what the plans are for the logger? Would be good to make sure these are in there if so.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

PR #2551 is the new logger PR

}
// Convert degrees to radian if indicated
if(_coordinateValues->isInDegrees()){
Expand Down
252 changes: 48 additions & 204 deletions OpenSim/Tools/InverseKinematicsTool.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,20 +58,9 @@ InverseKinematicsTool::~InverseKinematicsTool()
/**
* Default constructor.
*/
InverseKinematicsTool::InverseKinematicsTool() : Tool(),
_modelFileName(_modelFileNameProp.getValueStr()),
_constraintWeight(_constraintWeightProp.getValueDbl()),
_accuracy(_accuracyProp.getValueDbl()),
_ikTaskSetProp(PropertyObj("", IKTaskSet())),
_ikTaskSet((IKTaskSet&)_ikTaskSetProp.getValueObj()),
_markerFileName(_markerFileNameProp.getValueStr()),
_coordinateFileName(_coordinateFileNameProp.getValueStr()),
_timeRange(_timeRangeProp.getValueDblArray()),
_reportErrors(_reportErrorsProp.getValueBool()),
_outputMotionFileName(_outputMotionFileNameProp.getValueStr()),
_reportMarkerLocations(_reportMarkerLocationsProp.getValueBool())
InverseKinematicsTool::InverseKinematicsTool() : Tool()
{
setNull();
constructProperties();
}
//_____________________________________________________________________________
/**
Expand All @@ -83,176 +72,32 @@ InverseKinematicsTool::InverseKinematicsTool() : Tool(),
* @param aFileName File name of the document.
*/
InverseKinematicsTool::InverseKinematicsTool(const string &aFileName, bool aLoadModel) :
Tool(aFileName, true),
_modelFileName(_modelFileNameProp.getValueStr()),
_constraintWeight(_constraintWeightProp.getValueDbl()),
_accuracy(_accuracyProp.getValueDbl()),
_ikTaskSetProp(PropertyObj("", IKTaskSet())),
_ikTaskSet((IKTaskSet&)_ikTaskSetProp.getValueObj()),
_markerFileName(_markerFileNameProp.getValueStr()),
_coordinateFileName(_coordinateFileNameProp.getValueStr()),
_timeRange(_timeRangeProp.getValueDblArray()),
_reportErrors(_reportErrorsProp.getValueBool()),
_outputMotionFileName(_outputMotionFileNameProp.getValueStr()),
_reportMarkerLocations(_reportMarkerLocationsProp.getValueBool())
Tool(aFileName, true)
{
setNull();
constructProperties();
updateFromXMLDocument();

if(aLoadModel) {
//loadModel(aFileName);
}
}
//_____________________________________________________________________________
/**
* Copy constructor.
*
* @param aTool Object to be copied.

*/
InverseKinematicsTool::InverseKinematicsTool(const InverseKinematicsTool &aTool) :
Tool(aTool),
_modelFileName(_modelFileNameProp.getValueStr()),
_constraintWeight(_constraintWeightProp.getValueDbl()),
_accuracy(_accuracyProp.getValueDbl()),
_ikTaskSetProp(PropertyObj("", IKTaskSet())),
_ikTaskSet((IKTaskSet&)_ikTaskSetProp.getValueObj()),
_markerFileName(_markerFileNameProp.getValueStr()),
_coordinateFileName(_coordinateFileNameProp.getValueStr()),
_timeRange(_timeRangeProp.getValueDblArray()),
_reportErrors(_reportErrorsProp.getValueBool()),
_outputMotionFileName(_outputMotionFileNameProp.getValueStr()),
_reportMarkerLocations(_reportMarkerLocationsProp.getValueBool())
{
setNull();
*this = aTool;
}

//_____________________________________________________________________________
/**
* Set all member variables to their null or default values.
*/
void InverseKinematicsTool::setNull()
{
setupProperties();
_model = NULL;
}
//_____________________________________________________________________________
/**
* Connect properties to local pointers.
*/
void InverseKinematicsTool::setupProperties()
{
_modelFileNameProp.setComment(
"Name of the model file (.osim) to use for inverse kinematics.");
_modelFileNameProp.setName("model_file");
_propertySet.append( &_modelFileNameProp );

_constraintWeightProp.setComment(
"A positive scalar that weights the relative importance of satisfying "
"constraints. A weighting of 'Infinity' (the default) results in the "
"constraints being strictly enforced. Otherwise, the weighted-squared "
"constraint errors are appended to the cost function.");
_constraintWeightProp.setName("constraint_weight");
_constraintWeightProp.setValue(std::numeric_limits<SimTK::Real>::infinity());
_propertySet.append( &_constraintWeightProp );

_accuracyProp.setComment(
"The accuracy of the solution in absolute terms. Default is 1e-5. "
"It determines the number of significant digits to which the solution "
"can be trusted.");
_accuracyProp.setName("accuracy");
_accuracyProp.setValue(1e-5);
_propertySet.append( &_accuracyProp );

_ikTaskSetProp.setComment(
"Markers and coordinates to be considered (tasks) and their weightings. "
"The sum of weighted-squared task errors composes the cost function.");
_ikTaskSetProp.setName("IKTaskSet");
_propertySet.append(&_ikTaskSetProp);

_markerFileNameProp.setComment(
"TRC file (.trc) containing the time history of observations of marker "
"positions obtained during a motion capture experiment. Markers in this "
"file that have a corresponding task and model marker are included.");
_markerFileNameProp.setName("marker_file");
_propertySet.append(&_markerFileNameProp);

_coordinateFileNameProp.setComment(
"The name of the storage (.sto or .mot) file containing the time history"
" of coordinate observations. Coordinate values from this file are "
"included if there is a corresponding model coordinate and task. ");
_coordinateFileNameProp.setName("coordinate_file");
_propertySet.append(&_coordinateFileNameProp);

const double defaultTimeRange[] =
{-std::numeric_limits<SimTK::Real>::infinity(),
std::numeric_limits<SimTK::Real>::infinity()};
_timeRangeProp.setComment(
"The desired time range over which inverse kinematics is solved. "
"The closest start and final times from the provided observations "
"are used to specify the actual time range to be processed.");
_timeRangeProp.setName("time_range");
_timeRangeProp.setValue(2, defaultTimeRange);
_timeRangeProp.setAllowableListSize(2);
_propertySet.append(&_timeRangeProp);

_reportErrorsProp.setComment(
"Flag (true or false) indicating whether or not to report marker "
"errors from the inverse kinematics solution.");
_reportErrorsProp.setName("report_errors");
_reportErrorsProp.setValue(true);
_propertySet.append(&_reportErrorsProp);

_outputMotionFileNameProp.setComment(
"Name of the resulting inverse kinematics motion (.mot) file.");
_outputMotionFileNameProp.setName("output_motion_file");
_propertySet.append(&_outputMotionFileNameProp);

_reportMarkerLocationsProp.setComment(
"Flag indicating whether or not to report model marker locations. "
"Note, model marker locations are expressed in Ground.");
_reportMarkerLocationsProp.setName("report_marker_locations");
_reportMarkerLocationsProp.setValue(false);
_propertySet.append(&_reportMarkerLocationsProp);
}

//_____________________________________________________________________________
/**
* Register InverseKinematicsTool and any Object types it may employ internally.
*/
void InverseKinematicsTool::registerTypes()
{
Object::registerType(InverseKinematicsTool());
}
//=============================================================================
// OPERATORS
//=============================================================================
//_____________________________________________________________________________
/**
* Assignment operator.
*
* @return Reference to this object.
* Construct properties
*/
InverseKinematicsTool& InverseKinematicsTool::
operator=(const InverseKinematicsTool &aTool)
void InverseKinematicsTool::constructProperties()
{
// BASE CLASS
Tool::operator=(aTool);

// MEMBER VARIABLES
_modelFileName = aTool._modelFileName;
_constraintWeight = aTool._constraintWeight;
_accuracy = aTool._accuracy;
_ikTaskSet = aTool._ikTaskSet;
_markerFileName = aTool._markerFileName;
_timeRange = aTool._timeRange;
_coordinateFileName = aTool._coordinateFileName;
_reportErrors = aTool._reportErrors;
_outputMotionFileName = aTool._outputMotionFileName;
_reportMarkerLocations = aTool._reportMarkerLocations;

return(*this);
constructProperty_model_file("");
constructProperty_constraint_weight(Infinity);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is Infinity defined from Simbody? What's the rationale on switching this from std::numeric_limits<SimTK::Real>::infinity()?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Infinity is SimTK::Infinity and as such is much cleaner than the more complicated syntax that returns same value (unless you're aware of other potential side-effects) it was used in other code already.

constructProperty_accuracy(1e-5);
constructProperty_IKTaskSet(IKTaskSet());
constructProperty_marker_file("");
constructProperty_coordinate_file("");
Array<double> range{Infinity, 2};
range[0] = -Infinity; // Make range -Infinity to Infinity unless limited by
// data
constructProperty_time_range(range);

constructProperty_report_errors(true);
constructProperty_output_motion_file("");
constructProperty_report_marker_locations(false);
}

//=============================================================================
Expand All @@ -274,10 +119,10 @@ bool InverseKinematicsTool::run()
Kinematics* kinematicsReporter = nullptr;
try{
//Load and create the indicated model
if (!_model) {
OPENSIM_THROW_IF_FRMOBJ(_modelFileName.empty(), Exception,
"No model filename was provided.");
_model = new Model(_modelFileName);
if (_model.empty()) {
OPENSIM_THROW_IF_FRMOBJ(get_model_file().empty(), Exception,
"No model filename was provided.");
_model.reset(new Model(get_model_file()));
}
else
modelFromFile = false;
Expand Down Expand Up @@ -317,11 +162,11 @@ bool InverseKinematicsTool::run()
// specified then use time from marker reference.
// Adjust the time range for the tool if the provided range exceeds
// that of the marker data.
SimTK::Vec2 markersValidTimRange = markersReference.getValidTimeRange();
double start_time = (markersValidTimRange[0] > _timeRange[0]) ?
markersValidTimRange[0] : _timeRange[0];
double final_time = (markersValidTimRange[1] < _timeRange[1]) ?
markersValidTimRange[1] : _timeRange[1];
SimTK::Vec2 markersValidTimeRange = markersReference.getValidTimeRange();
double start_time = (markersValidTimeRange[0] > get_time_range(0)) ?
markersValidTimeRange[0] : get_time_range(0);
double final_time = (markersValidTimeRange[1] < get_time_range(1)) ?
markersValidTimeRange[1] : get_time_range(1);

SimTK_ASSERT2_ALWAYS(final_time >= start_time,
"InverseKinematicsTool final time (%f) is before start time (%f).",
Expand All @@ -337,8 +182,8 @@ bool InverseKinematicsTool::run()

// create the solver given the input data
InverseKinematicsSolver ikSolver(*_model, markersReference,
coordinateReferences, _constraintWeight);
ikSolver.setAccuracy(_accuracy);
coordinateReferences, get_constraint_weight());
ikSolver.setAccuracy(get_accuracy());
s.updTime() = times[start_ix];
ikSolver.assemble(s);
kinematicsReporter->begin(s);
Expand All @@ -352,9 +197,9 @@ bool InverseKinematicsTool::run()
SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));

Storage *modelMarkerLocations = _reportMarkerLocations ?
Storage *modelMarkerLocations = get_report_marker_locations() ?
new Storage(Nframes, "ModelMarkerLocations") : nullptr;
Storage *modelMarkerErrors = _reportErrors ?
Storage *modelMarkerErrors = get_report_errors() ?
new Storage(Nframes, "ModelMarkerErrors") : nullptr;

Stopwatch watch;
Expand All @@ -365,7 +210,7 @@ bool InverseKinematicsTool::run()
// show progress line every 1000 frames so users see progress
if (std::remainder(i - start_ix, 1000) == 0 && i != start_ix)
cout << "Solved " << i - start_ix << " frames..." << std::endl;
if(_reportErrors){
if(get_report_errors()){
Array<double> markerErrors(0.0, 3);
double totalSquaredMarkerError = 0.0;
double maxSquaredMarkerError = 0.0;
Expand Down Expand Up @@ -393,7 +238,7 @@ bool InverseKinematicsTool::run()
<< ikSolver.getMarkerNameForIndex(worst) << ")" << endl;
}

if(_reportMarkerLocations){
if(get_report_marker_locations()){
ikSolver.computeCurrentMarkerLocations(markerLocations);
Array<double> locations(0.0, 3*nm);
for(int j=0; j<nm; ++j){
Expand All @@ -411,8 +256,10 @@ bool InverseKinematicsTool::run()

// Do the maneuver to change then restore working directory
// so that output files are saved to same folder as setup file.
if (_outputMotionFileName!= "" && _outputMotionFileName!="Unassigned"){
kinematicsReporter->getPositionStorage()->print(_outputMotionFileName);
if (get_output_motion_file() != "" &&
get_output_motion_file() != "Unassigned") {
kinematicsReporter->getPositionStorage()->print(
get_output_motion_file());
}
// Remove the analysis we added to the model, this also deletes it
_model->removeAnalysis(kinematicsReporter);
Expand Down Expand Up @@ -472,7 +319,8 @@ bool InverseKinematicsTool::run()
"please see messages window for details..."));
}

if (modelFromFile) delete _model;
if (modelFromFile)
_model.reset();

return success;
}
Expand Down Expand Up @@ -511,9 +359,7 @@ void InverseKinematicsTool::updateFromXMLNode(SimTK::Xml::Element& aNode, int ve
for (; p!= trialIter->node_end(); ++p) {
iter->insertNodeAfter( iter->node_end(), p->clone());
}
iter->insertNodeAfter( iter->node_end(), Xml::Comment(_constraintWeightProp.getComment()));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just to double check, the comments will still be in the files due to the way the new property system works, correct?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, the goal is to construct an object with the correct property values, comments on the other hand are never read so setting them here is unnecessary

iter->insertNodeAfter( iter->node_end(), Xml::Element("constraint_weight", "20.0"));
iter->insertNodeAfter( iter->node_end(), Xml::Comment(_accuracyProp.getComment()));
iter->insertNodeAfter( iter->node_end(), Xml::Element("accuracy", "1e-4"));
// erase node for IKTrialSet
iter->eraseNode(toolIter);
Expand Down Expand Up @@ -545,9 +391,7 @@ void InverseKinematicsTool::updateFromXMLNode(SimTK::Xml::Element& aNode, int ve
for (; p!= trialIter->node_end(); ++p) {
root.insertNodeAfter( root.node_end(), p->clone());
}
root.insertNodeAfter( root.node_end(), Xml::Comment(_constraintWeightProp.getComment()));
root.insertNodeAfter( root.node_end(), Xml::Element("constraint_weight", "20.0"));
root.insertNodeAfter( root.node_end(), Xml::Comment(_accuracyProp.getComment()));
root.insertNodeAfter( root.node_end(), Xml::Element("accuracy", "1e-5"));
// erase node for IKTrialSet
root.eraseNode(toolIter);
Expand Down Expand Up @@ -577,8 +421,8 @@ void InverseKinematicsTool::populateReferences(MarkersReference& markersReferenc
FunctionSet *coordFunctions = NULL;
// Load the coordinate data
// bool haveCoordinateFile = false;
if (_coordinateFileName != "" && _coordinateFileName != "Unassigned") {
Storage coordinateValues(_coordinateFileName);
if (get_coordinate_file() != "" && get_coordinate_file() != "Unassigned") {
Storage coordinateValues(get_coordinate_file());
// Convert degrees to radian (TODO: this needs to have a check that the storage is, in fact, in degrees!)
_model->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
// haveCoordinateFile = true;
Expand All @@ -589,9 +433,9 @@ void InverseKinematicsTool::populateReferences(MarkersReference& markersReferenc
// Loop through old "IKTaskSet" and assign weights to the coordinate and marker references
// For coordinates, create the functions for coordinate reference values
int index = 0;
for (int i = 0; i < _ikTaskSet.getSize(); i++) {
if (!_ikTaskSet[i].getApply()) continue;
if (IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i])) {
for (int i = 0; i < get_IKTaskSet().getSize(); i++) {
if (!get_IKTaskSet()[i].getApply()) continue;
if (IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&get_IKTaskSet()[i])) {
CoordinateReference *coordRef = NULL;
if (coordTask->getValueType() == IKCoordinateTask::FromFile) {
if (!coordFunctions)
Expand Down Expand Up @@ -619,7 +463,7 @@ void InverseKinematicsTool::populateReferences(MarkersReference& markersReferenc

coordinateReferences.push_back(*coordRef);
}
else if (IKMarkerTask *markerTask = dynamic_cast<IKMarkerTask *>(&_ikTaskSet[i])) {
else if (IKMarkerTask *markerTask = dynamic_cast<IKMarkerTask *>(&get_IKTaskSet()[i])) {
if (markerTask->getApply()) {
// Only track markers that have a task and it is "applied"
markerWeights.adoptAndAppend(
Expand All @@ -631,7 +475,7 @@ void InverseKinematicsTool::populateReferences(MarkersReference& markersReferenc
//Read in the marker data file and set the weights for associated markers.
//Markers in the model and the marker file but not in the markerWeights are
//ignored
markersReference.initializeFromMarkersFile(_markerFileName, markerWeights);
markersReference.initializeFromMarkersFile(get_marker_file(), markerWeights);
}


Loading