Skip to content

Commit

Permalink
Merge pull request #118 from keenon/regularize_joint_accelerations
Browse files Browse the repository at this point in the history 
Add joint acceleration regularization
  • Loading branch information
@nickbianco
nickbianco authored Mar 3, 2023
2 parents 852ab1a + ee5531b commit 94311a9
Show file tree
Hide file tree
Showing 5 changed files with 159 additions and 25 deletions.
2 changes: 1 addition & 1 deletion VERSION.txt
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
0.8.80
0.8.81
104 changes: 86 additions & 18 deletions dart/biomechanics/DynamicsFitter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5200,6 +5200,7 @@ s_t DynamicsFitProblem::computeLoss(Eigen::VectorXs x, bool logExplanation)
s_t markerRMS = 0.0;
s_t poseRegularization = 0.0;
s_t accRegularization = 0.0;
s_t jointAccRegularization = 0.0;
s_t jointRMS = 0.0;
s_t axisRMS = 0.0;
int markerCount = 0;
Expand Down Expand Up @@ -5254,6 +5255,14 @@ s_t DynamicsFitProblem::computeLoss(Eigen::VectorXs x, bool logExplanation)
accRegularization += cost;
assert(!isnan(accRegularization));
}
if (mConfig.mRegularizeJointAcc > 0)
{
jointAccRegularization +=
mConfig.mRegularizeJointAcc *
(1.0 / totalAccTimesteps) *
block.acc.col(t).squaredNorm();
assert(!isnan(jointAccRegularization));
}
}

// Add marker RMS errors to every timestep
Expand Down Expand Up @@ -5319,6 +5328,7 @@ s_t DynamicsFitProblem::computeLoss(Eigen::VectorXs x, bool logExplanation)
sum += linearNewtonError;
sum += residualRMS;
sum += accRegularization;
sum += jointAccRegularization;
markerRMS *= mConfig.mMarkerWeight;
if (markerCount > 0)
{
Expand Down Expand Up @@ -5356,6 +5366,7 @@ struct LossExplanation
s_t markerRMS;
s_t poseRegularization;
s_t accRegularization;
s_t jointAccRegularization;
s_t jointRMS;
s_t axisRMS;
int markerCount;
Expand Down Expand Up @@ -5484,6 +5495,7 @@ s_t DynamicsFitProblem::computeLossParallel(
threadLoss.markerRMS = 0.0;
threadLoss.poseRegularization = 0.0;
threadLoss.accRegularization = 0.0;
threadLoss.jointAccRegularization = 0.0;
threadLoss.jointRMS = 0.0;
threadLoss.axisRMS = 0.0;
threadLoss.markerCount = 0;
Expand Down Expand Up @@ -5563,6 +5575,14 @@ s_t DynamicsFitProblem::computeLossParallel(
threadLoss.accRegularization += cost;
assert(!isnan(threadLoss.accRegularization));
}
if (mConfig.mRegularizeJointAcc > 0)
{
threadLoss.jointAccRegularization +=
mConfig.mRegularizeJointAcc *
(1.0 / totalAccTimesteps) *
block.acc.col(t).squaredNorm();
assert(!isnan(threadLoss.jointAccRegularization));
}
}

// Add marker RMS errors to every timestep
Expand Down Expand Up @@ -5642,6 +5662,7 @@ s_t DynamicsFitProblem::computeLossParallel(
s_t markerRMS = 0.0;
s_t poseRegularization = 0.0;
s_t accRegularization = 0.0;
s_t jointAccRegularization = 0.0;
s_t jointRMS = 0.0;
s_t axisRMS = 0.0;
int markerCount = 0;
Expand All @@ -5652,6 +5673,8 @@ s_t DynamicsFitProblem::computeLossParallel(
markerRMS += threadLossExplanations[threadIdx].markerRMS;
poseRegularization += threadLossExplanations[threadIdx].poseRegularization;
accRegularization += threadLossExplanations[threadIdx].accRegularization;
jointAccRegularization +=
threadLossExplanations[threadIdx].jointAccRegularization;
jointRMS += threadLossExplanations[threadIdx].jointRMS;
axisRMS += threadLossExplanations[threadIdx].axisRMS;
markerCount += threadLossExplanations[threadIdx].markerCount;
Expand All @@ -5660,6 +5683,7 @@ s_t DynamicsFitProblem::computeLossParallel(
sum += linearNewtonError;
sum += residualRMS;
sum += accRegularization;
sum += jointAccRegularization;
markerRMS *= mConfig.mMarkerWeight;
if (markerCount > 0)
{
Expand Down Expand Up @@ -6253,6 +6277,12 @@ Eigen::VectorXs DynamicsFitProblem::computeGradient(Eigen::VectorXs x)
neural::WithRespectTo::ACCELERATION,
mConfig.mRegularizeAccUseL1);
}
if (mConfig.mRegularizeJointAcc > 0)
{
accGrad += mConfig.mRegularizeJointAcc *
(2.0 / totalAccTimesteps) *
block.acc.col(t);
}
}

// Record marker gradients
Expand Down Expand Up @@ -6971,6 +7001,13 @@ Eigen::VectorXs DynamicsFitProblem::computeGradientParallel(Eigen::VectorXs x)
neural::WithRespectTo::ACCELERATION,
mConfig.mRegularizeAccUseL1);
}
if (mConfig.mRegularizeJointAcc > 0)
{
accGrad +=
mConfig.mRegularizeJointAcc *
(2.0 / totalAccTimesteps) *
block.acc.col(t);
}
}

