From 41aa96c2e8740acf2a8e219da76d5931716ba443 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Tue, 14 May 2024 11:44:19 -0700 Subject: [PATCH 1/3] Support for ablation studies for AddBiomechanics ECCV paper --- CMakeLists.txt | 1 - dart/biomechanics/DynamicsFitter.cpp | 400 ++++++++++++++++++++++++- dart/biomechanics/DynamicsFitter.hpp | 9 +- dart/biomechanics/ForcePlate.cpp | 27 +- dart/biomechanics/OpenSimParser.cpp | 20 +- dart/server/GUIStateMachine.cpp | 12 +- unittests/unit/test_DynamicsFitter.cpp | 55 ++++ 7 files changed, 514 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 87e2682a2..b75651b5c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,6 @@ project(dart) # Add this to enable asserts in Release mode # add_definitions(-UNDEBUG) - string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPERCASE) # Use MACOSX_RPATH by default on OS X. This was added in CMake 2.8.12 and diff --git a/dart/biomechanics/DynamicsFitter.cpp b/dart/biomechanics/DynamicsFitter.cpp index 860e286d7..7a646bc15 100644 --- a/dart/biomechanics/DynamicsFitter.cpp +++ b/dart/biomechanics/DynamicsFitter.cpp @@ -11215,7 +11215,8 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory( int maxTrialsToSolveMassOver, bool detectExternalForce, int driftCorrectionBlurRadius, - int driftCorrectionBlurInterval) + int driftCorrectionBlurInterval, + s_t regularizeUnobservedTimesteps) { if (mSkeleton->getRootJoint()->getType() != dynamics::EulerFreeJoint::getStaticType() @@ -11292,7 +11293,6 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory( { const Eigen::Vector3s gravity = Eigen::Vector3s(0, -9.81, 0); s_t regularizeForcePlateRotations = 10.0; - s_t regularizeUnobservedTimesteps = 0.1; s_t regularizeScaleGRF = 2.0; const s_t originalMass = mSkeleton->getMass(); @@ -11618,7 +11618,12 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory( // unifiedPositions - unifiedGravityOffset); Eigen::LeastSquaresConjugateGradient solver; solver.setTolerance(1e-9); - solver.setMaxIterations(unifiedLinearMap.rows() * 10); + int maxIterations = unifiedLinearMap.rows() * 10; + if (maxIterations > 1000) + { + maxIterations = 1000; + } + solver.setMaxIterations(maxIterations); solver.compute(unifiedLinearMap); Eigen::VectorXs tentativeResult = solver.solve(unifiedPositions - unifiedGravityOffset); @@ -12201,6 +12206,395 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory( return false; } +//============================================================================== +// 1. This is an ablation study, where we just try to separately solve for +// each section of observed GRF, and leave alone the unobserved sections. +bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectoryAblation( + std::shared_ptr init, int maxTrialsToSolveMassOver) +{ + std::cout << "WARNING: This method is NOT INTENDED FOR PRODUCTION USE! This " + "exists only for ablation studies." + << std::endl; + + int numTrials = init->poseTrials.size(); + (void)numTrials; + (void)maxTrialsToSolveMassOver; + + std::vector> trialGRFs; + std::vector> trialOriginalCOMs; + std::vector> trialOriginalCenteredCOMs; + std::vector> trialOriginalAccOffsets; + for (int i = 0; i < init->poseTrials.size(); i++) + { + trialGRFs.push_back(measuredGRFForces(init, i)); + trialOriginalCOMs.push_back(comPositions(init, i)); + trialOriginalCenteredCOMs.push_back(comPositionsToCenterResiduals(init, i)); + } + (void)trialOriginalCenteredCOMs; + + for (int i = 0; i < init->poseTrials.size(); i++) + { + std::vector accOffset; + s_t dt = init->trialTimesteps[i]; + + for (int t = 0; t < init->poseTrials[i].cols(); t++) + { + if (init->probablyMissingGRF.at(i).at(t) == MissingGRFStatus::yes) + { + } + // Compute the offset from the difference between a finite-difference's + // COM acceleration and an analytical one + if (t > 0 && t < init->poseTrials[i].cols() - 1) + { + Eigen::VectorXs q = init->poseTrials[i].col(t); + Eigen::VectorXs dq + = mSkeleton->getPositionDifferences( + init->poseTrials[i].col(t), init->poseTrials[i].col(t - 1)) + / dt; + Eigen::VectorXs ddq + = (mSkeleton->getPositionDifferences( + init->poseTrials[i].col(t + 1), init->poseTrials[i].col(t)) + - mSkeleton->getPositionDifferences( + init->poseTrials[i].col(t), init->poseTrials[i].col(t - 1))) + / (dt * dt); + mSkeleton->setPositions(q); + mSkeleton->setVelocities(dq); + mSkeleton->setAccelerations(ddq); + + Eigen::Vector3s analyticalAcc = mSkeleton->getCOMLinearAcceleration(); + Eigen::Vector3s fdAcc + = (trialOriginalCOMs[i][t + 1] - 2 * trialOriginalCOMs[i][t] + + trialOriginalCOMs[i][t - 1]) + / (dt * dt); + Eigen::Vector3s offset = analyticalAcc - fdAcc; + accOffset.push_back(offset); + } + } + trialOriginalAccOffsets.push_back(accOffset); + } + + const Eigen::Vector3s gravity = Eigen::Vector3s(0, -9.81, 0); + + const s_t originalMass = mSkeleton->getMass(); + + std::vector trialLinearMaps; + std::vector trialOriginalPositions; + std::vector trialOriginalGravityOffsets; + + std::cout << "Zeroing linear residuals in the COM trajectory" << std::endl; + + const int translationDofsStart + = mSkeleton->getRootJoint()->getDof(3)->getIndexInSkeleton(); + + std::vector masses; + for (int i = 0; i < init->poseTrials.size(); i++) + { + std::cout << "Constructing COM (linear) trajectory linear system for trial " + << i << "/" << init->poseTrials.size() << std::endl; + + const s_t dt = init->trialTimesteps[i]; + + // Collect all the start/stop points where we see GRF data + std::vector> startStopPoints; + int start = -1; + for (int t = 0; t < init->poseTrials[i].cols(); t++) + { + if (init->probablyMissingGRF.at(i).at(t) != MissingGRFStatus::yes) + { + if (start == -1) + { + start = t; + } + } + else + { + if (start != -1) + { + startStopPoints.push_back(std::pair(start, t - 1)); + start = -1; + } + } + } + if (start != -1) + { + startStopPoints.push_back( + std::pair(start, init->poseTrials[i].cols() - 1)); + } + + for (auto& pair : startStopPoints) + { + int numTimesteps = pair.second - pair.first + 1; + + // This has one col each for start COM positions, start COM velocities, + // and total mass. It outputs the physically consistent X, Y, Z + // coordinates of COM over time, given those inputs. Additionally, there + // are columns to account for small force plate rotation errors (3 angles + // per force plate) and for instantaneous COM velocities at time steps + // with missing ground reaction force data. + Eigen::MatrixXs trialLinearMapToPositions + = Eigen::MatrixXs::Zero(numTimesteps * 3, 6 + 1); + + // This is a vector with the original positions of our COM over time, + // which we will use to find good values for the position, velocity, and + // mass of our skeleton. + Eigen::VectorXs originalCOMPositions + = Eigen::VectorXs::Zero(numTimesteps * 3); + + Eigen::VectorXs comGravityOffset + = Eigen::VectorXs::Zero(numTimesteps * 3); + + const int startPosXCol = 0; + const int startPosYCol = 1; + const int startPosZCol = 2; + const int startVelXCol = 3; + const int startVelYCol = 4; + const int startVelZCol = 5; + const int massCol = 6; + + Eigen::Vector3s forceVelocity = Eigen::Vector3s::Zero(); + Eigen::Vector3s forceOffset = Eigen::Vector3s::Zero(); + Eigen::Vector3s gravityVelocity = Eigen::Vector3s::Zero(); + Eigen::Vector3s gravityOffset = Eigen::Vector3s::Zero(); + + for (int t = 0; t < numTimesteps; t++) + { + const int xRow = (t * 3); + const int yRow = (t * 3) + 1; + const int zRow = (t * 3) + 2; + + trialLinearMapToPositions(xRow, startPosXCol) = 1; + trialLinearMapToPositions(yRow, startPosYCol) = 1; + trialLinearMapToPositions(zRow, startPosZCol) = 1; + + trialLinearMapToPositions(xRow, startVelXCol) = t * dt; + trialLinearMapToPositions(yRow, startVelYCol) = t * dt; + trialLinearMapToPositions(zRow, startVelZCol) = t * dt; + + trialLinearMapToPositions(xRow, massCol) = forceOffset(0); + trialLinearMapToPositions(yRow, massCol) = forceOffset(1); + trialLinearMapToPositions(zRow, massCol) = forceOffset(2); + + comGravityOffset(xRow) = gravityOffset(0); + comGravityOffset(yRow) = gravityOffset(1); + comGravityOffset(zRow) = gravityOffset(2); + + forceVelocity += trialGRFs[i][t] * dt; + forceOffset += forceVelocity * dt; + gravityVelocity += gravity * dt; + + if (t > 0 && t < numTimesteps - 1) + { + gravityVelocity -= trialOriginalAccOffsets[i][t - 1] * dt; + } + gravityOffset += gravityVelocity * dt; + + originalCOMPositions(xRow) = trialOriginalCOMs[i][pair.first + t](0); + originalCOMPositions(yRow) = trialOriginalCOMs[i][pair.first + t](1); + originalCOMPositions(zRow) = trialOriginalCOMs[i][pair.first + t](2); + } + + trialLinearMaps.push_back(trialLinearMapToPositions); + trialOriginalPositions.push_back(originalCOMPositions); + trialOriginalGravityOffsets.push_back(comGravityOffset); + + Eigen::VectorXs solution + = trialLinearMapToPositions.householderQr().solve( + originalCOMPositions - comGravityOffset); + Eigen::VectorXs comTrajectory + = (trialLinearMapToPositions * solution) + comGravityOffset; + + s_t averageChange = 0.0; + for (int t = 0; t < numTimesteps; t++) + { + Eigen::Vector3s change = comTrajectory.segment<3>(t * 3) + - originalCOMPositions.segment<3>(t * 3); + init->poseTrials[i].block<3, 1>(translationDofsStart, t) += change; + averageChange += change.norm(); + } + averageChange /= numTimesteps; + s_t foundMass = 1.0 / solution(massCol); + std::cout << "Trial " << i << " frames " << pair.first << " to " + << pair.second << " moved COM by an average of " + << averageChange + << "m to achieve linear physical consistency. Found mass " + << foundMass << "kg" << std::endl; + masses.push_back(foundMass); + } + } + + s_t foundMass = 0.0; + for (int i = 0; i < masses.size(); i++) + { + foundMass += masses[i]; + } + foundMass /= masses.size(); + + // Now we want to scale every mass in the skeleton to get to + // `tentativeMass` total + s_t scalePercentage = foundMass / originalMass; + // init->bodyMasses *= scalePercentage; + // mSkeleton->setLinkMasses(init->bodyMasses); + mSkeleton->setLinkMasses(mSkeleton->getLinkMasses() * scalePercentage); + init->bodyMasses = mSkeleton->getLinkMasses(); + init->groupMasses = mSkeleton->getGroupMasses(); + init->regularizeGroupMassesTo = mSkeleton->getGroupMasses(); + + std::cout << "Found skeleton mass: " << foundMass << "kg" << std::endl; + + // Now that we've formulated our problem, we can attempt to solve: + + // Eigen::VectorXs tentativeResult = + // unifiedLinearMap.householderQr().solve( + // unifiedPositions - unifiedGravityOffset); + // Eigen::LeastSquaresConjugateGradient solver; + // solver.setTolerance(1e-9); + // int maxIterations = unifiedLinearMap.rows() * 10; + // if (maxIterations > 1000) + // { + // maxIterations = 1000; + // } + // solver.setMaxIterations(maxIterations); + // solver.compute(unifiedLinearMap); + // Eigen::VectorXs tentativeResult + // = solver.solve(unifiedPositions - unifiedGravityOffset); + // Eigen::VectorXs tentativeCOMTrajectory + // = (unifiedLinearMap * tentativeResult) + unifiedGravityOffset; + + // std::cout << "Solved!" << std::endl; + + // // Calculate the error + // double error + // = (unifiedPositions - unifiedGravityOffset - tentativeCOMTrajectory) + // .cwiseAbs() + // .mean(); + + // // Print the error + // std::cout << "COM trajectory solution average per-timestep error: " << + // error + // << "m" << std::endl; + + // // The linear map is in terms of "inverse mass", so we need to invert to + // // recover original mass. + // s_t foundMass = 1.0 / tentativeResult(massCol); + + // Go through and read out the solution values for each trial (or solve + // mini problems, if these trials were trimmed out) + // std::vector trialSolutionInput; + // std::vector trialSolutionOutput; + // colCursor = 0; + // rowCursor = 0; + // for (int trial = 0; trial < numTrials; trial++) + // { + // if (std::find(sampledTrials.begin(), sampledTrials.end(), trial) + // != sampledTrials.end()) + // { + // int trialCols = trialLinearMaps[trial].cols(); + // int trialRows = trialLinearMaps[trial].rows(); + + // trialSolutionInput.push_back( + // Eigen::VectorXs(tentativeResult.segment(colCursor, trialCols - + // 1))); + // trialSolutionOutput.push_back(Eigen::VectorXs( + // tentativeCOMTrajectory.segment(rowCursor, trialRows))); + + // // When written into the bulk linear map, the last column (mass) is + // // combined across all the trajectories, so we ignore it + // colCursor += trialCols - 1; + // rowCursor += trialRows; + // } + // else + // { + // // Otherwise, we have to solve using our found mass + // std::cout << "Solving trial " << trial + // << " COM trajectory while holding mass fixed" << std::endl; + // Eigen::MatrixXs A = trialLinearMaps[trial]; + // Eigen::VectorXs b = trialOriginalGravityOffsets[trial]; + + // Eigen::MatrixXs AFixedMass = A.block(0, 0, A.rows(), A.cols() - 1); + // Eigen::VectorXs bFixedMass = A.col(A.cols() - 1) * (1.0 / foundMass); + // bFixedMass.segment(0, b.size()) += b; + + // Eigen::VectorXs desired = Eigen::VectorXs::Zero(bFixedMass.size()); + // desired.segment(0, trialOriginalPositions[trial].size()) + // += trialOriginalPositions[trial]; + // desired -= bFixedMass; + + // // Old version, direct solve: + // // Eigen::VectorXs localResult + // // = AFixedMass.householderQr().solve(desired - bFixedMass); + // Eigen::LeastSquaresConjugateGradient solver; + // solver.setTolerance(1e-9); + // solver.setMaxIterations(200); + // solver.compute(AFixedMass); + // Eigen::VectorXs localResult = solver.solve(desired); + // Eigen::VectorXs recoveredTrajectory + // = AFixedMass * localResult + bFixedMass; + + // trialSolutionInput.push_back(localResult); + // trialSolutionOutput.push_back(recoveredTrajectory); + // } + // } + + // std::vector finalCOMTrajectory; + // // Set up and solve "drift correction" problems, for very long trials (> + // // twice the blur radius timesteps) where drift can become a major issue. + // for (int trial = 0; trial < numTrials; trial++) + // { + // int trialLength = init->poseTrials[trial].cols(); + // finalCOMTrajectory.push_back( + // trialSolutionOutput[trial].segment(0, trialLength * 3)); + // } + + // // Last, we need to decode the COM trajectory, and adjust our root + // // translations to hit that trajectory + // const int translationDofsStart + // = mSkeleton->getRootJoint()->getDof(3)->getIndexInSkeleton(); + // std::vector trialsChangedTooMuch; + // std::cout << "Checking " << numTrials << " trials for too much COM change" + // << std::endl; + // for (int trial = 0; trial < numTrials; trial++) + // { + // s_t totalChange = 0.0; + // std::vector originalCOMs = trialOriginalCOMs[trial]; + // std::vector comChanges; + // // In this case, we can just read the answer right off the unified + // // matrix + // for (int t = 0; t < originalCOMs.size(); t++) + // { + // Eigen::Vector3s change + // = finalCOMTrajectory[trial].segment<3>(t * 3) - originalCOMs[t]; + // const s_t dist = change.norm(); + // comChanges.push_back(dist); + // totalChange += dist; + // } + + // std::cout << "Trial " << trial << " moved COM by an average of " + // << (totalChange / originalCOMs.size()) + // << "m to achieve linear physical consistency." << std::endl; + // if ((totalChange / originalCOMs.size()) > 0.045) + // { + // std::cout << "Trial " << trial << " changed too much!" << std::endl; + // trialsChangedTooMuch.push_back(trial); + // } + // } + + // for (int trial = 0; trial < numTrials; trial++) + // { + // std::vector originalCOMs = trialOriginalCOMs[trial]; + + // for (int t = 0; t < originalCOMs.size(); t++) + // { + // Eigen::Vector3s change + // = finalCOMTrajectory[trial].segment<3>(t * 3) - originalCOMs[t]; + // init->poseTrials[trial].block<3, 1>(translationDofsStart, t) += change; + // } + // } + + // std::cout << "Finished zeroing linear residuals." << std::endl; + // return sampledTrials.size() > 0; + return true; +} + //============================================================================== // 1. Adjust the total mass of the body and the individual link masses for // each body, and change the initial positions and velocities of the body to diff --git a/dart/biomechanics/DynamicsFitter.hpp b/dart/biomechanics/DynamicsFitter.hpp index 474c912bc..88b41f056 100644 --- a/dart/biomechanics/DynamicsFitter.hpp +++ b/dart/biomechanics/DynamicsFitter.hpp @@ -1434,7 +1434,14 @@ class DynamicsFitter int maxTrialsToSolveMassOver = 4, bool detectExternalForce = true, int driftCorrectionBlurRadius = 250, - int driftCorrectionBlurInterval = 250); + int driftCorrectionBlurInterval = 250, + s_t regularizeUnobservedTimesteps = 0.01); + + // 1. This is an ablation study, where we just try to separately solve for + // each section of observed GRF, and leave alone the unobserved sections. + bool zeroLinearResidualsOnCOMTrajectoryAblation( + std::shared_ptr init, + int maxTrialsToSolveMassOver); // 1. Adjust the total mass of the body and the individual link masses for // each body, and change the initial positions and velocities of the body to diff --git a/dart/biomechanics/ForcePlate.cpp b/dart/biomechanics/ForcePlate.cpp index 80c2b4820..0b8540a48 100644 --- a/dart/biomechanics/ForcePlate.cpp +++ b/dart/biomechanics/ForcePlate.cpp @@ -26,6 +26,8 @@ void ForcePlate::autodetectNoiseThresholdAndClip( numExactlyZero++; } } + (void)numExactlyZero; + /* if (numExactlyZero > 5) { // If there are more than a few exactly zero force readings (so more @@ -36,6 +38,7 @@ void ForcePlate::autodetectNoiseThresholdAndClip( << std::endl; return; } + */ int numBins = 200; // Don't go higher than 200N as the max force, because above that we start @@ -50,8 +53,21 @@ void ForcePlate::autodetectNoiseThresholdAndClip( hist[bin]++; } + std::cout << numBins << " bins (from 0N to " << maxForce + << "N):" << std::endl; + for (int i = 0; i < numBins; i++) + { + std::cout << i << ": "; + s_t percent = (s_t)hist[i] / trialLen; + for (int j = 0; j < int(std::ceil(percent * 20)); j++) + { + std::cout << "*"; + } + std::cout << std::endl; + } + int avg_bin_value = trialLen / numBins; - auto max_it = std::max_element(hist.begin(), hist.end()); + auto max_it = std::max_element(hist.begin(), hist.end() - 1); int histMaxIndex = std::distance(hist.begin(), max_it); if (histMaxIndex < (int)((s_t)numBins * percentOfMaxToCut)) @@ -65,6 +81,8 @@ void ForcePlate::autodetectNoiseThresholdAndClip( break; } } + std::cout << "hist max index " << histMaxIndex << " < threshold " + << (int)((s_t)numBins * percentOfMaxToCut) << std::endl; if (rightBound > (int)((s_t)numBins * percentOfMaxToCheckThumbRightEdge)) { @@ -120,6 +138,13 @@ void ForcePlate::autodetectNoiseThresholdAndClip( } } } + else + { + std::cout << "Not clipping force plate because it has no obvious peak in " + "the histogram. Hist max index " + << histMaxIndex << " < threshold " + << (int)((s_t)numBins * percentOfMaxToCut) << std::endl; + } } void ForcePlate::detectAndFixCopMomentConvention(int trial, int i) diff --git a/dart/biomechanics/OpenSimParser.cpp b/dart/biomechanics/OpenSimParser.cpp index e2888451e..cca811a36 100644 --- a/dart/biomechanics/OpenSimParser.cpp +++ b/dart/biomechanics/OpenSimParser.cpp @@ -4022,6 +4022,24 @@ std::vector OpenSimParser::loadGRF( token.find("torque_z"), std::string("torque_z").size(), empty); wrench = 2; } + if (token.find("moment_x") != std::string::npos) + { + prefixSuffix.replace( + token.find("moment_x"), std::string("moment_x").size(), empty); + wrench = 0; + } + if (token.find("moment_y") != std::string::npos) + { + prefixSuffix.replace( + token.find("moment_y"), std::string("moment_y").size(), empty); + wrench = 1; + } + if (token.find("moment_z") != std::string::npos) + { + prefixSuffix.replace( + token.find("moment_z"), std::string("moment_z").size(), empty); + wrench = 2; + } if (token.find("torque_r_x") != std::string::npos || token.find("torque_l_x") != std::string::npos) { @@ -4077,7 +4095,7 @@ std::vector OpenSimParser::loadGRF( prefixSuffix.replace(prefixSuffix.find("__"), 2, std::string("_")); } - if (token == "time") + if (token == "time" || token == "Time") { // Default to plate 0 plate = 0; diff --git a/dart/server/GUIStateMachine.cpp b/dart/server/GUIStateMachine.cpp index cf3106db8..bfee2deb9 100644 --- a/dart/server/GUIStateMachine.cpp +++ b/dart/server/GUIStateMachine.cpp @@ -213,21 +213,27 @@ void GUIStateMachine::renderBasis( pointsZ.push_back(T * (Eigen::Vector3s::UnitZ() * scale)); deleteObjectsByPrefix(prefix + "__basis_"); + std::vector width; + width.push_back(1.0); + width.push_back(0.3); createLine( prefix + "__basis_unitX", pointsX, Eigen::Vector4s(1.0, 0.0, 0.0, 1.0), - layer); + layer, + width); createLine( prefix + "__basis_unitY", pointsY, Eigen::Vector4s(0.0, 1.0, 0.0, 1.0), - layer); + layer, + width); createLine( prefix + "__basis_unitZ", pointsZ, Eigen::Vector4s(0.0, 0.0, 1.0, 1.0), - layer); + layer, + width); } /// This is a high-level command that creates/updates all the shapes in a diff --git a/unittests/unit/test_DynamicsFitter.cpp b/unittests/unit/test_DynamicsFitter.cpp index cfbaad2cb..e2622efef 100644 --- a/unittests/unit/test_DynamicsFitter.cpp +++ b/unittests/unit/test_DynamicsFitter.cpp @@ -5764,6 +5764,61 @@ TEST(DynamicsFitter, TEST_ZERO_RESIDUALS) } #endif +#ifdef JACOBIAN_TESTS +TEST(DynamicsFitter, TEST_ZERO_RESIDUALS_ABLATION) +{ + std::vector motFiles; + std::vector c3dFiles; + std::vector trcFiles; + std::vector grfFiles; + + motFiles.push_back("dart://sample/grf/SprinterWithSpine/IK/JA1Gait35_ik.mot"); + trcFiles.push_back( + "dart://sample/grf/SprinterWithSpine/MarkerData/JA1Gait35.trc"); + grfFiles.push_back( + "dart://sample/grf/SprinterWithSpine/ID/JA1Gait35_grf.mot"); + + std::vector footNames; + footNames.push_back("calcn_r"); + footNames.push_back("calcn_l"); + + OpenSimFile standard = OpenSimParser::parseOsim( + "dart://sample/grf/SprinterWithSpine/Models/" + "optimized_scale_and_markers.osim"); + standard.skeleton->setGravity(Eigen::Vector3s(0, -9.81, 0)); + + std::shared_ptr init = createInitialization( + standard.skeleton, + standard.markersMap, + standard.trackingMarkers, + footNames, + motFiles, + c3dFiles, + trcFiles, + grfFiles, + -1); + + std::vector footNodes; + footNodes.push_back(standard.skeleton->getBodyNode("calcn_r")); + footNodes.push_back(standard.skeleton->getBodyNode("calcn_l")); + + DynamicsFitter fitter( + standard.skeleton, init->grfBodyNodes, init->trackingMarkers); + + Eigen::MatrixXs originalTrajectory = init->poseTrials[0]; + // this gets the mass of the skeleton + fitter.zeroLinearResidualsOnCOMTrajectoryAblation(init, 4); + + // fitter.moveComsToMinimizeAngularResiduals(init); + + fitter.saveDynamicsToGUI( + "../../../javascript/src/data/movement2.bin", + init, + 0, + (int)round(1.0 / init->trialTimesteps[0])); +} +#endif + #ifdef JACOBIAN_TESTS TEST(DynamicsFitter, TEST_MULTIMASS_ZERO_RESIDUALS) { From 8fab9f2553713ef0552a169121225165441fe38e Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Tue, 14 May 2024 11:49:59 -0700 Subject: [PATCH 2/3] Including changes to the web viewer --- javascript/src/NimbleView.ts | 6 ++++++ javascript/src/components/View.ts | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/javascript/src/NimbleView.ts b/javascript/src/NimbleView.ts index de3798fa8..e921406b9 100644 --- a/javascript/src/NimbleView.ts +++ b/javascript/src/NimbleView.ts @@ -1046,6 +1046,7 @@ class DARTView { mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; mesh.scale.set(size[0], size[1], size[2]); + mesh.frustumCulled = false; const material = mesh.material as THREE.MeshLambertMaterial; material.color.r = color[0]; @@ -1081,6 +1082,7 @@ class DARTView { mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; mesh.scale.set(size[0], size[1], size[2]); + mesh.frustumCulled = false; this.objects.set(key, mesh); this.disposeHandlers.set(key, () => { @@ -1129,6 +1131,7 @@ class DARTView { mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; mesh.scale.set(radii[0], radii[1], radii[2]); + mesh.frustumCulled = false; this.objects.set(key, mesh); this.disposeHandlers.set(key, () => { @@ -1180,6 +1183,7 @@ class DARTView { mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; mesh.scale.set(radius, height, radius); + mesh.frustumCulled = false; this.objects.set(key, mesh); this.disposeHandlers.set(key, () => { @@ -1231,6 +1235,7 @@ class DARTView { mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; mesh.scale.set(radius, height, radius); + mesh.frustumCulled = false; this.objects.set(key, mesh); this.disposeHandlers.set(key, () => { @@ -1304,6 +1309,7 @@ class DARTView { mesh.rotation.z = euler[2]; mesh.castShadow = castShadows; mesh.receiveShadow = receiveShadows; + mesh.frustumCulled = false; this.objects.set(key, mesh); this.disposeHandlers.set(key, () => { diff --git a/javascript/src/components/View.ts b/javascript/src/components/View.ts index 0def40533..f1ceb642d 100644 --- a/javascript/src/components/View.ts +++ b/javascript/src/components/View.ts @@ -52,8 +52,8 @@ class View { this.camera = new THREE.PerspectiveCamera( 65, this.width / this.height, - 10, - 100000 + 0.00001, + 10000000 ); this.camera.position.z = 500; From 3c1a8f122b58d4c56a262054b39e0ed66dfd1008 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Wed, 15 May 2024 19:53:46 -0700 Subject: [PATCH 3/3] Updating the DynamicsFitter bindings --- python/_nimblephysics/biomechanics/DynamicsFitter.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/python/_nimblephysics/biomechanics/DynamicsFitter.cpp b/python/_nimblephysics/biomechanics/DynamicsFitter.cpp index 373946763..977ab6a33 100644 --- a/python/_nimblephysics/biomechanics/DynamicsFitter.cpp +++ b/python/_nimblephysics/biomechanics/DynamicsFitter.cpp @@ -641,7 +641,14 @@ class DynamicsFitter ::py::arg("maxTrialsToSolveMassOver") = 4, ::py::arg("detectExternalForce") = true, ::py::arg("driftCorrectionBlurRadius") = 250, - ::py::arg("driftCorrectionBlurInterval") = 250) + ::py::arg("driftCorrectionBlurInterval") = 250, + ::py::arg("regularizeUnobservedTimesteps") = 0.1) + .def( + "zeroLinearResidualsOnCOMTrajectoryAblation", + &dart::biomechanics::DynamicsFitter:: + zeroLinearResidualsOnCOMTrajectoryAblation, + ::py::arg("init"), + ::py::arg("maxTrialsToSolveMassOver") = 4) .def( "multimassZeroLinearResidualsOnCOMTrajectory", &dart::biomechanics::DynamicsFitter::