diff --git a/dart/biomechanics/DynamicsFitter.cpp b/dart/biomechanics/DynamicsFitter.cpp index 860e286d7..9ab26d95e 100644 --- a/dart/biomechanics/DynamicsFitter.cpp +++ b/dart/biomechanics/DynamicsFitter.cpp @@ -1,6 +1,7 @@ #include "dart/biomechanics/DynamicsFitter.hpp" #include +#include #include #include #include @@ -8926,9 +8927,26 @@ bool DynamicsFitProblem::get_nlp_info( // Set the number of entries in the Hessian nnz_h_lag = n * n; + if ((int32_t)nnz_h_lag < 0 || nnz_h_lag >= std::numeric_limits::max() + || n > 46340) + { + // We've got so many dimensions that we can't store the size of the full + // hessian in 32-bits. + std::cout << "WARNING: nnz_h_lag is too large to store in signed 32-bits. " + "Setting " + "to 1." + << std::endl; + nnz_h_lag = 1; + } + // use the C style indexing (0-based) index_style = Ipopt::TNLP::C_STYLE; + std::cout << "DynamicsFitProbelem: Getting NLP info" << std::endl; + std::cout << " n=" << n << " m=" << m << std::endl; + std::cout << " nnz_jac_g=" << nnz_jac_g << " nnz_h_lag=" << nnz_h_lag + << std::endl; + return true; } @@ -9526,6 +9544,36 @@ std::shared_ptr DynamicsFitter::createInitialization( init->regularizeMarkerOffsetsTo[pair.first] = pair.second; } + // Create the joint data + init->joints.clear(); + for (int trial = 0; trial < poseTrials.size(); trial++) + { + init->joints.emplace_back(); + for (int i = 0; i < skel->getNumJoints(); i++) + { + init->joints.back().push_back(skel->getJoint(i)); + } + init->jointsAdjacentMarkers.emplace_back(); + init->jointWeights.push_back(Eigen::VectorXs::Ones(skel->getNumJoints())); + init->axisWeights.push_back(Eigen::VectorXs::Zero(skel->getNumJoints())); + } + + for (int trial = 0; trial < poseTrials.size(); trial++) + { + Eigen::MatrixXs& poses = poseTrials[trial]; + Eigen::MatrixXs jointCenters + = Eigen::MatrixXs::Zero(3 * skel->getNumJoints(), poses.cols()); + Eigen::MatrixXs jointAxis + = Eigen::MatrixXs::Zero(6 * skel->getNumJoints(), poses.cols()); + for (int t = 0; t < poses.cols(); t++) + { + skel->setPositions(poses.col(t)); + jointCenters.col(t) = skel->getJointWorldPositions(skel->getJoints()); + } + init->jointCenters.push_back(jointCenters); + init->jointAxis.push_back(jointAxis); + } + return init; } @@ -9589,6 +9637,10 @@ std::shared_ptr DynamicsFitter::createInitialization( // Copy over the joint data init->joints.clear(); + init->jointWeights.clear(); + init->axisWeights.clear(); + init->jointCenters.clear(); + init->jointAxis.clear(); for (int trial = 0; trial < kinematicInit.size(); trial++) { init->joints.emplace_back(); @@ -9606,6 +9658,9 @@ std::shared_ptr DynamicsFitter::createInitialization( for (int trial = 0; trial < init->poseTrials.size(); trial++) { + assert( + kinematicInit.at(trial).jointCenters.rows() / 3 + == init->joints.at(trial).size()); init->jointCenters.push_back(kinematicInit.at(trial).jointCenters); init->jointAxis.push_back(kinematicInit.at(trial).jointAxis); } @@ -11618,7 +11673,8 @@ bool DynamicsFitter::zeroLinearResidualsOnCOMTrajectory( // unifiedPositions - unifiedGravityOffset); Eigen::LeastSquaresConjugateGradient solver; solver.setTolerance(1e-9); - solver.setMaxIterations(unifiedLinearMap.rows() * 10); + solver.setMaxIterations( + std::min((int)(unifiedLinearMap.rows() * 10), 10000)); solver.compute(unifiedLinearMap); Eigen::VectorXs tentativeResult = solver.solve(unifiedPositions - unifiedGravityOffset); @@ -18350,6 +18406,8 @@ void DynamicsFitter::saveDynamicsToGUI( // Render the joints, if we have them int numJoints = init->jointCenters.at(trialIndex).rows() / 3; + assert(numJoints == init->jointWeights.at(trialIndex).size()); + assert(numJoints == init->joints.at(trialIndex).size()); server.createLayer( functionalJointCenterLayerName, functionalJointCenterLayerColor, true); for (int i = 0; i < numJoints; i++) diff --git a/dart/biomechanics/OpenSimParser.cpp b/dart/biomechanics/OpenSimParser.cpp index e2888451e..e2ca27159 100644 --- a/dart/biomechanics/OpenSimParser.cpp +++ b/dart/biomechanics/OpenSimParser.cpp @@ -4851,6 +4851,7 @@ Eigen::Vector3s readAttachedGeometry( dynamics::BodyNode* childBody, Eigen::Isometry3s relativeT, const std::string fileNameForErrorDisplay, + OpenSimFile& file, const std::string geometryFolder, const common::ResourceRetrieverPtr& geometryRetriever, bool ignoreGeometry) @@ -4885,6 +4886,19 @@ Eigen::Vector3s readAttachedGeometry( numScales++; } + Eigen::Isometry3s localT = Eigen::Isometry3s::Identity(); + tinyxml2::XMLElement* transformElem + = meshCursor->FirstChildElement("transform"); + if (transformElem != nullptr) + { + Eigen::Vector6s transformVec = readVec6(transformElem); + localT.linear() = math::eulerXYZToMatrix(transformVec.head<3>()); + localT.translation() = transformVec.tail<3>(); + } + file.meshMap[mesh_file] + = std::make_pair(childBody->getName(), relativeT * localT); + file.meshScaleMap[mesh_file] = scale; + if (!ignoreGeometry) { common::Uri meshUri = common::Uri::createFromRelativeUri( @@ -4903,16 +4917,6 @@ Eigen::Vector3s readAttachedGeometry( = childBody->createShapeNodeWith( meshShape); - Eigen::Isometry3s localT = Eigen::Isometry3s::Identity(); - tinyxml2::XMLElement* transformElem - = meshCursor->FirstChildElement("transform"); - if (transformElem != nullptr) - { - Eigen::Vector6s transformVec = readVec6(transformElem); - localT.linear() = math::eulerXYZToMatrix(transformVec.head<3>()); - localT.translation() = transformVec.tail<3>(); - } - meshShapeNode->setRelativeTransform(relativeT * localT); dynamics::VisualAspect* meshVisualAspect @@ -4954,6 +4958,7 @@ std::tuple createJoint( Eigen::Isometry3s transformFromParent, Eigen::Isometry3s transformFromChild, const std::string fileNameForErrorDisplay, + OpenSimFile& file, const std::string geometryFolder, const common::ResourceRetrieverPtr& geometryRetriever, bool ignoreGeometry) @@ -5922,6 +5927,9 @@ std::tuple createJoint( numScalesCounted++; } + file.meshMap[mesh_file] = std::make_pair(bodyName, transform); + file.meshScaleMap[mesh_file] = scale; + if (!ignoreGeometry) { common::Uri meshUri = common::Uri::createFromRelativeUri( @@ -5989,6 +5997,7 @@ std::tuple createJoint( childBody, relativeT, fileNameForErrorDisplay, + file, geometryFolder, geometryRetriever, ignoreGeometry); @@ -6007,6 +6016,7 @@ std::tuple createJoint( childBody, Eigen::Isometry3s::Identity(), fileNameForErrorDisplay, + file, geometryFolder, geometryRetriever, ignoreGeometry); @@ -6195,6 +6205,7 @@ OpenSimFile OpenSimParser::readOsim30( transformFromParent, transformFromChild, fileNameForErrorDisplay, + file, geometryFolder, geometryRetriever, ignoreGeometry); @@ -6390,6 +6401,7 @@ void recursiveCreateJoint( joint->fromParent, joint->fromChild, fileNameForErrorDisplay, + file, geometryFolder, geometryRetriever, ignoreGeometry); diff --git a/dart/biomechanics/OpenSimParser.hpp b/dart/biomechanics/OpenSimParser.hpp index fd90ff81f..5cdb75418 100644 --- a/dart/biomechanics/OpenSimParser.hpp +++ b/dart/biomechanics/OpenSimParser.hpp @@ -39,6 +39,11 @@ struct OpenSimFile // IMU map std::map> imuMap; + // Mesh map, so that we can access mesh positioning data even if we + // are not loading the geometry. + std::map> meshMap; + std::map meshScaleMap; + std::vector warnings; std::vector ignoredBodies; std::vector> jointsDrivenBy; diff --git a/dart/biomechanics/SubjectOnDisk.cpp b/dart/biomechanics/SubjectOnDisk.cpp index b0df7ec4f..e76a1b944 100644 --- a/dart/biomechanics/SubjectOnDisk.cpp +++ b/dart/biomechanics/SubjectOnDisk.cpp @@ -53,6 +53,8 @@ proto::ProcessingPassType passTypeToProto(ProcessingPassType type) return proto::ProcessingPassType::dynamics; case lowPassFilter: return proto::ProcessingPassType::lowPassFilter; + case accMinimizingFilter: + return proto::ProcessingPassType::accMinimizingFilter; } return proto::ProcessingPassType::kinematics; } @@ -67,6 +69,8 @@ ProcessingPassType passTypeFromProto(proto::ProcessingPassType type) return dynamics; case proto::ProcessingPassType::lowPassFilter: return lowPassFilter; + case proto::ProcessingPassType::accMinimizingFilter: + return accMinimizingFilter; // These are just here to keep Clang from complaining case proto::ProcessingPassType_INT_MIN_SENTINEL_DO_NOT_USE_: return kinematics; @@ -106,6 +110,20 @@ proto::MissingGRFReason missingGRFReasonToProto(MissingGRFReason reason) return proto::MissingGRFReason::manualReview; case footContactDetectedButNoForce: return proto::MissingGRFReason::footContactDetectedButNoForce; + case tooHighMarkerRMS: + return proto::MissingGRFReason::tooHighMarkerRMS; + case hasInputOutliers: + return proto::MissingGRFReason::hasInputOutliers; + case hasNoForcePlateData: + return proto::MissingGRFReason::hasNoForcePlateData; + case velocitiesStillTooHighAfterFiltering: + return proto::MissingGRFReason::velocitiesStillTooHighAfterFiltering; + case copOutsideConvexFootError: + return proto::MissingGRFReason::copOutsideConvexFootError; + case zeroForceFrame: + return proto::MissingGRFReason::zeroForceFrame; + case extendedToNearestPeakForce: + return proto::MissingGRFReason::extendedToNearestPeakForce; } return proto::MissingGRFReason::notMissingGRF; } @@ -138,6 +156,20 @@ MissingGRFReason missingGRFReasonFromProto(proto::MissingGRFReason reason) return manualReview; case proto::MissingGRFReason::footContactDetectedButNoForce: return footContactDetectedButNoForce; + case proto::MissingGRFReason::tooHighMarkerRMS: + return tooHighMarkerRMS; + case proto::MissingGRFReason::hasInputOutliers: + return hasInputOutliers; + case proto::MissingGRFReason::hasNoForcePlateData: + return hasNoForcePlateData; + case proto::MissingGRFReason::velocitiesStillTooHighAfterFiltering: + return velocitiesStillTooHighAfterFiltering; + case proto::MissingGRFReason::copOutsideConvexFootError: + return copOutsideConvexFootError; + case proto::MissingGRFReason::zeroForceFrame: + return zeroForceFrame; + case proto::MissingGRFReason::extendedToNearestPeakForce: + return extendedToNearestPeakForce; // These are just here to keep Clang from complaining case proto::MissingGRFReason_INT_MIN_SENTINEL_DO_NOT_USE_: return notMissingGRF; @@ -560,6 +592,11 @@ void SubjectOnDisk::loadAllFrames(bool doNotStandardizeForcePlateData) } } +bool SubjectOnDisk::hasLoadedAllFrames() +{ + return mLoadedAllFrames; +} + /// This returns the raw proto header for this subject, which can be used to /// write out a new B3D file std::shared_ptr SubjectOnDisk::getHeaderProto() @@ -1361,6 +1398,10 @@ int SubjectOnDisk::getNumProcessingPasses() ProcessingPassType SubjectOnDisk::getProcessingPassType(int processingPass) { + if (processingPass == -1) + { + processingPass = mHeader->mPasses.size() - 1; + } return mHeader->mPasses[processingPass]->mType; } @@ -1371,6 +1412,10 @@ std::vector SubjectOnDisk::getDofPositionsObserved( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial] ->mTrialPasses[processingPass] ->mDofPositionsObserved; @@ -1383,6 +1428,10 @@ std::vector SubjectOnDisk::getDofVelocitiesFiniteDifferenced( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial] ->mTrialPasses[processingPass] ->mDofVelocitiesFiniteDifferenced; @@ -1395,6 +1444,10 @@ std::vector SubjectOnDisk::getDofAccelerationsFiniteDifferenced( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial] ->mTrialPasses[processingPass] ->mDofAccelerationFiniteDifferenced; @@ -1407,6 +1460,10 @@ std::vector SubjectOnDisk::getTrialLinearResidualNorms( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial]->mTrialPasses[processingPass]->mLinearResidual; } @@ -1417,6 +1474,10 @@ std::vector SubjectOnDisk::getTrialAngularResidualNorms( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial] ->mTrialPasses[processingPass] ->mAngularResidual; @@ -1427,6 +1488,24 @@ std::vector SubjectOnDisk::getTrialMarkerRMSs( { if (trial < 0 || trial >= mHeader->mTrials.size()) { + std::cout << "Requested getTrialMarkerRMSs() for trial " << trial + << ", which is out of bounds. Returning empty vector." + << std::endl; + return std::vector(); + } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } + if (processingPass < 0 + || processingPass >= mHeader->mTrials[trial]->mTrialPasses.size()) + { + std::cout + << "Requested getTrialMarkerRMSs() for trial " << trial + << " and processing pass " << processingPass + << ", which is out of bounds on the existing processing passes (len=" + << mHeader->mTrials[trial]->mTrialPasses.size() + << "). Returning empty vector." << std::endl; return std::vector(); } return mHeader->mTrials[trial]->mTrialPasses[processingPass]->mMarkerRMS; @@ -1439,6 +1518,10 @@ std::vector SubjectOnDisk::getTrialMarkerMaxs( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial]->mTrialPasses[processingPass]->mMarkerMax; } @@ -1451,6 +1534,10 @@ std::vector SubjectOnDisk::getTrialMaxJointVelocity( { return std::vector(); } + if (processingPass == -1) + { + processingPass = mHeader->mTrials[trial]->mTrialPasses.size() - 1; + } return mHeader->mTrials[trial] ->mTrialPasses[processingPass] ->mJointsMaxVelocity; @@ -2268,7 +2355,6 @@ void SubjectOnDiskTrialPass::computeKinematicValues( - (T_wr.linear().transpose() * skel->getGravity()); rootSpatialAccInRootFrame.col(t).tail<3>() = rootLinAcc; } - } } linearResiduals.push_back(linearResidual); @@ -2335,8 +2421,14 @@ void SubjectOnDiskTrialPass::computeValuesFromForcePlates( s_t forcePlateZeroThresholdNewtons) { // 0. Compute kinematic values - computeKinematicValues(skel, timestep, poses, rootHistoryLen, - rootHistoryStride, explicitVels, explicitAccs); + computeKinematicValues( + skel, + timestep, + poses, + rootHistoryLen, + rootHistoryStride, + explicitVels, + explicitAccs); const auto& vels = getVels(); const auto& accs = getAccs(); @@ -2355,10 +2447,12 @@ void SubjectOnDiskTrialPass::computeValuesFromForcePlates( { dynamics::BodyNode* footBody = skel->getBodyNode(footName); - if (!footBody) { - std::cout << "WARNING: One of the foot bodies specified does not exist in " - "the skeleton. Skipping dynamics calculations..." - << std::endl; + if (!footBody) + { + std::cout + << "WARNING: One of the foot bodies specified does not exist in " + "the skeleton. Skipping dynamics calculations..." + << std::endl; return; } @@ -2573,6 +2667,23 @@ void SubjectOnDiskTrialPass::setForcePlateCutoffs(std::vector cutoffs) mForcePlateCutoffs = cutoffs; } +// If we filtered the position data with an acceleration minimizing filter, +// then what was the regularization weight that tracked the original position. +void SubjectOnDiskTrialPass::setAccelerationMinimizingRegularization( + s_t regularization) +{ + mAccelerationMinimizingRegularization = regularization; +} + +// If we filtered the position data with an acceleration minimizing filter, +// then what was the regularization weight that tracked the original force +// data +void SubjectOnDiskTrialPass::setAccelerationMinimizingForceRegularization( + s_t weight) +{ + mAccelerationMinimizingForceRegularization = weight; +} + void SubjectOnDiskTrialPass::setLinearResidual(std::vector linearResidual) { mLinearResidual = linearResidual; @@ -2664,6 +2775,10 @@ void SubjectOnDiskTrialPass::read( mLowpassCutoffFrequency = proto.lowpass_cutoff_frequency(); mLowpassFilterOrder = proto.lowpass_filter_order(); + mAccelerationMinimizingRegularization + = proto.acc_minimizing_regularization_weight(); + mAccelerationMinimizingForceRegularization + = proto.acc_minimizing_force_regularization_weight(); mForcePlateCutoffs.clear(); for (int i = 0; i < proto.force_plate_cutoff_size(); i++) { @@ -2721,6 +2836,10 @@ void SubjectOnDiskTrialPass::write( proto->set_lowpass_cutoff_frequency(mLowpassCutoffFrequency); proto->set_lowpass_filter_order(mLowpassFilterOrder); + proto->set_acc_minimizing_regularization_weight( + mAccelerationMinimizingRegularization); + proto->set_acc_minimizing_force_regularization_weight( + mAccelerationMinimizingForceRegularization); for (int i = 0; i < mForcePlateCutoffs.size(); i++) { proto->add_force_plate_cutoff(mForcePlateCutoffs[i]); @@ -3203,6 +3322,36 @@ SubjectOnDiskHeader::getTrials() return mTrials; } +void SubjectOnDiskHeader::filterTrials(std::vector keepTrials) +{ + if (keepTrials.size() != mTrials.size()) + { + std::cout << "SubjectOnDisk::writeSubject() passed bad info: keepTrials " + "size is " + << keepTrials.size() << " but we have " << mTrials.size() + << " trials" << std::endl; + return; + } + std::vector> newTrials; + for (int i = 0; i < keepTrials.size(); i++) + { + if (keepTrials[i]) + { + newTrials.push_back(mTrials[i]); + } + } + mTrials = newTrials; +} + +void SubjectOnDiskHeader::trimToProcessingPasses(int numPasses) +{ + mPasses.resize(numPasses); + for (int i = 0; i < mTrials.size(); i++) + { + mTrials[i]->mTrialPasses.resize(numPasses); + } +} + void SubjectOnDiskHeader::setTrials( std::vector> trials) { diff --git a/dart/biomechanics/SubjectOnDisk.hpp b/dart/biomechanics/SubjectOnDisk.hpp index 65456944c..33c5d7499 100644 --- a/dart/biomechanics/SubjectOnDisk.hpp +++ b/dart/biomechanics/SubjectOnDisk.hpp @@ -152,6 +152,13 @@ class SubjectOnDiskTrialPass // If we filtered the force plates, then what was the cutoff frequency of that // filtering? void setForcePlateCutoffs(std::vector cutoffs); + // If we filtered the position data with an acceleration minimizing filter, + // then what was the regularization weight that tracked the original position. + void setAccelerationMinimizingRegularization(s_t regularization); + // If we filtered the position data with an acceleration minimizing filter, + // then what was the regularization weight that tracked the original force + // data + void setAccelerationMinimizingForceRegularization(s_t weight); // This is for allowing the user to set all the values of a pass at once, // without having to manually compute them in Python, which turns out to be @@ -270,6 +277,12 @@ class SubjectOnDiskTrialPass // If we're doing a lowpass filter on this pass, then what was the order of // that (Butterworth) filter? int mLowpassFilterOrder; + // If we filtered position with an acceleration minimizing filter, then this + // is the regularization weight that tracked the original position. + s_t mAccelerationMinimizingRegularization; + // If we filtered position with an acceleration minimizing filter, then this + // is the regularization weight that tracked the original force data + s_t mAccelerationMinimizingForceRegularization; // If we filtered the force plates, then what was the cutoff frequency of that // filtering? std::vector mForcePlateCutoffs; @@ -424,6 +437,8 @@ class SubjectOnDiskHeader std::vector> getProcessingPasses(); std::shared_ptr addTrial(); std::vector> getTrials(); + void filterTrials(std::vector keepTrials); + void trimToProcessingPasses(int numPasses); void setTrials(std::vector> trials); void recomputeColumnNames(); void write(dart::proto::SubjectOnDiskHeader* proto); @@ -505,6 +520,8 @@ class SubjectOnDisk /// matrices in the proto header classes. void loadAllFrames(bool doNotStandardizeForcePlateData = false); + bool hasLoadedAllFrames(); + /// This returns the raw proto header for this subject, which can be used to /// write out a new B3D file std::shared_ptr getHeaderProto(); diff --git a/dart/biomechanics/enums.hpp b/dart/biomechanics/enums.hpp index 8f060e1f4..733880856 100644 --- a/dart/biomechanics/enums.hpp +++ b/dart/biomechanics/enums.hpp @@ -7,6 +7,7 @@ namespace biomechanics { enum MissingGRFReason { notMissingGRF, + // These are the legacy filter's reasons measuredGrfZeroWhenAccelerationNonZero, unmeasuredExternalForceDetected, footContactDetectedButNoForce, @@ -17,7 +18,15 @@ enum MissingGRFReason missingBlip, shiftGRF, manualReview, - interpolatedClippedGRF + interpolatedClippedGRF, + // These are the new filter's reasons + tooHighMarkerRMS, + hasInputOutliers, + hasNoForcePlateData, + velocitiesStillTooHighAfterFiltering, + copOutsideConvexFootError, + zeroForceFrame, + extendedToNearestPeakForce }; enum MissingGRFStatus @@ -31,7 +40,8 @@ enum ProcessingPassType { kinematics, dynamics, - lowPassFilter + lowPassFilter, + accMinimizingFilter }; } // namespace biomechanics diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 8bac6e8a6..47b641ef3 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -5028,6 +5028,35 @@ void BodyNode::debugJacobianOfMinvXForward( (void)x; } +//============================================================================== +/// Compute the distance between a point and the convex hull formed by the +/// body node and all of its children, in world space. +s_t BodyNode::distanceFrom2DConvexHullWithChildren( + Eigen::Vector3s point, Eigen::Vector3s up) +{ + (void)point; + (void)up; + std::vector centers; + + // Fill in the list of centers + + std::vector queue; + queue.push_back(this); + while (queue.size() > 0) + { + BodyNode* node = queue.back(); + queue.pop_back(); + Eigen::Vector3s center = node->getWorldTransform().translation(); + centers.push_back(center); + for (int i = 0; i < node->getNumChildJoints(); i++) + { + queue.push_back(node->getChildJoint(i)->getChildBodyNode()); + } + } + + return math::distancePointToConvexHullProjectedTo2D(point, centers, up); +} + //============================================================================== const Eigen::Vector6s& BodyNode::getBiasForce() const { diff --git a/dart/dynamics/BodyNode.hpp b/dart/dynamics/BodyNode.hpp index 44bc4503f..d608647dc 100644 --- a/dart/dynamics/BodyNode.hpp +++ b/dart/dynamics/BodyNode.hpp @@ -1229,6 +1229,15 @@ class BodyNode void debugJacobianOfMinvXForward( neural::WithRespectTo* wrt, Eigen::VectorXs x); + //---------------------------------------------------------------------------- + /// \{ \name Recursive algorithms geometric approximations + //---------------------------------------------------------------------------- + + /// Compute the distance between a point and the convex hull formed by the + /// body node and all of its children, in world space. + s_t distanceFrom2DConvexHullWithChildren( + Eigen::Vector3s point, Eigen::Vector3s up = Eigen::Vector3s::UnitY()); + // protected: public: //-------------------------------------------------------------------------- diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 8ea041f48..2153791d6 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -4788,6 +4788,182 @@ Eigen::Isometry3s iterativeClosestPoint( return T; } +//============================================================================== +// Comparison function for sorting points lexicographically +bool comparePoints2D(const Eigen::Vector2s& a, const Eigen::Vector2s& b) +{ + if (a.x() != b.x()) + return a.x() < b.x(); + else + return a.y() < b.y(); +} + +//============================================================================== +// Cross product of OA and OB vectors +s_t cross2D( + const Eigen::Vector2s& O, + const Eigen::Vector2s& A, + const Eigen::Vector2s& B) +{ + return (A - O).x() * (B - O).y() - (A - O).y() * (B - O).x(); +} + +//============================================================================== +// Convex hull algorithm: Andrew's monotone chain algorithm +std::vector convexHull2D(std::vector& P) +{ + int n = P.size(), k = 0; + if (n == 1) + return P; + + std::vector H(2 * n); + + // Sort points lexicographically + std::sort(P.begin(), P.end(), comparePoints2D); + + // Build the lower hull + for (int i = 0; i < n; ++i) + { + while (k >= 2 && cross2D(H[k - 2], H[k - 1], P[i]) <= 0) + --k; + H[k++] = P[i]; + } + + // Build the upper hull + for (int i = n - 2, t = k + 1; i >= 0; --i) + { + while (k >= t && cross2D(H[k - 2], H[k - 1], P[i]) <= 0) + --k; + H[k++] = P[i]; + } + + H.resize(k - 1); // Remove the last point (same as the first) + return H; +} + +//============================================================================== +// Check if a point is inside a convex polygon (assumes the polygon is already +// sorted by convex hull) +bool isPointInsideConvexPolygon2D( + const Eigen::Vector2s& P, const std::vector& polygon) +{ + int n = polygon.size(); + if (n == 0) + return false; + + double sign = 0; + + for (int i = 0; i < n; ++i) + { + const Eigen::Vector2s& A = polygon[i]; + const Eigen::Vector2s& B = polygon[(i + 1) % n]; + double cross_product = cross2D(A, B, P); + if (cross_product == 0) + continue; // Point is on the line + + if (sign == 0) + sign = cross_product > 0 ? 1 : -1; + else if ((cross_product > 0 && sign < 0) || (cross_product < 0 && sign > 0)) + return false; + } + + return true; +} + +//============================================================================== +// Compute the distance from a point to a line segment +s_t distancePointToSegment2D( + const Eigen::Vector2s& P, + const Eigen::Vector2s& A, + const Eigen::Vector2s& B) +{ + Eigen::Vector2s AP = P - A; + Eigen::Vector2s AB = B - A; + double ab2 = AB.dot(AB); + double t = AP.dot(AB) / ab2; + if (t < 0.0) + t = 0.0; + else if (t > 1.0) + t = 1.0; + Eigen::Vector2s projection = A + t * AB; + return (P - projection).norm(); +} + +//============================================================================== +// Compute the distance from a point to the convex hull +s_t distancePointToConvexHull2D( + const Eigen::Vector2s& P, std::vector& points) +{ + if (points.size() == 1) + { + return (P - points[0]).norm(); + } + else if (points.size() == 2) + { + return distancePointToSegment2D(P, points[0], points[1]); + } + + // Compute the convex hull + std::vector hull = convexHull2D(points); + + // Check if the point is inside the convex hull + if (isPointInsideConvexPolygon2D(P, hull)) + return 0.0; + + // Compute the minimal distance from P to the edges of the convex hull + double min_distance = std::numeric_limits::max(); + int n = hull.size(); + + for (int i = 0; i < n; ++i) + { + const Eigen::Vector2s& A = hull[i]; + const Eigen::Vector2s& B = hull[(i + 1) % n]; + double distance = distancePointToSegment2D(P, A, B); + if (distance < min_distance) + min_distance = distance; + } + + return min_distance; +} + +//============================================================================== +/// Compute the distance from a point to the convex hull projected to a plane +s_t distancePointToConvexHullProjectedTo2D( + const Eigen::Vector3s& P, + std::vector& points, + Eigen::Vector3s up) +{ + // Get a 2D basis that is orthogonal to the up vector + if (up.norm() == 0) + { + up = Eigen::Vector3s::UnitY(); + } + else + { + up.normalize(); + } + Eigen::Vector3s x = up.cross(Eigen::Vector3s::UnitZ()); + if (x.norm() == 0) + { + x = up.cross(Eigen::Vector3s::UnitX()); + } + Eigen::Vector3s y = up.cross(x); + + // Project all the centers into 2D space + std::vector centers2D; + for (Eigen::Vector3s center : points) + { + Eigen::Vector2s center2D; + center2D << center.dot(x), center.dot(y); + centers2D.push_back(center2D); + } + Eigen::Vector2s point2D; + point2D << P.dot(x), P.dot(y); + + // Now we can use the geometry library to compute the convex hull + return math::distancePointToConvexHull2D(point2D, centers2D); +} + BoundingBox::BoundingBox() : mMin(0, 0, 0), mMax(0, 0, 0) { } diff --git a/dart/math/Geometry.hpp b/dart/math/Geometry.hpp index 19c180646..8643f6ae7 100644 --- a/dart/math/Geometry.hpp +++ b/dart/math/Geometry.hpp @@ -926,6 +926,30 @@ Eigen::Isometry3s iterativeClosestPoint( Eigen::Isometry3s transform = Eigen::Isometry3s::Identity(), bool verbose = false); +// Convex hull algorithm: Andrew's monotone chain algorithm +std::vector convexHull2D(std::vector& P); + +// Check if a point is inside a convex polygon (assumes the polygon is already +// sorted by convex hull) +bool isPointInsideConvexPolygon2D( + const Eigen::Vector2s& P, const std::vector& polygon); + +// Compute the distance from a point to a line segment +s_t distancePointToSegment2D( + const Eigen::Vector2s& P, + const Eigen::Vector2s& A, + const Eigen::Vector2s& B); + +// Compute the distance from a point to the convex hull +s_t distancePointToConvexHull2D( + const Eigen::Vector2s& P, std::vector& points); + +/// Compute the distance from a point to the convex hull projected to a plane +s_t distancePointToConvexHullProjectedTo2D( + const Eigen::Vector3s& P, + std::vector& points, + Eigen::Vector3s normal = Eigen::Vector3s::UnitY()); + // Represents a bounding box with minimum and maximum coordinates. class BoundingBox { diff --git a/dart/proto/SubjectOnDisk.proto b/dart/proto/SubjectOnDisk.proto index ac96a06af..2d1a1e38a 100644 --- a/dart/proto/SubjectOnDisk.proto +++ b/dart/proto/SubjectOnDisk.proto @@ -14,12 +14,20 @@ enum MissingGRFReason { notMissingGRF = 0; interpolatedClippedGRF = 9; manualReview = 10; footContactDetectedButNoForce = 11; + tooHighMarkerRMS = 12; + hasInputOutliers = 13; + hasNoForcePlateData = 14; + velocitiesStillTooHighAfterFiltering = 15; + copOutsideConvexFootError = 16; + zeroForceFrame = 17; + extendedToNearestPeakForce = 18; }; enum ProcessingPassType { kinematics = 0; dynamics = 1; lowPassFilter = 2; + accMinimizingFilter = 3; }; // Many of the ML tasks we want to support from SubjectOnDisk data include @@ -59,6 +67,10 @@ message SubjectOnDiskTrialProcessingPassHeader { int32 lowpass_filter_order = 11; // If we clipped background noise on the force plates this pass, then this is the cutoff we used for each plate repeated float force_plate_cutoff = 12; + // If we ran an acceleration minimizing smoother on this pass, then this is the regularization weight we used to track the original positions + float acc_minimizing_regularization_weight = 13; + // If we ran an acceleration minimizing smoother on this pass, then this is the regularization weight we used to track the original forces + float acc_minimizing_force_regularization_weight = 14; } message SubjectOnDiskTrialHeader { diff --git a/dart/utils/AccelerationTrackAndMinimize.cpp b/dart/utils/AccelerationTrackAndMinimize.cpp new file mode 100644 index 000000000..f2fc408af --- /dev/null +++ b/dart/utils/AccelerationTrackAndMinimize.cpp @@ -0,0 +1,190 @@ +#include "AccelerationTrackAndMinimize.hpp" + +#include + +// #include +// #include +#include +// #include + +#include "dart/math/MathTypes.hpp" + +namespace dart { +namespace utils { + +AccelerationTrackAndMinimize::AccelerationTrackAndMinimize( + int numTimesteps, + std::vector trackAccelerationAtTimesteps, + s_t zeroUnobservedAccWeight, + s_t trackObservedAccWeight, + s_t regularizationWeight, + s_t dt, + int numIterations) + : mTimesteps(numTimesteps), + mDt(dt), + mTrackAccelerationAtTimesteps(trackAccelerationAtTimesteps), + mTrackObservedAccWeight(trackObservedAccWeight), + mZeroUnobservedAccWeight(zeroUnobservedAccWeight), + mRegularizationWeight(regularizationWeight), + mNumIterations(numIterations), + mNumIterationsBackoff(6), + mDebugIterationBackoff(false), + mConvergenceTolerance(1e-10) +{ + if (mTimesteps < 3) + { + std::cout << "AccelerationTrackAndMinimize requires at least 3 timesteps" + << std::endl; + return; + } + if (mTrackAccelerationAtTimesteps.size() != mTimesteps) + { + std::cout << "trackAccelerationAtTimesteps.size() != mTimesteps" + << std::endl; + return; + } + + Eigen::Vector3s stamp; + stamp << 1, -2, 1; + stamp *= 1.0 / (mDt * mDt); + + typedef Eigen::Triplet T; + std::vector tripletList; + const int accTimesteps = mTimesteps - 2; + for (int i = 0; i < accTimesteps; i++) + { + for (int j = 0; j < 3; j++) + { + if (trackAccelerationAtTimesteps[i + 1]) + { + tripletList.push_back(T(i, i + j, stamp(j) * mTrackObservedAccWeight)); + } + else + { + tripletList.push_back(T(i, i + j, stamp(j) * mZeroUnobservedAccWeight)); + } + } + } + for (int i = 0; i < mTimesteps; i++) + { + tripletList.push_back(T(accTimesteps + i, i, mRegularizationWeight)); + } + // Add one extra input (to solve for) which applies a linear offset to the + // tracked accelerations + for (int i = 0; i < accTimesteps; i++) + { + if (trackAccelerationAtTimesteps[i + 1]) + { + tripletList.push_back(T(i, mTimesteps, 1.0)); + } + } + mB_sparse + = Eigen::SparseMatrix(accTimesteps + mTimesteps, mTimesteps + 1); + mB_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); + mB_sparse.makeCompressed(); + mB_sparseSolver.analyzePattern(mB_sparse); + mB_sparseSolver.factorize(mB_sparse); + if (mB_sparseSolver.info() != Eigen::Success) + { + std::cout << "mB_sparseSolver.factorize(mB_sparse) error: " + << mB_sparseSolver.lastErrorMessage() << std::endl; + } + assert(mB_sparseSolver.info() == Eigen::Success); +} + +AccelerationTrackingResult AccelerationTrackAndMinimize::minimize( + Eigen::VectorXs series, Eigen::VectorXs trackAcc) +{ + AccelerationTrackingResult result; + result.series = series; + result.accelerationOffset = 0.0; + if (series.size() != mTimesteps) + { + std::cout << "series.size() != mTimesteps" << std::endl; + return result; + } + if (trackAcc.size() != mTimesteps) + { + std::cout << "trackAcc.size() != mTimesteps" << std::endl; + return result; + } + + const int accTimesteps = mTimesteps - 2; + Eigen::VectorXs b = Eigen::VectorXs(accTimesteps + mTimesteps); + b.segment(0, accTimesteps) = trackAcc.segment(1, accTimesteps); + for (int i = 0; i < accTimesteps; i++) + { + if (!mTrackAccelerationAtTimesteps[i + 1]) + { + if (b(i) != 0) + { + std::cout << "Warning: trackAcc[" << i << "] is non-zero, but we're " + << "not tracking acceleration at this timestep. Setting it " + << "to zero. Check how you are formatting your input array!" + << std::endl; + } + b(i) = 0; + } + else + { + b(i) *= mTrackObservedAccWeight; + } + } + b.segment(accTimesteps, mTimesteps) = series * mRegularizationWeight; + + Eigen::VectorXs paddedSeries = Eigen::VectorXs::Zero(mTimesteps + 1); + paddedSeries.head(mTimesteps) = series; + + int iterations = mNumIterations; + for (int i = 0; i < mNumIterationsBackoff; i++) + { + Eigen::LeastSquaresConjugateGradient> solver; + solver.compute(mB_sparse); + solver.setTolerance(mConvergenceTolerance); + solver.setMaxIterations(iterations); + paddedSeries = solver.solveWithGuess(b, paddedSeries); + // Check convergence + if (solver.info() == Eigen::Success) + { + // Converged + break; + } + else + { + if (mDebugIterationBackoff) + { + std::cout << "[AccelerationTrackAndMinimize] " + "LeastSquaresConjugateGradient did " + "not converge in " + << iterations << ", with error " << solver.error() + << " so doubling iteration count and trying again." + << std::endl; + } + iterations *= 2; + } + } + + result.series = paddedSeries.head(mTimesteps); + result.accelerationOffset + = paddedSeries(mTimesteps) / mTrackObservedAccWeight; + + return result; +} + +void AccelerationTrackAndMinimize::setDebugIterationBackoff(bool debug) +{ + mDebugIterationBackoff = debug; +} + +void AccelerationTrackAndMinimize::setNumIterationsBackoff(int numIterations) +{ + mNumIterationsBackoff = numIterations; +} + +void AccelerationTrackAndMinimize::setConvergenceTolerance(s_t tolerance) +{ + mConvergenceTolerance = tolerance; +} + +} // namespace utils +} // namespace dart \ No newline at end of file diff --git a/dart/utils/AccelerationTrackAndMinimize.hpp b/dart/utils/AccelerationTrackAndMinimize.hpp new file mode 100644 index 000000000..2e29c669a --- /dev/null +++ b/dart/utils/AccelerationTrackAndMinimize.hpp @@ -0,0 +1,59 @@ +#ifndef UTILS_ACC_TRACK_AND_MINIMIZE +#define UTILS_ACC_TRACK_AND_MINIMIZE + +#include +#include + +#include "dart/math/MathTypes.hpp" + +namespace dart { +namespace utils { + +typedef struct AccelerationTrackingResult +{ + Eigen::VectorXs series; + s_t accelerationOffset; +} AccelerationTrackingResult; + +class AccelerationTrackAndMinimize +{ +public: + AccelerationTrackAndMinimize( + int numTimesteps, + std::vector trackAccelerationAtTimesteps, + s_t zeroUnobservedAccWeight = 1.0, + s_t trackObservedAccWeight = 1.0, + s_t regularizationWeight = 0.01, + s_t dt = 1.0, + int numIterations = 10000); + + AccelerationTrackingResult minimize( + Eigen::VectorXs series, Eigen::VectorXs trackAcc); + + void setDebugIterationBackoff(bool debug); + + void setNumIterationsBackoff(int numIterations); + + void setConvergenceTolerance(s_t tolerance); + +protected: + int mTimesteps; + s_t mDt; + s_t mZeroUnobservedAccWeight; + s_t mTrackObservedAccWeight; + s_t mRegularizationWeight; + std::vector mTrackAccelerationAtTimesteps; + + int mNumIterations; + int mNumIterationsBackoff; + bool mDebugIterationBackoff; + s_t mConvergenceTolerance; + Eigen::SparseMatrix mB_sparse; + Eigen::SparseQR, Eigen::NaturalOrdering> + mB_sparseSolver; +}; + +} // namespace utils +} // namespace dart + +#endif \ No newline at end of file diff --git a/python/_nimblephysics/biomechanics/DynamicsFitter.cpp b/python/_nimblephysics/biomechanics/DynamicsFitter.cpp index 3770724c7..160bdc535 100644 --- a/python/_nimblephysics/biomechanics/DynamicsFitter.cpp +++ b/python/_nimblephysics/biomechanics/DynamicsFitter.cpp @@ -21,6 +21,7 @@ void DynamicsFitter(py::module& m) { py::enum_(m, "MissingGRFReason") + // These values are used by the legacy filter .value( "notMissingGRF", dart::biomechanics::MissingGRFReason::notMissingGRF) .value( @@ -50,6 +51,29 @@ void DynamicsFitter(py::module& m) .value( "footContactDetectedButNoForce", dart::biomechanics::MissingGRFReason::footContactDetectedButNoForce) + // These values are used by the new filter + .value( + "tooHighMarkerRMS", + dart::biomechanics::MissingGRFReason::tooHighMarkerRMS) + .value( + "hasInputOutliers", + dart::biomechanics::MissingGRFReason::hasInputOutliers) + .value( + "hasNoForcePlateData", + dart::biomechanics::MissingGRFReason::hasNoForcePlateData) + .value( + "velocitiesStillTooHighAfterFiltering", + dart::biomechanics::MissingGRFReason:: + velocitiesStillTooHighAfterFiltering) + .value( + "copOutsideConvexFootError", + dart::biomechanics::MissingGRFReason::copOutsideConvexFootError) + .value( + "zeroForceFrame", + dart::biomechanics::MissingGRFReason::zeroForceFrame) + .value( + "extendedToNearestPeakForce", + dart::biomechanics::MissingGRFReason::extendedToNearestPeakForce) .export_values(); py::enum_(m, "MissingGRFStatus") @@ -106,7 +130,40 @@ void DynamicsFitter(py::module& m) ::py::arg("forcesConcat"), ::py::arg("wrt"), ::py::arg("torquesMultiple"), - ::py::arg("useL1") = false); + ::py::arg("useL1") = false) + .def( + "calculateComToCenterAngularResiduals", + &dart::biomechanics::ResidualForceHelper:: + calculateComToCenterAngularResiduals, + ::py::arg("q"), + ::py::arg("dq"), + ::py::arg("ddq"), + ::py::arg("forcesConcat"), + "This computes the location that we would need to move the COM to in " + "order to center the angular residuals. Moving the COM to the " + "computed location doesn't remove angular residuals, but ensures " + "that any remaining residuals are parallel to the net external force " + "on the body.") + .def( + "calculateCOMAngularResidual", + &dart::biomechanics::ResidualForceHelper::calculateCOMAngularResidual, + ::py::arg("q"), + ::py::arg("dq"), + ::py::arg("ddq"), + ::py::arg("forcesConcat"), + "This computes the residual at the root, then transforms that to the " + "COM and expresses the torque as a spatial vector (even if the root " + "joint uses euler coordinates for rotation).") + .def( + "calculateResidualFreeRootAcceleration", + &dart::biomechanics::ResidualForceHelper:: + calculateResidualFreeRootAcceleration, + ::py::arg("q"), + ::py::arg("dq"), + ::py::arg("ddq"), + ::py::arg("forcesConcat"), + "This computes the acceleration we would need at the root in order " + "to remove all residual forces."); ::py::class_< dart::biomechanics::DynamicsInitialization, diff --git a/python/_nimblephysics/biomechanics/OpenSimParser.cpp b/python/_nimblephysics/biomechanics/OpenSimParser.cpp index d6e9759e2..c18f88687 100644 --- a/python/_nimblephysics/biomechanics/OpenSimParser.cpp +++ b/python/_nimblephysics/biomechanics/OpenSimParser.cpp @@ -72,8 +72,10 @@ void OpenSimParser(py::module& m) "ignoredBodies", &dart::biomechanics::OpenSimFile::ignoredBodies) .def_readwrite( "jointsDrivenBy", &dart::biomechanics::OpenSimFile::jointsDrivenBy) + .def_readwrite("markersMap", &dart::biomechanics::OpenSimFile::markersMap) + .def_readwrite("meshMap", &dart::biomechanics::OpenSimFile::meshMap) .def_readwrite( - "markersMap", &dart::biomechanics::OpenSimFile::markersMap); + "meshScaleMap", &dart::biomechanics::OpenSimFile::meshScaleMap); ::py::class_(m, "OpenSimMot") .def_readwrite("poses", &dart::biomechanics::OpenSimMot::poses) diff --git a/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp b/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp index 550339eb2..691f158ff 100644 --- a/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp +++ b/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp @@ -43,7 +43,12 @@ void SubjectOnDisk(py::module& m) "LOW_PASS_FILTER", dart::biomechanics::ProcessingPassType::lowPassFilter, "This is the pass where we apply a low-pass filter to the " - "kinematics and dynamics."); + "kinematics and dynamics.") + .value( + "ACC_MINIMIZING_FILTER", + dart::biomechanics::ProcessingPassType::accMinimizingFilter, + "This is the pass where we apply an acceleration minimizing " + "filter to the kinematics and dynamics."); auto framePass = ::py::class_< @@ -572,6 +577,16 @@ Note that these are specified in the local body frame, acting on the body at its &dart::biomechanics::SubjectOnDiskTrialPass:: setForcePlateCutoffs, ::py::arg("cutoffs")) + .def( + "setAccelerationMinimizingRegularization", + &dart::biomechanics::SubjectOnDiskTrialPass:: + setAccelerationMinimizingRegularization, + ::py::arg("reg")) + .def( + "setAccelerationMinimizingForceRegularization", + &dart::biomechanics::SubjectOnDiskTrialPass:: + setAccelerationMinimizingForceRegularization, + ::py::arg("reg")) .def( "computeValues", &dart::biomechanics::SubjectOnDiskTrialPass::computeValues, @@ -965,6 +980,15 @@ Note that these are specified in the local body frame, acting on the body at its .def( "getTrials", &dart::biomechanics::SubjectOnDiskHeader::getTrials) + .def( + "filterTrials", + &dart::biomechanics::SubjectOnDiskHeader::filterTrials, + ::py::arg("keepTrials")) + .def( + "trimToProcessingPasses", + &dart::biomechanics::SubjectOnDiskHeader:: + trimToProcessingPasses, + ::py::arg("numPasses")) .def( "setTrials", &dart::biomechanics::SubjectOnDiskHeader::setTrials, @@ -999,6 +1023,11 @@ Note that these are specified in the local body frame, acting on the body at its ::py::arg("doNotStandardizeForcePlateData") = false, "This loads all the frames of data, and fills in the " "processing pass data matrices in the proto header classes.") + .def( + "hasLoadedAllFrames", + &dart::biomechanics::SubjectOnDisk::hasLoadedAllFrames, + "This returns true if all the frames have been loaded into " + "memory.") .def( "getHeaderProto", &dart::biomechanics::SubjectOnDisk::getHeaderProto, diff --git a/python/_nimblephysics/dynamics/BodyNode.cpp b/python/_nimblephysics/dynamics/BodyNode.cpp index b56159346..a60463377 100644 --- a/python/_nimblephysics/dynamics/BodyNode.cpp +++ b/python/_nimblephysics/dynamics/BodyNode.cpp @@ -1296,7 +1296,12 @@ void BodyNode( +[](dart::dynamics::BodyNode* self) { self->dirtyExternalForces(); }) .def( "dirtyCoriolisForces", - +[](dart::dynamics::BodyNode* self) { self->dirtyCoriolisForces(); }); + +[](dart::dynamics::BodyNode* self) { self->dirtyCoriolisForces(); }) + .def( + "distanceFrom2DConvexHullWithChildren", + &dynamics::BodyNode::distanceFrom2DConvexHullWithChildren, + ::py::arg("point"), + ::py::arg("up") = Eigen::Vector3s::UnitY()); } } // namespace python diff --git a/python/_nimblephysics/math/Geometry.cpp b/python/_nimblephysics/math/Geometry.cpp index f48f730b5..80d3e6e18 100644 --- a/python/_nimblephysics/math/Geometry.cpp +++ b/python/_nimblephysics/math/Geometry.cpp @@ -33,7 +33,9 @@ #include #include +#include #include +#include #include "dart/math/MathTypes.hpp" @@ -266,6 +268,26 @@ void Geometry(py::module& m) ::py::arg("T"), ::py::arg("p")); + m.def( + "distancePointToConvexHull2D", + +[](Eigen::Vector2s P, std::vector& points) -> s_t { + return dart::math::distancePointToConvexHull2D(P, points); + }, + ::py::arg("P"), + ::py::arg("points")); + + m.def( + "distancePointToConvexHullProjectedTo2D", + +[](Eigen::Vector3s P, + std::vector& points, + Eigen::Vector3s normal) -> s_t { + return dart::math::distancePointToConvexHullProjectedTo2D( + P, points, normal); + }, + ::py::arg("P"), + ::py::arg("points"), + ::py::arg("normal") = Eigen::Vector3s::UnitY()); + ::py::class_(m, "BoundingBox") .def(::py::init()) .def( diff --git a/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp b/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp new file mode 100644 index 000000000..4c7fb9721 --- /dev/null +++ b/python/_nimblephysics/utils/AccelerationTrackAndMinimize.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace dart { +namespace python { + +void AccelerationTrackAndMinimize(py::module& m) +{ + py::class_( + m, "AccelerationTrackingResult") + .def_readwrite("series", &dart::utils::AccelerationTrackingResult::series) + .def_readwrite( + "accelerationOffset", + &dart::utils::AccelerationTrackingResult::accelerationOffset); + + ::py::class_( + m, "AccelerationTrackAndMinimize") + .def( + ::py::init, s_t, s_t, s_t, s_t, int>(), + ::py::arg("numTimesteps"), + ::py::arg("trackAccelerationAtTimesteps"), + ::py::arg("zeroUnobservedAccWeight") = 1.0, + ::py::arg("trackObservedAccWeight") = 1.0, + ::py::arg("regularizationWeight") = 0.01, + ::py::arg("dt") = 1.0, + ::py::arg("numIterations") = 10000) + .def( + "minimize", + &dart::utils::AccelerationTrackAndMinimize::minimize, + ::py::arg("series"), + ::py::arg("trackAcc")) + .def( + "setDebugIterationBackoff", + &dart::utils::AccelerationTrackAndMinimize::setDebugIterationBackoff, + ::py::arg("iterations")) + .def( + "setNumIterationsBackoff", + &dart::utils::AccelerationTrackAndMinimize::setNumIterationsBackoff, + ::py::arg("series")) + .def( + "setConvergenceTolerance", + &dart::utils::AccelerationTrackAndMinimize::setConvergenceTolerance, + ::py::arg("tolerance")); +} + +} // namespace python +} // namespace dart \ No newline at end of file diff --git a/python/_nimblephysics/utils/module.cpp b/python/_nimblephysics/utils/module.cpp index b734b4dac..240d8a887 100644 --- a/python/_nimblephysics/utils/module.cpp +++ b/python/_nimblephysics/utils/module.cpp @@ -45,6 +45,7 @@ void MJCFExporter(py::module& sm); void UniversalLoader(py::module& sm); void AccelerationSmoother(py::module& sm); void AccelerationMinimizer(py::module& sm); +void AccelerationTrackAndMinimize(py::module& sm); void dart_utils(py::module& m) { @@ -58,6 +59,7 @@ void dart_utils(py::module& m) UniversalLoader(sm); AccelerationSmoother(sm); AccelerationMinimizer(sm); + AccelerationTrackAndMinimize(sm); } } // namespace python diff --git a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi index 015de8dc4..e7bdacb9b 100644 --- a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi +++ b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi @@ -2220,6 +2220,22 @@ class OpenSimFile(): def markersMap(self, arg0: typing.Dict[str, typing.Tuple[nimblephysics_libs._nimblephysics.dynamics.BodyNode, numpy.ndarray[numpy.float64, _Shape[3, 1]]]]) -> None: pass @property + def meshMap(self) -> typing.Dict[str, typing.Tuple[str, nimblephysics_libs._nimblephysics.math.Isometry3]]: + """ + :type: typing.Dict[str, typing.Tuple[str, nimblephysics_libs._nimblephysics.math.Isometry3]] + """ + @meshMap.setter + def meshMap(self, arg0: typing.Dict[str, typing.Tuple[str, nimblephysics_libs._nimblephysics.math.Isometry3]]) -> None: + pass + @property + def meshScaleMap(self) -> typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]: + """ + :type: typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]] + """ + @meshScaleMap.setter + def meshScaleMap(self, arg0: typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]) -> None: + pass + @property def skeleton(self) -> nimblephysics_libs._nimblephysics.dynamics.Skeleton: """ :type: nimblephysics_libs._nimblephysics.dynamics.Skeleton @@ -2381,6 +2397,8 @@ class ProcessingPassType(): DYNAMICS : This is the pass where we solve for dynamics. LOW_PASS_FILTER : This is the pass where we apply a low-pass filter to the kinematics and dynamics. + + ACC_MINIMIZING_FILTER : This is the pass where we apply an acceleration minimizing filter to the kinematics and dynamics. """ def __eq__(self, other: object) -> bool: ... def __getstate__(self) -> int: ... @@ -2401,14 +2419,28 @@ class ProcessingPassType(): """ :type: int """ + ACC_MINIMIZING_FILTER: nimblephysics_libs._nimblephysics.biomechanics.ProcessingPassType # value = DYNAMICS: nimblephysics_libs._nimblephysics.biomechanics.ProcessingPassType # value = KINEMATICS: nimblephysics_libs._nimblephysics.biomechanics.ProcessingPassType # value = LOW_PASS_FILTER: nimblephysics_libs._nimblephysics.biomechanics.ProcessingPassType # value = - __members__: dict # value = {'KINEMATICS': , 'DYNAMICS': , 'LOW_PASS_FILTER': } + __members__: dict # value = {'KINEMATICS': , 'DYNAMICS': , 'LOW_PASS_FILTER': , 'ACC_MINIMIZING_FILTER': } pass class ResidualForceHelper(): def __init__(self, skeleton: nimblephysics_libs._nimblephysics.dynamics.Skeleton, forceBodies: typing.List[int]) -> None: ... + def calculateCOMAngularResidual(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: + """ + This computes the residual at the root, then transforms that to the COM and expresses the torque as a spatial vector (even if the root joint uses euler coordinates for rotation). + """ + def calculateComToCenterAngularResiduals(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: + """ + This computes the location that we would need to move the COM to in order to center the angular residuals. Moving the COM to the computed location doesn't remove angular residuals, but ensures that any remaining residuals are parallel to the net external force on the body. + """ + def calculateInverseDynamics(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ... def calculateResidual(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[6, 1]]: ... + def calculateResidualFreeRootAcceleration(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[6, 1]]: + """ + This computes the acceleration we would need at the root in order to remove all residual forces. + """ def calculateResidualJacobianWrt(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], wrt: nimblephysics_libs._nimblephysics.neural.WithRespectTo) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ... def calculateResidualNorm(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], torquesMultiple: float, useL1: bool = False) -> float: ... def calculateResidualNormGradientWrt(self, q: numpy.ndarray[numpy.float64, _Shape[m, 1]], dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], forcesConcat: numpy.ndarray[numpy.float64, _Shape[m, 1]], wrt: nimblephysics_libs._nimblephysics.neural.WithRespectTo, torquesMultiple: float, useL1: bool = False) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ... @@ -2691,6 +2723,10 @@ class SubjectOnDisk(): """ This returns the timestep size for the trial requested, in seconds per frame """ + def hasLoadedAllFrames(self) -> bool: + """ + This returns true if all the frames have been loaded into memory. + """ def loadAllFrames(self, doNotStandardizeForcePlateData: bool = False) -> None: """ This loads all the frames of data, and fills in the processing pass data matrices in the proto header classes. @@ -2733,6 +2769,7 @@ class SubjectOnDiskHeader(): def setNumJoints(self, joints: int) -> SubjectOnDiskHeader: ... def setSubjectTags(self, subjectTags: typing.List[str]) -> SubjectOnDiskHeader: ... def setTrials(self, trials: typing.List[SubjectOnDiskTrial]) -> None: ... + def trimToProcessingPasses(self, numPasses: int) -> None: ... pass class SubjectOnDiskPassHeader(): def __init__(self) -> None: ... @@ -2799,6 +2836,8 @@ class SubjectOnDiskTrialPass(): def getRootSpatialVelInRootFrame(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ... def getTaus(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ... def getVels(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ... + def setAccelerationMinimizingForceRegularization(self, reg: float) -> None: ... + def setAccelerationMinimizingRegularization(self, reg: float) -> None: ... def setAccs(self, accs: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> None: ... def setAngularResidual(self, angularResidual: typing.List[float]) -> None: ... def setComAccs(self, accs: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> None: ... diff --git a/stubs/_nimblephysics-stubs/dynamics/__init__.pyi b/stubs/_nimblephysics-stubs/dynamics/__init__.pyi index 917a07a3c..ac9bfe83f 100644 --- a/stubs/_nimblephysics-stubs/dynamics/__init__.pyi +++ b/stubs/_nimblephysics-stubs/dynamics/__init__.pyi @@ -5453,6 +5453,7 @@ class BodyNode(TemplatedJacobianBodyNode, JacobianNode, Frame, Entity): def dirtyExternalForces(self) -> None: ... def dirtyTransform(self) -> None: ... def dirtyVelocity(self) -> None: ... + def distanceFrom2DConvexHullWithChildren(self, point: numpy.ndarray[numpy.float64, _Shape[3, 1]], up: numpy.ndarray[numpy.float64, _Shape[3, 1]] = array([0., 1., 0.])) -> float: ... def duplicateNodes(self, otherBodyNode: BodyNode) -> None: ... @typing.overload def getAngularMomentum(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: ... diff --git a/stubs/_nimblephysics-stubs/math/__init__.pyi b/stubs/_nimblephysics-stubs/math/__init__.pyi index e9fba81c2..b53edaeb3 100644 --- a/stubs/_nimblephysics-stubs/math/__init__.pyi +++ b/stubs/_nimblephysics-stubs/math/__init__.pyi @@ -19,6 +19,8 @@ __all__ = [ "Random", "dAdInvT", "dAdT", + "distancePointToConvexHull2D", + "distancePointToConvexHullProjectedTo2D", "eulerXYXToMatrix", "eulerXYZToMatrix", "eulerXZXToMatrix", @@ -241,6 +243,10 @@ def dAdInvT(R: numpy.ndarray[numpy.float64, _Shape[3, 3]], p: numpy.ndarray[nump pass def dAdT(R: numpy.ndarray[numpy.float64, _Shape[3, 3]], p: numpy.ndarray[numpy.float64, _Shape[3, 1]], S: numpy.ndarray[numpy.float64, _Shape[6, 1]]) -> numpy.ndarray[numpy.float64, _Shape[6, 1]]: pass +def distancePointToConvexHull2D(P: numpy.ndarray[numpy.float64, _Shape[2, 1]], points: typing.List[numpy.ndarray[numpy.float64, _Shape[2, 1]]]) -> float: + pass +def distancePointToConvexHullProjectedTo2D(P: numpy.ndarray[numpy.float64, _Shape[3, 1]], points: typing.List[numpy.ndarray[numpy.float64, _Shape[3, 1]]], normal: numpy.ndarray[numpy.float64, _Shape[3, 1]] = array([0., 1., 0.])) -> float: + pass def eulerXYXToMatrix(angle: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 3]]: pass def eulerXYZToMatrix(angle: numpy.ndarray[numpy.float64, _Shape[3, 1]]) -> numpy.ndarray[numpy.float64, _Shape[3, 3]]: diff --git a/stubs/_nimblephysics-stubs/utils/__init__.pyi b/stubs/_nimblephysics-stubs/utils/__init__.pyi index 9ec654f32..283962428 100644 --- a/stubs/_nimblephysics-stubs/utils/__init__.pyi +++ b/stubs/_nimblephysics-stubs/utils/__init__.pyi @@ -10,6 +10,8 @@ _Shape = typing.Tuple[int, ...] __all__ = [ "AccelerationMinimizer", "AccelerationSmoother", + "AccelerationTrackAndMinimize", + "AccelerationTrackingResult", "DartLoader", "MJCFExporter", "SdfParser", @@ -20,7 +22,7 @@ __all__ = [ class AccelerationMinimizer(): - def __init__(self, numTimesteps: int, smoothingWeight: float, regularizationWeight: float, numIterations: int = 10000) -> None: ... + def __init__(self, numTimesteps: int, smoothingWeight: float = 1.0, regularizationWeight: float = 0.01, startPositionZeroWeight: float = 0.0, endPositionZeroWeight: float = 0.0, startVelocityZeroWeight: float = 0.0, endVelocityZeroWeight: float = 0.0, numIterations: int = 10000) -> None: ... def minimize(self, series: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: ... def setConvergenceTolerance(self, tolerance: float) -> None: ... def setDebugIterationBackoff(self, iterations: bool) -> None: ... @@ -32,6 +34,31 @@ class AccelerationSmoother(): def setIterations(self, iterations: int) -> None: ... def smooth(self, series: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: ... pass +class AccelerationTrackAndMinimize(): + def __init__(self, numTimesteps: int, trackAccelerationAtTimesteps: typing.List[bool], zeroUnobservedAccWeight: float = 1.0, trackObservedAccWeight: float = 1.0, regularizationWeight: float = 0.01, dt: float = 1.0, numIterations: int = 10000) -> None: ... + def minimize(self, series: numpy.ndarray[numpy.float64, _Shape[m, 1]], trackAcc: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> AccelerationTrackingResult: ... + def setConvergenceTolerance(self, tolerance: float) -> None: ... + def setDebugIterationBackoff(self, iterations: bool) -> None: ... + def setNumIterationsBackoff(self, series: int) -> None: ... + pass +class AccelerationTrackingResult(): + @property + def accelerationOffset(self) -> float: + """ + :type: float + """ + @accelerationOffset.setter + def accelerationOffset(self, arg0: float) -> None: + pass + @property + def series(self) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: + """ + :type: numpy.ndarray[numpy.float64, _Shape[m, 1]] + """ + @series.setter + def series(self, arg0: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> None: + pass + pass class DartLoader(): def __init__(self) -> None: ... def addPackageDirectory(self, packageName: str, packageDirectory: str) -> None: ... diff --git a/unittests/unit/CMakeLists.txt b/unittests/unit/CMakeLists.txt index 3d8f7b811..8907c2b9f 100644 --- a/unittests/unit/CMakeLists.txt +++ b/unittests/unit/CMakeLists.txt @@ -48,6 +48,9 @@ if(TARGET dart-utils) dart_add_test("unit" test_AccelerationMinimizer) target_link_libraries(test_AccelerationMinimizer dart-utils) + dart_add_test("unit" test_AccelerationTrackAndMinimize) + target_link_libraries(test_AccelerationTrackAndMinimize dart-utils) + dart_add_test("unit" test_IKInitializer) target_link_libraries(test_IKInitializer dart-utils) @@ -75,6 +78,9 @@ if(TARGET dart-utils) dart_add_test("unit" test_SkeletonToBallJoints) target_link_libraries(test_SkeletonToBallJoints dart-utils) + dart_add_test("unit" test_Convex2DDistanceMeasurement) + target_link_libraries(test_Convex2DDistanceMeasurement dart-utils) + dart_add_test("unit" test_SkeletonSimplification) target_link_libraries(test_SkeletonSimplification dart-utils) diff --git a/unittests/unit/test_AccelerationTrackAndMinimize.cpp b/unittests/unit/test_AccelerationTrackAndMinimize.cpp new file mode 100644 index 000000000..e674c45e2 --- /dev/null +++ b/unittests/unit/test_AccelerationTrackAndMinimize.cpp @@ -0,0 +1,172 @@ +#include +#include + +#include + +#include "dart/math/MathTypes.hpp" +#include "dart/utils/AccelerationMinimizer.hpp" +#include "dart/utils/AccelerationTrackAndMinimize.hpp" + +#include "TestHelpers.hpp" + +using namespace dart; +using namespace utils; + +// #define ALL_TESTS + +#ifdef ALL_TESTS +TEST(ACCEL_TRACK_AND_MINIMIZE, DOES_NOT_CRASH) +{ + int timesteps = 50; + std::vector trackAcc = std::vector(timesteps, false); + for (int i = 0; i < timesteps; i++) + { + if (i % 2 == 0) + { + trackAcc[i] = true; + } + } + AccelerationTrackAndMinimize minimizer(timesteps, trackAcc, 1.0, 1.0); + + Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps); + Eigen::VectorXs track = Eigen::VectorXs::Random(timesteps); + for (int i = 0; i < timesteps; i++) + { + if (!trackAcc[i]) + { + track[i] = 0.0; + } + } + + Eigen::VectorXs x = minimizer.minimize(series, track).series; + + std::cout << "Finished" << std::endl; + std::cout << x << std::endl; +} +#endif + +#ifdef ALL_TESTS +TEST(ACCEL_TRACK_AND_MINIMIZE, REDUCES_TO_ACC_MIN) +{ + int timesteps = 50; + std::vector trackAcc = std::vector(timesteps, false); + s_t smoothingWeight = 1.0; + s_t regularizationWeight = 0.01; + s_t dt = 1.0; + AccelerationTrackAndMinimize minimizer( + timesteps, trackAcc, smoothingWeight, 0.0, regularizationWeight, dt); + AccelerationMinimizer minimizer2(timesteps, 1.0, 0.01); + + Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps); + Eigen::VectorXs track = Eigen::VectorXs::Random(timesteps); + for (int i = 0; i < timesteps; i++) + { + if (!trackAcc[i]) + { + track[i] = 0.0; + } + } + + Eigen::VectorXs x = minimizer.minimize(series, track).series; + Eigen::VectorXs x2 = minimizer2.minimize(series); + + s_t dist = (x - x2).norm(); + if (dist > 1e-8) + { + std::cout << "x: " << x << std::endl; + std::cout << "x2: " << x2 << std::endl; + } + EXPECT_TRUE(dist < 1e-8); +} +#endif + +#ifdef ALL_TESTS +TEST(ACCEL_TRACK_AND_MINIMIZE, PERFECTLY_TRACKS_ACC) +{ + int timesteps = 10; + std::vector trackAcc = std::vector(timesteps, true); + s_t dt = 0.1; + AccelerationTrackAndMinimize minimizer( + timesteps, trackAcc, 0.0, 1.0, 0.0, dt); + minimizer.setDebugIterationBackoff(true); + + Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps); + Eigen::VectorXs acc = Eigen::VectorXs::Zero(timesteps); + for (int i = 1; i < timesteps - 1; i++) + { + acc[i] = (series[i + 1] - 2 * series[i] + series[i - 1]) / (dt * dt); + } + acc[0] = acc[1]; + acc[timesteps - 1] = acc[timesteps - 2]; + + Eigen::VectorXs initialization = Eigen::VectorXs::Random(timesteps); + + Eigen::VectorXs x = minimizer.minimize(initialization, acc).series; + + Eigen::VectorXs recoveredAcc = Eigen::VectorXs::Zero(timesteps); + for (int i = 1; i < timesteps - 1; i++) + { + recoveredAcc[i] = (x[i + 1] - 2 * x[i] + x[i - 1]) / (dt * dt); + } + recoveredAcc[0] = recoveredAcc[1]; + recoveredAcc[timesteps - 1] = recoveredAcc[timesteps - 2]; + s_t dist = (recoveredAcc - acc).norm(); + if (dist > 1e-8) + { + Eigen::MatrixXs compare = Eigen::MatrixXs(timesteps, 3); + compare.col(0) = acc; + compare.col(1) = recoveredAcc; + compare.col(2) = acc - recoveredAcc; + std::cout << "acc - recovered acc - diff: " << std::endl; + std::cout << compare << std::endl; + } + EXPECT_TRUE(dist < 1e-8); +} +#endif + +#ifdef ALL_TESTS +TEST(ACCEL_TRACK_AND_MINIMIZE, TRACKS_OFFSET_ACCS) +{ + int timesteps = 10; + std::vector trackAcc = std::vector(timesteps, true); + s_t dt = 0.1; + AccelerationTrackAndMinimize minimizer( + timesteps, trackAcc, 1.0, 1.0, 1.0, dt); + minimizer.setDebugIterationBackoff(true); + + Eigen::VectorXs series = Eigen::VectorXs::Random(timesteps); + Eigen::VectorXs acc = Eigen::VectorXs::Zero(timesteps); + for (int i = 1; i < timesteps - 1; i++) + { + acc[i] = (series[i + 1] - 2 * series[i] + series[i - 1]) / (dt * dt); + } + acc[0] = acc[1]; + acc[timesteps - 1] = acc[timesteps - 2]; + + Eigen::VectorXs offsetAcc = acc + Eigen::VectorXs::Ones(timesteps) * 0.5; + + AccelerationTrackingResult result = minimizer.minimize(series, offsetAcc); + Eigen::VectorXs x = result.series; + s_t offset = result.accelerationOffset; + EXPECT_NEAR(offset, 0.5, 1e-6); + + Eigen::VectorXs recoveredAcc = Eigen::VectorXs::Zero(timesteps); + for (int i = 1; i < timesteps - 1; i++) + { + recoveredAcc[i] = (x[i + 1] - 2 * x[i] + x[i - 1]) / (dt * dt); + } + recoveredAcc[0] = recoveredAcc[1]; + recoveredAcc[timesteps - 1] = recoveredAcc[timesteps - 2]; + s_t dist = (recoveredAcc - acc).norm(); + if (dist > 1e-6) + { + Eigen::MatrixXs compare = Eigen::MatrixXs(timesteps, 3); + compare.col(0) = acc; + compare.col(1) = recoveredAcc; + compare.col(2) = acc - recoveredAcc; + std::cout << "acc - recovered acc - diff: " << std::endl; + std::cout << compare << std::endl; + } + EXPECT_TRUE(dist < 1e-6); +} +#endif \ No newline at end of file diff --git a/unittests/unit/test_Convex2DDistanceMeasurement.cpp b/unittests/unit/test_Convex2DDistanceMeasurement.cpp new file mode 100644 index 000000000..366462e0f --- /dev/null +++ b/unittests/unit/test_Convex2DDistanceMeasurement.cpp @@ -0,0 +1,36 @@ +#include + +#include "dart/biomechanics/OpenSimParser.hpp" +#include "dart/biomechanics/SkeletonConverter.hpp" +#include "dart/dynamics/BallJoint.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/FreeJoint.hpp" +#include "dart/dynamics/Joint.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/math/Geometry.hpp" +#include "dart/math/MathTypes.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/utils/DartResourceRetriever.hpp" + +#include "GradientTestUtils.hpp" +#include "TestHelpers.hpp" + +// #define ALL_TESTS + +using namespace dart; +using namespace biomechanics; +using namespace server; +using namespace realtime; + +TEST(DIST_FROM_CONVEX_HULL_WITH_CHILDREN, DOES_NOT_CRASH) +{ + std::shared_ptr osim + = OpenSimParser::parseOsim( + "dart://sample/osim/Rajagopal2015/Rajagopal2015.osim") + .skeleton; + dynamics::BodyNode* leftFoot = osim->getBodyNode("calcn_l"); + s_t distance = leftFoot->distanceFrom2DConvexHullWithChildren( + Eigen::Vector3s(0, 0, 0), Eigen::Vector3s(0, 1.0, 0)); + std::cout << "Distance: " << distance << std::endl; +} \ No newline at end of file diff --git a/unittests/unit/test_Geometry.cpp b/unittests/unit/test_Geometry.cpp index 381ff0ca7..98b7eede3 100644 --- a/unittests/unit/test_Geometry.cpp +++ b/unittests/unit/test_Geometry.cpp @@ -54,7 +54,10 @@ using namespace simulation; #define LIE_GROUP_OPT_TOL 1e-12 -#define ALL_TESTS +// Tolerance for floating-point comparisons +#define EPSILON 1e-6 + +// #define ALL_TESTS /******************************************************************************/ Eigen::Matrix4s toMatrixForm(const Eigen::Vector6s& v) @@ -1248,3 +1251,233 @@ TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS) } } #endif + +/******************************************************************************/ +// #ifdef ALL_TESTS +TEST(TWO_D_CONVEX_OPS, SIMPLE_TEST) +{ + // Define the set of points (some of them may be internal) + std::vector points; + points.push_back(Eigen::Vector2s(0, 0)); + points.push_back(Eigen::Vector2s(1, 0)); + points.push_back(Eigen::Vector2s(1, 1)); + points.push_back(Eigen::Vector2s(0, 1)); + points.push_back(Eigen::Vector2s(0.5, 0.5)); // Internal point + + // Define the point P + Eigen::Vector2s P(0.5, -0.5); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + std::cout << "Distance from point to convex hull: " << distance << std::endl; + + EXPECT_EQ(distance, 0.5); +} +// #endif + +// Test 1: Point outside a square convex hull +TEST(TWO_D_CONVEX_OPS, PointOutsideSquare) +{ + // Define the set of points (square) + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(1, 0)); + points.push_back(Vector2s(1, 1)); + points.push_back(Vector2s(0, 1)); + + // Define the point P outside the square + Vector2s P(0.5, -0.5); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 0.5 + EXPECT_NEAR(distance, 0.5, EPSILON); +} + +// Test 2: Point inside the convex hull +TEST(TWO_D_CONVEX_OPS, PointInsideConvexHull) +{ + // Define the set of points (square) + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(2, 0)); + points.push_back(Vector2s(2, 2)); + points.push_back(Vector2s(0, 2)); + + // Define the point P inside the square + Vector2s P(1, 1); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 0.0 (inside the convex hull) + EXPECT_NEAR(distance, 0.0, EPSILON); +} + +// Test 3: Point on the edge of the convex hull +TEST(TWO_D_CONVEX_OPS, PointOnEdge) +{ + // Define the set of points (triangle) + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(2, 0)); + points.push_back(Vector2s(1, std::sqrt(3))); + + // Define the point P on one of the edges + Vector2s P(1, 0); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 0.0 (on the edge) + EXPECT_NEAR(distance, 0.0, EPSILON); +} + +// Test 4: Point outside a convex hull with internal points +TEST(TWO_D_CONVEX_OPS, ConvexHullWithInternalPoints) +{ + // Define the set of points forming a convex hull with internal points + std::vector points; + points.push_back(Vector2s(-1, 0)); + points.push_back(Vector2s(0, 1)); + points.push_back(Vector2s(1, 0)); + points.push_back(Vector2s(0, -1)); + points.push_back(Vector2s(0, 0)); // Internal point + + // Define the point P outside the convex hull + Vector2s P(2, 0); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 1.0 (distance from P(2,0) to the hull edge at (1,0)) + EXPECT_NEAR(distance, 1.0, EPSILON); +} + +// Test 5: Convex hull is a line segment +TEST(TWO_D_CONVEX_OPS, ConvexHullLineSegment) +{ + // Define the set of points forming a line segment + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(2, 0)); + points.push_back(Vector2s(1, 0)); // Colinear point + + // Define the point P above the line segment + Vector2s P(1, 1); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 1.0 (vertical distance) + EXPECT_NEAR(distance, 1.0, EPSILON); +} + +// Test 6: Convex hull is a single point +TEST(TWO_D_CONVEX_OPS, ConvexHullSinglePoint) +{ + // Define the set of points with a single point + std::vector points; + points.push_back(Vector2s(0, 0)); + + // Define the point P somewhere else + Vector2s P(1, 1); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is sqrt(2) + EXPECT_NEAR(distance, std::sqrt(2), EPSILON); +} + +// Test 7: Point outside an irregular convex hull +TEST(TWO_D_CONVEX_OPS, IrregularConvexHull) +{ + // Define the set of points forming an irregular convex shape + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(2, 1)); + points.push_back(Vector2s(1, 3)); + points.push_back(Vector2s(-1, 2)); + points.push_back(Vector2s(-2, 1)); + points.push_back(Vector2s(0, 1)); // Internal point + + // Define the point P outside the convex hull + Vector2s P(3, 2); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is approximately distance from P(3,2) to edge between + // (2,1) and (1,3) + s_t expected_distance = std::hypot(1.2, 0.6); + + EXPECT_NEAR(distance, expected_distance, EPSILON); +} + +// Test 8: Point at a vertex of the convex hull +TEST(TWO_D_CONVEX_OPS, PointAtVertex) +{ + // Define the set of points forming a convex quadrilateral + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(2, 0)); + points.push_back(Vector2s(2, 2)); + points.push_back(Vector2s(0, 2)); + + // Define the point P at one of the vertices + Vector2s P(2, 2); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is 0.0 (point is on the convex hull) + EXPECT_NEAR(distance, 0.0, EPSILON); +} + +// Test 9: Point very close to the convex hull edge +TEST(TWO_D_CONVEX_OPS, PointNearEdge) +{ + // Define the set of points forming a square + std::vector points; + points.push_back(Vector2s(0, 0)); + points.push_back(Vector2s(1, 0)); + points.push_back(Vector2s(1, 1)); + points.push_back(Vector2s(0, 1)); + + // Define the point P very close to the right edge + Vector2s P(1 + 1e-7, 0.5); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is approximately 1e-7 + EXPECT_NEAR(distance, 1e-7, EPSILON); +} + +// Test 10: Large set of random points forming a circle +TEST(TWO_D_CONVEX_OPS, LargeRandomPoints) +{ + // Generate points around a circle + std::vector points; + int num_points = 100; + s_t radius = 10.0; + + for (int i = 0; i < num_points; ++i) + { + s_t angle = (2 * M_PI * i) / num_points; + points.push_back( + Vector2s(radius * std::cos(angle), radius * std::sin(angle))); + } + + // Define the point P outside the circle + Vector2s P(0, 15); + + // Compute the distance + s_t distance = distancePointToConvexHull2D(P, points); + + // Expected distance is approximately 5.0 + EXPECT_NEAR(distance, 5.0, EPSILON); +} \ No newline at end of file