// Record marker gradients
Expand Down Expand Up @@ -7922,6 +7959,7 @@ DynamicsFitProblemConfig::DynamicsFitProblemConfig(
mRegularizeAccBodyWeights(
Eigen::VectorXs::Ones(skeleton->getNumBodyNodes())),
mRegularizeAccUseL1(false),
mRegularizeJointAcc(0.0),
mRegularizeMasses(0.0),
mRegularizeCOMs(0.0),
mRegularizeInertias(0.0),
Expand Down Expand Up @@ -7987,6 +8025,7 @@ DynamicsFitProblemConfig& DynamicsFitProblemConfig::setDefaults(bool l1)
mResidualTorqueMultiple = 2.0;
mRegularizeAcc = 0;
mRegularizeAccUseL1 = false;
mRegularizeJointAcc = 0;
mRegularizeMasses = 1.0;
mRegularizeCOMs = 1.0;
mRegularizeInertias = 1.0;
Expand Down Expand Up @@ -8186,6 +8225,14 @@ DynamicsFitProblemConfig::setRegularizeSpatialAccUseL1(bool l1)
return *(this);
}

//==============================================================================
DynamicsFitProblemConfig&
DynamicsFitProblemConfig::setRegularizeJointAcc(s_t value)
{
mRegularizeJointAcc = value;
return *(this);
}

