Skip to content
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

Update MocoTrajectory::generateSpeedsFromValues() to not override auxiliary states #3603

Merged
merged 3 commits into from
Nov 14, 2023
Merged

Update MocoTrajectory::generateSpeedsFromValues() to not override auxiliary states #3603

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bfc3584
Fix MocoTrajectory::generateSpeedsFromValues() so it does not overrid…
nickbianco Nov 8, 2023
0a8e29b
Update Moco changelog
nickbianco Nov 8, 2023
ad6029a
Merge branch 'main' into moco_trajectory_generate_fix
nickbianco Nov 9, 2023
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
5 changes: 4 additions & 1 deletion CHANGELOG_MOCO.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,11 @@
Moco Change Log
===============

1.2.2
1.3.0
-----
- 2023-11-08: Fixed a bug where `MocoTrajectory::generateSpeedsFromValues()` was
accidentally deleting auxiliary state values.

- 2023-10-25: Fixed a bug preventing deserialization of `MocoFrameDistanceConstraint`.

- 2023-10-25: Locked coordinates are now explicitly disallowed in `MocoProblem`s,
Expand Down
29 changes: 26 additions & 3 deletions OpenSim/Moco/MocoTrajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -369,17 +369,24 @@ void MocoTrajectory::insertControlsTrajectory(
void MocoTrajectory::generateSpeedsFromValues() {
auto valuesTable = exportToValuesTable();
int numValues = (int)valuesTable.getNumColumns();
const std::vector<std::string>& valueNames = valuesTable.getColumnLabels();
OPENSIM_THROW_IF(!numValues, Exception,
"Tried to compute speeds from coordinate values, but no values "
"exist in the trajectory.");
m_states.resize(getNumTimes(), 2*numValues);

// Save the auxiliary states to fill back in later.
SimTK::Matrix auxiliaryStates = getAuxiliaryStatesTrajectory();
std::vector<std::string> auxiliaryStateNames = getAuxiliaryStateNames();

// Resize the states trajectory to hold the values, speeds, and any
// auxiliary states.
m_states.resize(getNumTimes(), 2*numValues + getNumAuxiliaryStates());

// Spline the values trajectory.
GCVSplineSet splines(valuesTable, {}, std::min(getNumTimes() - 1, 5));

// Create the speed names.
std::vector<std::string> stateNames;
const std::vector<std::string>& valueNames =
valuesTable.getColumnLabels();
std::vector<std::string> speedNames;
for (int ivalue = 0; ivalue < numValues; ++ivalue) {
std::string name(valueNames[ivalue]);
Expand All @@ -389,6 +396,14 @@ void MocoTrajectory::generateSpeedsFromValues() {
speedNames.push_back(name);
}

// Append the speed names to the state names.
stateNames.insert(stateNames.end(), speedNames.begin(), speedNames.end());

// Append the auxiliary state names to the state names.
stateNames.insert(stateNames.end(), auxiliaryStateNames.begin(),
auxiliaryStateNames.end());

// Fill in the coordinate and speed values based on the splined values.
SimTK::Vector currTime(1, SimTK::NaN);
for (int ivalue = 0; ivalue < numValues; ++ivalue) {
const auto& name = valueNames[ivalue];
Expand All @@ -402,6 +417,14 @@ void MocoTrajectory::generateSpeedsFromValues() {
}
}

// Fill back in any auxiliary state values.
for (int iaux = 0; iaux < getNumAuxiliaryStates(); ++iaux) {
for (int itime = 0; itime < m_time.size(); ++itime) {
currTime[0] = m_time[itime];
m_states(itime, 2*numValues + iaux) = auxiliaryStates(itime, iaux);
}
}

// Assign state names.
m_state_names = stateNames;
}
Expand Down
73 changes: 73 additions & 0 deletions OpenSim/Moco/MocoTrajectory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -472,9 +472,20 @@ class OSIMMOCO_API MocoTrajectory {
}

int getNumSpeeds() const {
ensureUnsealed();
return (int)getSpeedIndices().size();
}

int getNumMultibodyStates() const {
ensureUnsealed();
return (int)getMultibodyStateIndices().size();
}

int getNumAuxiliaryStates() const {
ensureUnsealed();
return (int)getAuxiliaryStateIndices().size();
}

int getNumAccelerations() const {
ensureUnsealed();
return (int)getAccelerationIndices().size();
Expand Down Expand Up @@ -528,6 +539,29 @@ class OSIMMOCO_API MocoTrajectory {
}
return speedNames;
}
std::vector<std::string> getMultibodyStateNames() const {
ensureUnsealed();
std::vector<std::string> multibodyStateNames;
for (const auto& name : m_state_names) {
if (name.find("/value") != std::string::npos) {
multibodyStateNames.push_back(name);
} else if (name.find("/speed") != std::string::npos) {
multibodyStateNames.push_back(name);
}
}
return multibodyStateNames;
}
std::vector<std::string> getAuxiliaryStateNames() const {
ensureUnsealed();
std::vector<std::string> auxiliaryStateNames;
for (const auto& name : m_state_names) {
if (name.find("/value") == std::string::npos &&
name.find("/speed") == std::string::npos) {
auxiliaryStateNames.push_back(name);
}
}
return auxiliaryStateNames;
}
std::vector<std::string> getAccelerationNames() const {
ensureUnsealed();
std::vector<std::string> accelerationNames;
Expand Down Expand Up @@ -589,6 +623,20 @@ class OSIMMOCO_API MocoTrajectory {
return {m_states.block(0, indices[0],
m_states.nrow(), getNumSpeeds())};
}
SimTK::Matrix getMultibodyStatesTrajectory() const {
ensureUnsealed();
auto indices = getMultibodyStateIndices();
if (indices.empty()) indices.push_back(0);
return {m_states.block(0, indices[0],
m_states.nrow(), getNumMultibodyStates())};
}
SimTK::Matrix getAuxiliaryStatesTrajectory() const {
ensureUnsealed();
auto indices = getAuxiliaryStateIndices();
if (indices.empty()) indices.push_back(0);
return {m_states.block(0, indices[0],
m_states.nrow(), getNumAuxiliaryStates())};
}
SimTK::Matrix getAccelerationsTrajectory() const {
ensureUnsealed();
auto indices = getAccelerationIndices();
Expand Down Expand Up @@ -877,6 +925,31 @@ class OSIMMOCO_API MocoTrajectory {
}
return speedIndices;
}
std::vector<int> getMultibodyStateIndices() const {
ensureUnsealed();
std::vector<int> multibodyStateIndices;
auto valueIndices = getValueIndices();
auto speedIndices = getSpeedIndices();
multibodyStateIndices.reserve(
valueIndices.size() + speedIndices.size());
multibodyStateIndices.insert(multibodyStateIndices.end(),
valueIndices.begin(), valueIndices.end());
multibodyStateIndices.insert(multibodyStateIndices.end(),
speedIndices.begin(), speedIndices.end());
return multibodyStateIndices;
}
std::vector<int> getAuxiliaryStateIndices() const {
ensureUnsealed();
std::vector<int> auxiliaryStateIndices;
for (int i = 0; i < (int)m_state_names.size(); ++i) {
const auto& name = m_state_names[i];
if (name.find("/value") == std::string::npos &&
name.find("/speed") == std::string::npos) {
auxiliaryStateIndices.push_back(i);
}
}
return auxiliaryStateIndices;
}
std::vector<int> getAccelerationIndices() const {
ensureUnsealed();
std::vector<int> accelerationIndices;
Expand Down
35 changes: 34 additions & 1 deletion OpenSim/Moco/Test/testMocoInterface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2126,8 +2126,41 @@ TEST_CASE("Objective breakdown", "[casadi]") {
CHECK(solution.getObjectiveTerm("goal_b") == Approx(0.01 * 7.3));
}

TEST_CASE("generateSpeedsFromValues() does not overwrite auxiliary states.") {
int N = 20;
SimTK::Vector time = createVectorLinspace(20, 0.0, 1.0);
std::vector<std::string> snames{"/jointset/joint/coord/value",
"/jointset/joint/coord/speed",
"/forceset/muscle/normalized_tendon_force"};
std::vector<std::string> cnames{"/forceset/muscle"};
std::vector<std::string> dnames{
"/forceset/muscle/implicitderiv_normalized_tendon_force"};
SimTK::Matrix states = SimTK::Test::randMatrix(N, 3);
SimTK::Matrix controls = SimTK::Test::randMatrix(N, 1);
SimTK::Matrix derivatives = SimTK::Test::randMatrix(N, 1);
MocoTrajectory traj(time, snames, cnames, {}, dnames, {}, states, controls,
SimTK::Matrix(), derivatives, SimTK::RowVector());

traj.generateSpeedsFromValues();
CHECK(traj.getNumStates() == 3);
CHECK(traj.getStateNames() == snames);
CHECK(traj.getNumDerivatives() == 1);
CHECK(traj.getDerivativeNames() == dnames);
CHECK(traj.getNumAuxiliaryStates() == 1);
std::vector<std::string>
auxnames{"/forceset/muscle/normalized_tendon_force"};
CHECK(traj.getAuxiliaryStateNames() == auxnames);

SimTK::Matrix auxiliaryStates = traj.getAuxiliaryStatesTrajectory();
double error = 0.0;
for (int irow = 0; irow < states.nrow(); ++irow) {
error += pow(states(irow, 2) - auxiliaryStates(irow, 0), 2);
}
SimTK_TEST_EQ(error, 0.0);
}

TEST_CASE("generateAccelerationsFromXXX() does not overwrite existing "
"non-accleration derivatives.") {
"non-acceleration derivatives.") {
int N = 20;
SimTK::Vector time = createVectorLinspace(20, 0.0, 1.0);
std::vector<std::string> snames{"/jointset/joint/coord/value",
Expand Down