diff --git a/dart/exo/ExoSolverPinnedContact.cpp b/dart/exo/ExoSolverPinnedContact.cpp index 1888156e5..1b484daa0 100644 --- a/dart/exo/ExoSolverPinnedContact.cpp +++ b/dart/exo/ExoSolverPinnedContact.cpp @@ -184,6 +184,25 @@ Eigen::VectorXs ExoSolverPinnedContact::estimateHumanTorques( return tau; } +//============================================================================== +/// This is part of the main exoskeleton solver. It takes in the current +/// joint velocities and accelerations, and returns the estimated total +/// joint torques for the human + exoskeleton system. +Eigen::VectorXs ExoSolverPinnedContact::estimateTotalTorques( + Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces) +{ + mRealSkel->setVelocities(dq); + + Eigen::MatrixXs M = mRealSkel->getMassMatrix(); + Eigen::VectorXs C = mRealSkel->getCoriolisAndGravityForces() + - mRealSkel->getExternalForces(); + Eigen::VectorXs contactJointTorques + = getContactJacobian().transpose() * contactForces; + Eigen::VectorXs tau = M * ddq + C - contactJointTorques; + + return tau; +} + //============================================================================== /// This is part of the main exoskeleton solver. It takes in the current /// estimated human pilot joint torques, and computes the accelerations we @@ -311,9 +330,13 @@ ExoSolverPinnedContact::getPinnedRealDynamicsLinearMap(Eigen::VectorXs dq) /// This is part of the main exoskeleton solver. It takes in how the digital /// twin of the exo pilot is accelerating, and attempts to solve for the /// torques that the exo needs to apply to get as close to that as possible. +/// It resolves ambiguities by minimizing the exo torques. std::pair ExoSolverPinnedContact::getPinnedTotalTorques( - Eigen::VectorXs dq, Eigen::VectorXs ddqDesired) + Eigen::VectorXs dq, + Eigen::VectorXs ddqDesired, + Eigen::VectorXs centeringTau, + Eigen::VectorXs centeringForces) { mRealSkel->setVelocities(dq); @@ -334,7 +357,14 @@ ExoSolverPinnedContact::getPinnedTotalTorques( A.block(0, numDofs, contactJointTorques.cols(), contactJointTorques.rows()) = contactJointTorques.transpose(); - Eigen::VectorXs solution = A.completeOrthogonalDecomposition().solve(b); + Eigen::VectorXs centering + = Eigen::VectorXs::Zero(numDofs + contactJointTorques.rows()); + centering.segment(0, numDofs) = centeringTau; + centering.segment(numDofs, contactJointTorques.rows()) = centeringForces; + + Eigen::VectorXs solution + = A.completeOrthogonalDecomposition().solve(b - A * centering) + + centering; Eigen::VectorXs tauTotal = solution.segment(0, numDofs); Eigen::VectorXs f = solution.segment(numDofs, contactJointTorques.rows()); @@ -396,6 +426,40 @@ ExoSolverPinnedContact::projectTorquesToExoControlSpaceLinearMap() return J.completeOrthogonalDecomposition().pseudoInverse(); } +//============================================================================== +/// Often our estimates for `dq` and `ddq` violate the pin constraints. That +/// leads to exo torques that do not tend to zero as the virtual human exactly +/// matches the real human+exo system. To solve this problem, we can solve a +/// set of least-squares equations to find the best set of ddq values to +/// satisfy the constraint. +Eigen::VectorXs ExoSolverPinnedContact:: + getClosestRealAccelerationConsistentWithPinsAndContactForces( + Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces) +{ + mRealSkel->setVelocities(dq); + + Eigen::MatrixXs J = getContactJacobian(); + Eigen::MatrixXs M = mRealSkel->getMassMatrix(); + Eigen::VectorXs C = mRealSkel->getCoriolisAndGravityForces() + - mRealSkel->getExternalForces(); + Eigen::VectorXs contactTau = J.transpose() * contactForces; + + const int numDofs = dq.size(); + + Eigen::MatrixXs A = Eigen::MatrixXs::Zero(6 + J.rows(), numDofs); + A.block(0, 0, 6, numDofs) = M.block(0, 0, 6, numDofs); + A.block(6, 0, J.rows(), numDofs) = J; + + Eigen::VectorXs b = Eigen::VectorXs::Zero(6 + J.rows()); + b.segment<6>(0) = contactTau.segment<6>(0) - C.segment<6>(0); + + // Solve, centered on ddq + Eigen::VectorXs solution + = A.completeOrthogonalDecomposition().solve(b - A * ddq) + ddq; + + return solution; +} + //============================================================================== /// This runs the entire exoskeleton solver pipeline, spitting out the /// torques to apply to the exoskeleton actuators. @@ -405,20 +469,25 @@ Eigen::VectorXs ExoSolverPinnedContact::solveFromAccelerations( Eigen::VectorXs lastExoTorques, Eigen::VectorXs contactForces) { + Eigen::VectorXs systemTau = estimateTotalTorques(dq, ddq, contactForces); Eigen::VectorXs humanTau = estimateHumanTorques(dq, ddq, contactForces, lastExoTorques); - return solveFromBiologicalTorques(dq, humanTau); + return solveFromBiologicalTorques(dq, humanTau, systemTau, contactForces); } //============================================================================== /// This is a subset of the steps in solveFromAccelerations, which can take /// the biological joint torques directly, and solve for the exo torques. Eigen::VectorXs ExoSolverPinnedContact::solveFromBiologicalTorques( - Eigen::VectorXs dq, Eigen::VectorXs humanTau) + Eigen::VectorXs dq, + Eigen::VectorXs humanTau, + Eigen::VectorXs centeringTau, + Eigen::VectorXs centeringForces) { Eigen::VectorXs virtualDdq = getPinnedVirtualDynamics(dq, humanTau).first; Eigen::VectorXs totalRealTorques - = getPinnedTotalTorques(dq, virtualDdq).first; + = getPinnedTotalTorques(dq, virtualDdq, centeringTau, centeringForces) + .first; Eigen::VectorXs netTorques = totalRealTorques - humanTau; Eigen::VectorXs exoTorques = projectTorquesToExoControlSpace(netTorques); return exoTorques; @@ -459,7 +528,8 @@ std::tuple ExoSolverPinnedContact::getPinnedForwardDynamicsForExoAndHuman( Eigen::VectorXs dq, Eigen::VectorXs humanTau) { - Eigen::VectorXs exoTau = solveFromBiologicalTorques(dq, humanTau); + Eigen::VectorXs exoTau = solveFromBiologicalTorques( + dq, humanTau, humanTau, Eigen::VectorXs::Zero(3 * mPins.size())); Eigen::VectorXs exoTauOnJoints = getExoToJointTorquesJacobian() * exoTau; Eigen::VectorXs totalTau = humanTau + exoTauOnJoints; std::pair ddqAndForces @@ -520,7 +590,8 @@ ExoSolverPinnedContact::getHumanAndExoTorques( .solve(ddq - humanTorquesLinearForwardDynamics.second); // Reconstruct the exo torque from the human torque - Eigen::VectorXs exoTau = solveFromBiologicalTorques(dq, humanTau); + Eigen::VectorXs exoTau = solveFromBiologicalTorques( + dq, humanTau, humanTau, Eigen::VectorXs::Zero(3 * mPins.size())); return std::make_pair(humanTau, exoTau); } diff --git a/dart/exo/ExoSolverPinnedContact.hpp b/dart/exo/ExoSolverPinnedContact.hpp index 275ba839e..1c2d2419a 100644 --- a/dart/exo/ExoSolverPinnedContact.hpp +++ b/dart/exo/ExoSolverPinnedContact.hpp @@ -65,6 +65,12 @@ class ExoSolverPinnedContact Eigen::VectorXs contactForces, Eigen::VectorXs lastExoTorques); + /// This is part of the main exoskeleton solver. It takes in the current + /// joint velocities and accelerations, and returns the estimated total + /// joint torques for the human + exoskeleton system. + Eigen::VectorXs estimateTotalTorques( + Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces); + /// This is part of the main exoskeleton solver. It takes in the current /// estimated human pilot joint torques, and computes the accelerations we /// would see on the virtual skeleton if we applied those same torques, with @@ -93,8 +99,12 @@ class ExoSolverPinnedContact /// This is part of the main exoskeleton solver. It takes in how the digital /// twin of the exo pilot is accelerating, and attempts to solve for the /// torques that the exo needs to apply to get as close to that as possible. + /// It resolves ambiguities by minimizing the change in total system torques. std::pair getPinnedTotalTorques( - Eigen::VectorXs dq, Eigen::VectorXs ddqDesired); + Eigen::VectorXs dq, + Eigen::VectorXs ddqDesired, + Eigen::VectorXs centeringTau, + Eigen::VectorXs centeringForces); /// This does the same thing as getPinnedTotalTorques, but returns the Ax + /// b values A and b such that Ax + b = tau, accounting for the pin @@ -111,6 +121,14 @@ class ExoSolverPinnedContact /// the matrix to multiply by the torques to get the exo torques. Eigen::MatrixXs projectTorquesToExoControlSpaceLinearMap(); + /// Often our estimates for `dq` and `ddq` violate the pin constraints. That + /// leads to exo torques that do not tend to zero as the virtual human exactly + /// matches the real human+exo system. To solve this problem, we can solve a + /// set of least-squares equations to find the best set of ddq values to + /// satisfy the constraint. + Eigen::VectorXs getClosestRealAccelerationConsistentWithPinsAndContactForces( + Eigen::VectorXs dq, Eigen::VectorXs ddq, Eigen::VectorXs contactForces); + /// This runs the entire exoskeleton solver pipeline, spitting out the /// torques to apply to the exoskeleton actuators. Eigen::VectorXs solveFromAccelerations( @@ -122,7 +140,10 @@ class ExoSolverPinnedContact /// This is a subset of the steps in solveFromAccelerations, which can take /// the biological joint torques directly, and solve for the exo torques. Eigen::VectorXs solveFromBiologicalTorques( - Eigen::VectorXs dq, Eigen::VectorXs tau); + Eigen::VectorXs dq, + Eigen::VectorXs humanTau, + Eigen::VectorXs centeringTau, + Eigen::VectorXs centeringForces); /// This is the same as solveFromBiologicalTorques, but returns the Ax + b /// values A and b such that Ax + b = exo_tau, accounting for the pin diff --git a/python/_nimblephysics/dynamics/Skeleton.cpp b/python/_nimblephysics/dynamics/Skeleton.cpp index f8110daea..6d53e9265 100644 --- a/python/_nimblephysics/dynamics/Skeleton.cpp +++ b/python/_nimblephysics/dynamics/Skeleton.cpp @@ -1096,6 +1096,7 @@ void Skeleton( bool scaleBodies, double convergenceThreshold, int maxStepCount, + int numIndependentStarts, double leastSquaresDamping, bool lineSearch, bool logOutput) -> double { @@ -1108,6 +1109,7 @@ void Skeleton( .setConvergenceThreshold(convergenceThreshold) .setMaxStepCount(maxStepCount) .setLeastSquaresDamping(leastSquaresDamping) + .setMaxRestarts(numIndependentStarts) .setLineSearch(lineSearch) .setLogOutput(logOutput)); }, @@ -1117,6 +1119,7 @@ void Skeleton( ::py::arg("scaleBodies") = false, ::py::arg("convergenceThreshold") = 1e-7, ::py::arg("maxStepCount") = 100, + ::py::arg("numIndependentStarts") = 1, ::py::arg("leastSquaresDamping") = 0.01, ::py::arg("lineSearch") = true, ::py::arg("logOutput") = false) diff --git a/python/_nimblephysics/exo/ExoSolverPinnedContact.cpp b/python/_nimblephysics/exo/ExoSolverPinnedContact.cpp index d504a99d2..07c084eb1 100644 --- a/python/_nimblephysics/exo/ExoSolverPinnedContact.cpp +++ b/python/_nimblephysics/exo/ExoSolverPinnedContact.cpp @@ -112,6 +112,15 @@ void ExoSolverPinnedContact(py::module& m) "current joint velocities and accelerations, and the last " "exoskeleton torques, and returns the estimated human pilot joint " "torques.") + .def( + "estimateTotalTorques", + &dart::exo::ExoSolverPinnedContact::estimateTotalTorques, + ::py::arg("dq"), + ::py::arg("ddq"), + ::py::arg("contactForces"), + "This is part of the main exoskeleton solver. It takes in the " + "current joint velocities and accelerations, and returns the " + "estimated human + exo system joint torques.") .def( "getPinnedVirtualDynamics", &dart::exo::ExoSolverPinnedContact::getPinnedVirtualDynamics, @@ -148,6 +157,8 @@ void ExoSolverPinnedContact(py::module& m) &dart::exo::ExoSolverPinnedContact::getPinnedTotalTorques, ::py::arg("dq"), ::py::arg("ddqDesired"), + ::py::arg("centeringTau"), + ::py::arg("centeringForces"), "This is part of the main exoskeleton solver. It takes in how the " "digital twin of the exo pilot is accelerating, and attempts to " "solve for the torques that the exo needs to apply to get as close " @@ -166,6 +177,19 @@ void ExoSolverPinnedContact(py::module& m) "This is part of the main exoskeleton solver. It takes in the " "desired torques for the exoskeleton, and returns the torques on the " "actuated DOFs that can be used to drive the exoskeleton.") + .def( + "getClosestRealAccelerationConsistentWithPinsAndContactForces", + &dart::exo::ExoSolverPinnedContact:: + getClosestRealAccelerationConsistentWithPinsAndContactForces, + ::py::arg("dq"), + ::py::arg("ddq"), + ::py::arg("contactForces"), + "Often our estimates for `dq` and `ddq` violate the pin " + "constraints. That leads to exo torques that do not tend to zero " + "as the virtual human exactly matches the real human+exo system. " + "To solve this problem, we can solve a set of least-squares " + "equations to find the best set of ddq values to satisfy the " + "constraint.") .def( "projectTorquesToExoControlSpaceLinearMap", &dart::exo::ExoSolverPinnedContact:: @@ -187,6 +211,8 @@ void ExoSolverPinnedContact(py::module& m) &dart::exo::ExoSolverPinnedContact::solveFromBiologicalTorques, ::py::arg("dq"), ::py::arg("tau"), + ::py::arg("centeringTau"), + ::py::arg("centeringForce"), "This is a subset of the steps in solveFromAccelerations, which can " "take the biological joint torques directly, and solve for the exo " "torques.") diff --git a/stubs/_nimblephysics-stubs/dynamics/__init__.pyi b/stubs/_nimblephysics-stubs/dynamics/__init__.pyi index 340d11210..917a07a3c 100644 --- a/stubs/_nimblephysics-stubs/dynamics/__init__.pyi +++ b/stubs/_nimblephysics-stubs/dynamics/__init__.pyi @@ -4723,7 +4723,7 @@ class Skeleton(MetaSkeleton): def enableAdjacentBodyCheck(self) -> None: ... def enableSelfCollisionCheck(self) -> None: ... def fitJointsToWorldPositions(self, positionJoints: typing.List[Joint], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ... - def fitMarkersToWorldPositions(self, markers: typing.List[typing.Tuple[BodyNode, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], markerWeights: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ... + def fitMarkersToWorldPositions(self, markers: typing.List[typing.Tuple[BodyNode, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], targetPositions: numpy.ndarray[numpy.float64, _Shape[m, 1]], markerWeights: numpy.ndarray[numpy.float64, _Shape[m, 1]], scaleBodies: bool = False, convergenceThreshold: float = 1e-07, maxStepCount: int = 100, numIndependentStarts: int = 1, leastSquaresDamping: float = 0.01, lineSearch: bool = True, logOutput: bool = False) -> float: ... def getAccMapReadings(self, accelerometers: typing.Dict[str, typing.Tuple[BodyNode, nimblephysics_libs._nimblephysics.math.Isometry3]]) -> typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]: """ These are a set of bodies, and offsets in local body space where gyros diff --git a/stubs/_nimblephysics-stubs/exo/__init__.pyi b/stubs/_nimblephysics-stubs/exo/__init__.pyi index b7695fd5e..1bb58e86d 100644 --- a/stubs/_nimblephysics-stubs/exo/__init__.pyi +++ b/stubs/_nimblephysics-stubs/exo/__init__.pyi @@ -25,10 +25,18 @@ class ExoSolverPinnedContact(): """ This is part of the main exoskeleton solver. It takes in the current joint velocities and accelerations, and the last exoskeleton torques, and returns the estimated human pilot joint torques. """ + def estimateTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], contactForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: + """ + This is part of the main exoskeleton solver. It takes in the current joint velocities and accelerations, and returns the estimated human + exo system joint torques. + """ def finiteDifferenceContactJacobian(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: """ This is only used for testing: Get the Jacobian relating world space velocity of the contact points to joint velocities, by finite differencing. """ + def getClosestRealAccelerationConsistentWithPinsAndContactForces(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddq: numpy.ndarray[numpy.float64, _Shape[m, 1]], contactForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: + """ + Often our estimates for `dq` and `ddq` violate the pin constraints. That leads to exo torques that do not tend to zero as the virtual human exactly matches the real human+exo system. To solve this problem, we can solve a set of least-squares equations to find the best set of ddq values to satisfy the constraint. + """ def getContactJacobian(self) -> numpy.ndarray[numpy.float64, _Shape[m, n]]: """ Get the Jacobian relating world space velocity of the contact points to joint velocities. @@ -58,7 +66,7 @@ class ExoSolverPinnedContact(): """ This does the same thing as getPinndRealDynamics, but returns the Ax + b values A and b such that Ax + b = ddq, accounting for the pin constraints. """ - def getPinnedTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddqDesired: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> typing.Tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]]: + def getPinnedTotalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], ddqDesired: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringTau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringForces: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> typing.Tuple[numpy.ndarray[numpy.float64, _Shape[m, 1]], numpy.ndarray[numpy.float64, _Shape[m, 1]]]: """ This is part of the main exoskeleton solver. It takes in how the digital twin of the exo pilot is accelerating, and attempts to solve for the torques that the exo needs to apply to get as close to that as possible. """ @@ -95,7 +103,7 @@ class ExoSolverPinnedContact(): """ This runs the entire exoskeleton solver pipeline, spitting out the torques to apply to the exoskeleton actuators. """ - def solveFromBiologicalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], tau: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: + def solveFromBiologicalTorques(self, dq: numpy.ndarray[numpy.float64, _Shape[m, 1]], tau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringTau: numpy.ndarray[numpy.float64, _Shape[m, 1]], centeringForce: numpy.ndarray[numpy.float64, _Shape[m, 1]]) -> numpy.ndarray[numpy.float64, _Shape[m, 1]]: """ This is a subset of the steps in solveFromAccelerations, which can take the biological joint torques directly, and solve for the exo torques. """ diff --git a/unittests/unit/test_ExoSolverPinnedContact.cpp b/unittests/unit/test_ExoSolverPinnedContact.cpp index d845cdddb..d5403b024 100644 --- a/unittests/unit/test_ExoSolverPinnedContact.cpp +++ b/unittests/unit/test_ExoSolverPinnedContact.cpp @@ -292,7 +292,11 @@ TEST(EXO_SOLVER_PINNED_CONTACT, TEST_PINNED_INVERSE_DYNAMICS) Eigen::VectorXs contactForces = jointAccelerationsAndContactForces.second; std::pair jointTorquesAndContactForces - = solver.getPinnedTotalTorques(dq, ddq); + = solver.getPinnedTotalTorques( + dq, + ddq, + Eigen::VectorXs::Zero(dq.size()), + Eigen::VectorXs::Zero(pins.size() * 3)); std::pair jointTorquesLinearMap = solver.getPinnedTotalTorquesLinearMap(dq); Eigen::VectorXs recoveredTau = jointTorquesAndContactForces.first; @@ -364,7 +368,11 @@ TEST(EXO_SOLVER_PINNED_CONTACT, TEST_EXO_TORQUES_LINEAR_MAP) Eigen::VectorXs tau = Eigen::VectorXs::Random(realSkel->getNumDofs()); tau.head<6>().setZero(); - Eigen::VectorXs exoTorques = solver.solveFromBiologicalTorques(dq, tau); + Eigen::VectorXs exoTorques = solver.solveFromBiologicalTorques( + dq, + tau, + Eigen::VectorXs::Zero(dq.size()), + Eigen::VectorXs::Zero(pins.size() * 3)); std::pair exoTorquesLinearMap = solver.getExoTorquesLinearMap(dq); @@ -528,4 +536,89 @@ TEST(EXO_SOLVER_PINNED_CONTACT, TEST_RECONSTRUCT_EXO_DYNAMICS) } } } +#endif + +#ifdef ALL_TESTS +TEST(EXO_SOLVER_PINNED_CONTACT, ADJUST_TO_RESIDUALS_AND_CLAMPS) +{ + OpenSimFile file = OpenSimParser::parseOsim( + "dart://sample/osim/Rajagopal2015/Rajagopal2015.osim"); + std::shared_ptr realSkel = file.skeleton; + std::shared_ptr virtualSkel = realSkel->cloneSkeleton(); + + ExoSolverPinnedContact solver = ExoSolverPinnedContact(realSkel, virtualSkel); + + // Add contact points + std::vector> pins; + pins.emplace_back( + realSkel->getBodyNode("calcn_r")->getIndexInSkeleton(), + Eigen::Vector3s(0.0, 0, 0.0)); + pins.emplace_back( + realSkel->getBodyNode("calcn_l")->getIndexInSkeleton(), + Eigen::Vector3s(0.0, 0, 0.0)); + solver.setContactPins(pins); + + // Add a knee exo + solver.addMotorDof(realSkel->getDof("knee_angle_r")->getIndexInSkeleton()); + solver.addMotorDof(realSkel->getDof("knee_angle_l")->getIndexInSkeleton()); + + for (int i = 0; i < 10; i++) + { + Eigen::VectorXs q = realSkel->getRandomPose(); + solver.setPositions(q); + + Eigen::VectorXs dq = realSkel->getRandomVelocity(); + Eigen::VectorXs ddq = realSkel->getRandomVelocity(); + Eigen::MatrixXs J = solver.getContactJacobian(); + Eigen::VectorXs contactForces = Eigen::VectorXs::Random(J.rows()); + Eigen::VectorXs compliantDdq + = solver.getClosestRealAccelerationConsistentWithPinsAndContactForces( + dq, ddq, contactForces); + + Eigen::VectorXs recoveredConstraints = J * compliantDdq; + if (recoveredConstraints.norm() > 1e-6) + { + std::cout << "recoveredConstraints: " << recoveredConstraints.transpose() + << std::endl; + EXPECT_TRUE(recoveredConstraints.norm() < 1e-6); + return; + } + + Eigen::VectorXs tau = solver.estimateHumanTorques( + dq, compliantDdq, contactForces, Eigen::VectorXs::Zero(2)); + Eigen::Vector6s residual = tau.head<6>(); + + if (residual.norm() > 1e-6) + { + std::cout << "residual: " << residual.transpose() << std::endl; + EXPECT_TRUE(residual.norm() < 1e-6); + return; + } + + Eigen::VectorXs recoveredDdq + = solver.getPinnedVirtualDynamics(dq, tau).first; + if (!equals(compliantDdq, recoveredDdq, 1e-6)) + { + Eigen::MatrixXs compare = Eigen::MatrixXs::Zero(compliantDdq.size(), 3); + compare.col(0) = compliantDdq; + compare.col(1) = recoveredDdq; + compare.col(2) = compliantDdq - recoveredDdq; + std::cout << "ddq - recoveredDdq - diff: " << std::endl + << compare << std::endl; + EXPECT_TRUE(equals(compliantDdq, recoveredDdq, 1e-6)); + return; + } + + Eigen::VectorXs totalRealTorques + = solver.getPinnedTotalTorques(dq, recoveredDdq, tau, contactForces) + .first; + Eigen::VectorXs netTorques = totalRealTorques - tau; + if (netTorques.norm() > 1e-6) + { + std::cout << "netTorques: " << netTorques.transpose() << std::endl; + EXPECT_TRUE(netTorques.norm() < 1e-6); + return; + } + } +} #endif \ No newline at end of file