//==============================================================================
DynamicsFitProblemConfig& DynamicsFitProblemConfig::setRegularizeMasses(
s_t value)
Expand Down Expand Up @@ -15122,7 +15169,8 @@ void DynamicsFitter::writeCSVData(
std::string path,
std::shared_ptr<DynamicsInitialization> init,
int trial,
bool useAdjustedGRFs)
bool useAdjustedGRFs,
std::vector<double> timestamps)
{
// time, pos, vel, acc, [contact] * feet, [cop, wrench] * feet
std::ofstream csvFile;
Expand Down Expand Up @@ -15174,31 +15222,51 @@ void DynamicsFitter::writeCSVData(
(void)init;
(void)trial;

s_t time = 0.0;
s_t dt = init->trialTimesteps[trial];
bool useTimestamps = false;
if (!timestamps.empty()) {
if (timestamps.size() == init->poseTrials[trial].cols()) {
useTimestamps = true;
} else {
std::cout << "WARNING, writeCSVData: The timestamps vector does not "
<< "match the number of row in the trajectory. Using a generic "
<< "time column instead..."
<< std::endl;
}
}

ResidualForceHelper helper(mSkeleton, init->grfBodyIndices);

time += dt;

for (int t = 1; t < init->poseTrials[trial].cols() - 1; t++)
s_t time = 0.0;
s_t dt = init->trialTimesteps[trial];
int nrows = init->poseTrials[trial].cols();
for (int t = 0; t < nrows; t++)
{
csvFile << std::endl;

csvFile << time;
if (useTimestamps) {
csvFile << timestamps[t];
} {
csvFile << time;
}

Eigen::VectorXs q = init->poseTrials[trial].col(t);
Eigen::VectorXs dq
= (init->poseTrials[trial].col(t) - init->poseTrials[trial].col(t - 1))
/ dt;
Eigen::VectorXs ddq = (init->poseTrials[trial].col(t + 1)
- 2 * init->poseTrials[trial].col(t)
+ init->poseTrials[trial].col(t - 1))
/ (dt * dt);
Eigen::VectorXs tau = useAdjustedGRFs
? init->perfectTorques[trial].col(t)
: helper.calculateInverseDynamics(
q, dq, ddq, init->grfTrials[trial].col(t));
Eigen::VectorXs dq = Eigen::VectorXs::Zero(q.size());
Eigen::VectorXs ddq = Eigen::VectorXs::Zero(q.size());
Eigen::VectorXs tau = Eigen::VectorXs::Zero(q.size());
if (t > 0 || t < nrows-1) {
dq = (init->poseTrials[trial].col(t)
- init->poseTrials[trial].col(t - 1))
/ dt;
ddq = (init->poseTrials[trial].col(t + 1)
- 2 * init->poseTrials[trial].col(t)
+ init->poseTrials[trial].col(t - 1))
/ (dt * dt);
tau = useAdjustedGRFs
? init->perfectTorques[trial].col(t)
: helper.calculateInverseDynamics(
q, dq, ddq, init->grfTrials[trial].col(t));
}

Eigen::VectorXs footContactData
= Eigen::VectorXs::Zero(9 * init->grfBodyIndices.size());
if (useAdjustedGRFs)
Expand Down
5 changes: 4 additions & 1 deletion dart/biomechanics/DynamicsFitter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -900,6 +900,7 @@ class DynamicsFitProblemConfig
DynamicsFitProblemConfig& setRegularizeSpatialAccBodyWeights(
Eigen::VectorXs bodyWeights);
DynamicsFitProblemConfig& setRegularizeSpatialAccUseL1(bool l1);
DynamicsFitProblemConfig& setRegularizeJointAcc(s_t value);

DynamicsFitProblemConfig& setResidualTorqueMultiple(s_t value);
DynamicsFitProblemConfig& setRegularizeMasses(s_t value);
Expand Down Expand Up @@ -952,6 +953,7 @@ class DynamicsFitProblemConfig
s_t mRegularizeAcc;
Eigen::VectorXs mRegularizeAccBodyWeights;
bool mRegularizeAccUseL1;
s_t mRegularizeJointAcc;

s_t mResidualTorqueMultiple;
s_t mRegularizeMasses;
Expand Down Expand Up @@ -1549,7 +1551,8 @@ class DynamicsFitter
std::string path,
std::shared_ptr<DynamicsInitialization> init,
int trial,
bool useAdjustedGRFs = false);
bool useAdjustedGRFs = false,
std::vector<double> timestamps = {});

// This writes a random-seekable binary format to disk
void writeSubjectOnDisk(
Expand Down
3 changes: 2 additions & 1 deletion python/_nimblephysics/biomechanics/DynamicsFitter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -758,7 +758,8 @@ class DynamicsFitter
::py::arg("path"),
::py::arg("init"),
::py::arg("trialIndex"),
::py::arg("useAdjustedGRFs") = false)
::py::arg("useAdjustedGRFs") = false,
::py::arg("timestamps") = ::py::list())
.def(
"writeSubjectOnDisk",
&dart::biomechanics::DynamicsFitter::writeSubjectOnDisk,
Expand Down
70 changes: 66 additions & 4 deletions unittests/unit/test_DynamicsFitter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include "GradientTestUtils.hpp"
#include "TestHelpers.hpp"

//#define JACOBIAN_TESTS
// #define JACOBIAN_TESTS
// #define ALL_TESTS

using namespace dart;
Expand Down Expand Up @@ -3115,7 +3115,8 @@ std::shared_ptr<DynamicsInitialization> runEngine(
"Generated in test_DynamicsFitter");

fitter.writeCSVData(
"../../../javascript/src/data/movement2.csv", init, trajectoryIndex);
"../../../javascript/src/data/movement2.csv", init, trajectoryIndex,
false);
fitter.saveDynamicsToGUI(
"../../../javascript/src/data/movement2.bin",
init,
Expand Down Expand Up @@ -4914,6 +4915,67 @@ TEST(DynamicsFitter, FIT_PROBLEM_GRAD_SPATIAL_ACC_REG)
}
#endif

