diff --git a/dart/biomechanics/SubjectOnDisk.cpp b/dart/biomechanics/SubjectOnDisk.cpp index d87bf813a..3388b83d9 100644 --- a/dart/biomechanics/SubjectOnDisk.cpp +++ b/dart/biomechanics/SubjectOnDisk.cpp @@ -1836,6 +1836,11 @@ void SubjectOnDiskTrialPass::setType(ProcessingPassType type) mType = type; } +ProcessingPassType SubjectOnDiskTrialPass::getType() +{ + return mType; +} + void SubjectOnDiskTrialPass::setPoses(Eigen::MatrixXs poses) { mPos = poses; @@ -2034,6 +2039,31 @@ Eigen::MatrixXs SubjectOnDiskTrialPass::getRootEulerHistoryInRootFrame() return mRootEulerHistoryInRootFrame; } +// This gets the data from `getGroundBodyCopTorqueForce()` in the form of +// ForcePlate objects, which are easier to work with. +std::vector SubjectOnDiskTrialPass::getProcessedForcePlates() +{ + std::vector plates; + int numPlates = (int)(mGroundBodyCopTorqueForce.rows() / 9); + for (int i = 0; i < numPlates; i++) + { + ForcePlate plate; + for (int t = 0; t < mGroundBodyCopTorqueForce.cols(); t++) + { + Eigen::Vector3s cop = mGroundBodyCopTorqueForce.block(i * 9, t, 3, 1); + Eigen::Vector3s torque + = mGroundBodyCopTorqueForce.block(i * 9 + 3, t, 3, 1); + Eigen::Vector3s force + = mGroundBodyCopTorqueForce.block(i * 9 + 6, t, 3, 1); + plate.centersOfPressure.push_back(cop); + plate.moments.push_back(torque); + plate.forces.push_back(force); + } + plates.push_back(plate); + } + return plates; +} + // This will return a matrix where every one of our properties with setters is // stacked together vertically. Each column represents time, and each row is a // different property of interest. The point here is not to introspect into @@ -3112,6 +3142,46 @@ void SubjectOnDiskTrial::setSplitIndex(int split) mSplitIndex = split; } +int SubjectOnDiskTrial::getOriginalTrialStartFrame() +{ + return mOriginalTrialStartFrame; +} + +void SubjectOnDiskTrial::setOriginalTrialStartFrame(int startFrame) +{ + mOriginalTrialStartFrame = startFrame; +} + +int SubjectOnDiskTrial::getOriginalTrialEndFrame() +{ + return mOriginalTrialEndFrame; +} + +void SubjectOnDiskTrial::setOriginalTrialEndFrame(int endFrame) +{ + mOriginalTrialEndFrame = endFrame; +} + +s_t SubjectOnDiskTrial::getOriginalTrialStartTime() +{ + return mOriginalTrialStartTime; +} + +void SubjectOnDiskTrial::setOriginalTrialStartTime(s_t startTime) +{ + mOriginalTrialStartTime = startTime; +} + +s_t SubjectOnDiskTrial::getOriginalTrialEndTime() +{ + return mOriginalTrialEndTime; +} + +void SubjectOnDiskTrial::setOriginalTrialEndTime(s_t endTime) +{ + mOriginalTrialEndTime = endTime; +} + std::vector SubjectOnDiskTrial::getMissingGRFReason() { return mMissingGRFReason; @@ -3214,6 +3284,11 @@ void SubjectOnDiskTrial::read(const proto::SubjectOnDiskTrialHeader& proto) mSplitIndex = proto.split_index(); + mOriginalTrialStartFrame = proto.original_trial_start_frame(); + mOriginalTrialEndFrame = proto.original_trial_end_frame(); + mOriginalTrialStartTime = proto.original_trial_start_time(); + mOriginalTrialEndTime = proto.original_trial_end_time(); + mTrialPasses.clear(); for (int i = 0; i < proto.processing_pass_header_size(); i++) { @@ -3281,6 +3356,10 @@ void SubjectOnDiskTrial::write(proto::SubjectOnDiskTrialHeader* proto) proto->set_original_name(mOriginalTrialName); proto->set_split_index(mSplitIndex); + proto->set_original_trial_start_frame(mOriginalTrialStartFrame); + proto->set_original_trial_end_frame(mOriginalTrialEndFrame); + proto->set_original_trial_start_time(mOriginalTrialStartTime); + proto->set_original_trial_end_time(mOriginalTrialEndTime); // std::vector mTrialTags; for (int i = 0; i < mTrialTags.size(); i++) diff --git a/dart/biomechanics/SubjectOnDisk.hpp b/dart/biomechanics/SubjectOnDisk.hpp index af1f63c77..0f4d0c177 100644 --- a/dart/biomechanics/SubjectOnDisk.hpp +++ b/dart/biomechanics/SubjectOnDisk.hpp @@ -134,6 +134,7 @@ class SubjectOnDiskTrialPass public: SubjectOnDiskTrialPass(); void setType(ProcessingPassType type); + ProcessingPassType getType(); void setDofPositionsObserved(std::vector dofPositionsObserved); void setDofVelocitiesFiniteDifferenced( std::vector dofVelocitiesFiniteDifferenced); @@ -248,6 +249,10 @@ class SubjectOnDiskTrialPass void setRootEulerHistoryInRootFrame(Eigen::MatrixXs rootHistory); Eigen::MatrixXs getRootEulerHistoryInRootFrame(); + // This gets the data from `getGroundBodyCopTorqueForce()` in the form of + // ForcePlate objects, which are easier to work with. + std::vector getProcessedForcePlates(); + // This will return a matrix where every one of our properties with setters is // stacked together vertically. Each column represents time, and each row is a // different property of interest. The point here is not to introspect into @@ -332,6 +337,14 @@ class SubjectOnDiskTrial void setOriginalTrialName(const std::string& name); int getSplitIndex(); void setSplitIndex(int split); + int getOriginalTrialStartFrame(); + void setOriginalTrialStartFrame(int startFrame); + int getOriginalTrialEndFrame(); + void setOriginalTrialEndFrame(int endFrame); + s_t getOriginalTrialStartTime(); + void setOriginalTrialStartTime(s_t startTime); + s_t getOriginalTrialEndTime(); + void setOriginalTrialEndTime(s_t endTime); std::vector getMissingGRFReason(); void setMissingGRFReason(std::vector missingGRFReason); void setCustomValues(std::vector customValues); @@ -367,6 +380,11 @@ class SubjectOnDiskTrial std::string mOriginalTrialName; int mSplitIndex; + int mOriginalTrialStartFrame; + int mOriginalTrialEndFrame; + s_t mOriginalTrialStartTime; + s_t mOriginalTrialEndTime; + /////////////////////////////////////////////////////////////////////////// // Recovered proto summaries, for incremental loading of Frames /////////////////////////////////////////////////////////////////////////// diff --git a/dart/proto/SubjectOnDisk.proto b/dart/proto/SubjectOnDisk.proto index 2d1a1e38a..0ebd6f17d 100644 --- a/dart/proto/SubjectOnDisk.proto +++ b/dart/proto/SubjectOnDisk.proto @@ -97,6 +97,11 @@ message SubjectOnDiskTrialHeader { string original_name = 10; // This is the split index within the original trial -- 0 if we didn't split or if we're the first split int32 split_index = 11; + // This is the shorthand for values from the original trial + int32 original_trial_start_frame = 12; + int32 original_trial_end_frame = 13; + float original_trial_start_time = 14; + float original_trial_end_time = 15; } message SubjectOnDiskPass { diff --git a/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp b/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp index 534117806..bb4e16e5a 100644 --- a/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp +++ b/python/_nimblephysics/biomechanics/SubjectOnDisk.cpp @@ -533,6 +533,8 @@ Note that these are specified in the local body frame, acting on the body at its "setType", &dart::biomechanics::SubjectOnDiskTrialPass::setType, ::py::arg("type")) + .def( + "getType", &dart::biomechanics::SubjectOnDiskTrialPass::getType) .def( "setDofPositionsObserved", &dart::biomechanics::SubjectOnDiskTrialPass:: @@ -798,7 +800,11 @@ Note that these are specified in the local body frame, acting on the body at its .def( "getResamplingMatrix", &dart::biomechanics::SubjectOnDiskTrialPass:: - getResamplingMatrix); + getResamplingMatrix) + .def( + "getProcessedForcePlates", + &dart::biomechanics::SubjectOnDiskTrialPass:: + getProcessedForcePlates); auto subjectOnDiskTrial = ::py::class_< @@ -842,6 +848,42 @@ Note that these are specified in the local body frame, acting on the body at its .def( "getSplitIndex", &dart::biomechanics::SubjectOnDiskTrial::getSplitIndex) + .def( + "setOriginalTrialStartFrame", + &dart::biomechanics::SubjectOnDiskTrial:: + setOriginalTrialStartFrame, + ::py::arg("startFrame")) + .def( + "getOriginalTrialStartFrame", + &dart::biomechanics::SubjectOnDiskTrial:: + getOriginalTrialStartFrame) + .def( + "setOriginalTrialEndFrame", + &dart::biomechanics::SubjectOnDiskTrial:: + setOriginalTrialEndFrame, + ::py::arg("endFrame")) + .def( + "getOriginalTrialEndFrame", + &dart::biomechanics::SubjectOnDiskTrial:: + getOriginalTrialEndFrame) + .def( + "setOriginalTrialStartTime", + &dart::biomechanics::SubjectOnDiskTrial:: + setOriginalTrialStartTime, + ::py::arg("startTime")) + .def( + "getOriginalTrialStartTime", + &dart::biomechanics::SubjectOnDiskTrial:: + getOriginalTrialStartTime) + .def( + "setOriginalTrialEndTime", + &dart::biomechanics::SubjectOnDiskTrial:: + setOriginalTrialEndTime, + ::py::arg("endTime")) + .def( + "getOriginalTrialEndTime", + &dart::biomechanics::SubjectOnDiskTrial:: + getOriginalTrialEndTime) .def( "setMissingGRFReason", &dart::biomechanics::SubjectOnDiskTrial::setMissingGRFReason,