#ifdef JACOBIAN_TESTS
TEST(DynamicsFitter, FIT_PROBLEM_GRAD_JOINT_ACC_REG)
{
std::vector<std::string> motFiles;
std::vector<std::string> c3dFiles;
std::vector<std::string> trcFiles;
std::vector<std::string> grfFiles;

motFiles.push_back("dart://sample/grf/Subject4/IK/walking1_ik.mot");
trcFiles.push_back("dart://sample/grf/Subject4/MarkerData/walking1.trc");
grfFiles.push_back("dart://sample/grf/Subject4/ID/walking1_grf.mot");

OpenSimFile standard = OpenSimParser::parseOsim(
"dart://sample/grf/Subject4/Models/"
"optimized_scale_and_markers.osim");

std::vector<std::string> footNames;
footNames.push_back("calcn_r");
footNames.push_back("calcn_l");

std::shared_ptr<DynamicsInitialization> init = createInitialization(
standard.skeleton,
standard.markersMap,
standard.trackingMarkers,
footNames,
motFiles,
c3dFiles,
trcFiles,
grfFiles,
20);

DynamicsFitProblemConfig config(standard.skeleton);
config.setRegularizeJointAcc(1);

config.setIncludeBodyScales(true);
config.setIncludeCOMs(true);
config.setIncludeInertias(true);
config.setIncludeMarkerOffsets(true);
config.setIncludeMasses(true);
config.setIncludePoses(true);

DynamicsFitProblem problem(
init, standard.skeleton, standard.trackingMarkers, config);

std::cout << "Problem dim: " << problem.getProblemSize() << std::endl;

Eigen::VectorXs x = problem.flatten();
Eigen::VectorXs analytical = problem.computeGradient(x);
Eigen::VectorXs fd = problem.finiteDifferenceGradient(x);

bool result = problem.debugErrors(fd, analytical, 1e-6);
if (result)
{
std::cout << "Gradient of DynamicsFitProblem linear Newton not equal!"
<< std::endl;
EXPECT_FALSE(result);
return;
}
}
#endif

#ifdef JACOBIAN_TESTS
TEST(DynamicsFitter, FIT_PROBLEM_GRAD_MARKERS_L2)
{
Expand Down Expand Up @@ -6325,7 +6387,7 @@ TEST(DynamicsFitter, KirstenTest)
}
#endif

//#ifdef ALL_TESTS
#ifdef ALL_TESTS
TEST(DynamicsFitter, HamnerMultipleTrials)
{
std::vector<std::string> motFiles;
Expand Down Expand Up @@ -6360,7 +6422,7 @@ TEST(DynamicsFitter, HamnerMultipleTrials)
false,
maxTrialsToSolveMassOver);
}
//#endif
#endif

#ifdef ALL_TESTS
TEST(DynamicsFitter, MARKERS_TO_DYNAMICS_OPENCAP)
Expand Down

0 comments on commit 94311a9

Please sign in to comment.