Description: Fix velocity and position integration for FreeJoint Critical fix for the simulation of wheeled vehicles in robotics. All the information in: https://github.com/dartsim/dart/issues/1433 Origin: https://github.com/dartsim/dart/pull/1437 Forwarded: https://github.com/dartsim/dart/pull/1437 Applied-Upstream: https://github.com/dartsim/dart/pull/1437 Last-Update: 2021-12-20 --- This patch header follows DEP-3: http://dep.debian.net/deps/dep3/ diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 4e2bc4fa15f2..a063cf3a2ac6 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -589,12 +589,59 @@ bool FreeJoint::isCyclic(std::size_t _index) const //============================================================================== void FreeJoint::integratePositions(double _dt) { - const Eigen::Isometry3d Qnext - = getQ() * convertToTransform(getVelocitiesStatic() * _dt); + const Eigen::Isometry3d Qdiff + = convertToTransform(getVelocitiesStatic() * _dt); + const Eigen::Isometry3d Qnext = getQ() * Qdiff; + const Eigen::Isometry3d QdiffInv = Qdiff.inverse(); + setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic())); + setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic())); setPositionsStatic(convertToPositions(Qnext)); } +//============================================================================== +void FreeJoint::integrateVelocities(double _dt) +{ + // Integrating the acceleration gives us the new velocity of child body frame. + // But if there is any linear acceleration, the frame will be displaced. If we + // apply euler integration direcly on the spatial acceleration, it will + // produce the velocity of a point that is instantaneously coincident with the + // previous location of the child body frame. However, we want to compute the + // spatial velocity at the current location of the child body frame. To + // accomplish this, we first convert the linear portion of the spatial + // acceleration into classical linear acceleration and apply the integration. + Eigen::Vector6d accel = getAccelerationsStatic(); + const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); + setVelocitiesStatic(math::integrateVelocity( + getVelocitiesStatic(), accel, _dt)); + + // Since the velocity has been updated, we use the new velocity to recompute + // the spatial acceleration. This is needed to ensure that functions like + // BodyNode::getLinearAcceleration work properly. + const Eigen::Vector6d& velAfter = getVelocitiesStatic(); + accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>()); + setAccelerationsStatic(accel); +} + +//============================================================================== +void FreeJoint::updateConstrainedTerms(double timeStep) +{ + const double invTimeStep = 1.0 / timeStep; + + const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + Eigen::Vector6d accel = getAccelerationsStatic(); + accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); + + setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges); + + const Eigen::Vector6d& velAfter = getVelocitiesStatic(); + accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>()); + setAccelerationsStatic(accel + mVelocityChanges * invTimeStep); + this->mAspectState.mForces.noalias() += mImpulses * invTimeStep; + // Note: As long as this is only called from BodyNode::updateConstrainedTerms +} + //============================================================================== void FreeJoint::updateDegreeOfFreedomNames() { diff --git a/dart/dynamics/FreeJoint.hpp b/dart/dynamics/FreeJoint.hpp index 4432728c303a..22c463063a05 100644 --- a/dart/dynamics/FreeJoint.hpp +++ b/dart/dynamics/FreeJoint.hpp @@ -330,6 +330,12 @@ class FreeJoint : public GenericJoint // Documentation inherited void integratePositions(double _dt) override; + // Documentation inherited + void integrateVelocities(double _dt) override; + + // Documentation inherited + void updateConstrainedTerms(double timeStep) override; + // Documentation inherited void updateDegreeOfFreedomNames() override; diff --git a/data/sdf/test/issue1193_revolute_test.sdf b/data/sdf/test/issue1193_revolute_test.sdf new file mode 100644 index 000000000000..a6cc24d012fd --- /dev/null +++ b/data/sdf/test/issue1193_revolute_test.sdf @@ -0,0 +1,62 @@ + + + 0 0 0 + + 0.0 0.0 0 + 1000.000000 + 0.001000 + + + 1 1 0 0 0 0 + + 0 0 2 0 0 0 + + 1.0 + + 1.0 + 1.0 + 1.0 + 0.0 + 0.0 + 0.0 + + + + + + 1 1 1 + + + + + 0 0 -2 0 0 0 + 0.1 + + + + link1 + link2 + + 1 0 0 + + + + 0 0 -2 0 0 0 + + 1.0 + + 1.0 + 1.0 + 1.0 + 0.0 + 0.0 + 0.0 + + + + 1 1 1 + + + + + diff --git a/data/sdf/test/issue1193_revolute_with_offset_test.sdf b/data/sdf/test/issue1193_revolute_with_offset_test.sdf new file mode 100644 index 000000000000..13f813dc3209 --- /dev/null +++ b/data/sdf/test/issue1193_revolute_with_offset_test.sdf @@ -0,0 +1,62 @@ + + + 0 0 0 + + 0.0 0.0 0 + 1000.000000 + 0.001000 + + + + 0 0 2 0 0 0 + + 1.0 + + 1.0 + 1.0 + 1.0 + 0.0 + 0.0 + 0.0 + + + + + + 1 1 1 + + + + + 0 0 -2 0 0 0 + 0.1 + + + + 0 0 2 0 0 0 + link1 + link2 + + 1 0 0 + + + + 0 0 -2 0 0 0 + + 1.0 + + 1.0 + 1.0 + 1.0 + 0.0 + 0.0 + 0.0 + + + + 1 1 1 + + + + + diff --git a/unittests/comprehensive/test_Dynamics.cpp b/unittests/comprehensive/test_Dynamics.cpp index 12baf6661718..f89f3fe82f64 100644 --- a/unittests/comprehensive/test_Dynamics.cpp +++ b/unittests/comprehensive/test_Dynamics.cpp @@ -1117,31 +1117,19 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates( = skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep; VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep; - EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE)); - EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE)); - EXPECT_TRUE(equals(ddq0, ddqFD1, TOLERANCE)); - EXPECT_TRUE(equals(ddq0, ddqFD2, TOLERANCE)); + EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE)) + << "dq0 : " << dq0.transpose() << "\ndq0FD: " << dq0FD.transpose(); - if (!equals(dq0FD, dq0, TOLERANCE)) - { - std::cout << "dq0 : " << dq0.transpose() << std::endl; - std::cout << "dq0FD: " << dq0FD.transpose() << std::endl; - } - if (!equals(dq1, dq1FD, TOLERANCE)) - { - std::cout << "dq1 : " << dq1.transpose() << std::endl; - std::cout << "dq1FD: " << dq1FD.transpose() << std::endl; - } - if (!equals(ddq0, ddqFD1, TOLERANCE)) - { - std::cout << "ddq0 : " << ddq0.transpose() << std::endl; - std::cout << "ddqFD1: " << ddqFD1.transpose() << std::endl; - } - if (!equals(ddq0, ddqFD2, TOLERANCE)) - { - std::cout << "ddq0 : " << ddq0.transpose() << std::endl; - std::cout << "ddqFD2: " << ddqFD2.transpose() << std::endl; - } + EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE)) + << "dq1 : " << dq1.transpose() << "\ndq1FD: " << dq1FD.transpose(); + + EXPECT_TRUE(equals(ddq0, ddqFD1, 10 * TOLERANCE)) + << "ddq0 : " << ddq0.transpose() + << "\nddqFD1: " << ddqFD1.transpose(); + + EXPECT_TRUE(equals(ddq0, ddqFD2, 10 * TOLERANCE)) + << "ddq0 : " << ddq0.transpose() + << "\nddqFD2: " << ddqFD2.transpose() << std::endl; } } } @@ -2605,3 +2593,70 @@ TEST_F(DynamicsTest, HybridDynamics) EXPECT_NEAR(command(i, 4), output(i, 4), tol); } } + +//============================================================================== +TEST_F(DynamicsTest, OffsetCOM) +{ + WorldPtr world = World::create(); + ASSERT_TRUE(world != nullptr); + world->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); + const double dt = 0.001; + world->setTimeStep(dt); + + SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0)); + world->addSkeleton(boxSkel); + BodyNode* box = boxSkel->getBodyNode(0); + dynamics::Inertia inertia; + inertia.setMass(60); + inertia.setMoment( + box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass())); + inertia.setLocalCOM({0, 500, 0}); + box->setInertia(inertia); + Eigen::Isometry3d boxInitialPose = box->getWorldTransform(); + Eigen::Vector3d torque(0, 0, 100); + box->addExtTorque(torque, false); + + { + Vector3d comVel = box->getCOMLinearVelocity(); + EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel)) + << "comVel: " << comVel.transpose(); + } + { + Vector3d linVel = box->getLinearVelocity(); + EXPECT_TRUE(equals(Vector3d(0, 0, 0), linVel)) + << "linVel: " << linVel.transpose(); + } + + world->step(); + // Velocity at COM should be zero since the object is just rotating. + { + Vector3d comVel = box->getCOMLinearVelocity(); + EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4)) + << "comVel: " << comVel.transpose(); + } + { + Vector3d angVel = box->getAngularVelocity(); + // We can compute expectedAngAccel like so because the initial angular + // velocity is zero. + Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque; + Vector3d expectedAngVel = expectedAngAccel * dt; + EXPECT_TRUE(equals(expectedAngVel, angVel)) + << "angVel: " << angVel.transpose(); + Vector3d linVel = box->getLinearVelocity(); + Vector3d expLinVel + = angVel.cross(boxInitialPose.linear() * -box->getLocalCOM()); + EXPECT_TRUE(equals(expLinVel, linVel)) + << "Expected: " << expLinVel.transpose() + << "\nActual: " << linVel.transpose(); + + Vector3d angAccel = box->getAngularAcceleration(); + EXPECT_TRUE(equals(expectedAngAccel, angAccel)) + << "angAccel: " << angAccel.transpose(); + Vector3d linAccel = box->getLinearAcceleration(); + Vector3d expLinAccel + = expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM()); + EXPECT_TRUE(equals(expLinAccel, linAccel)) + << "Expected: " << expLinAccel.transpose() + << "\nActual: " << linAccel.transpose(); + } +} diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt index ace85bafc91c..bcbc62e12113 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -43,3 +43,8 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode) dart-utils) endif() + +if(TARGET dart-utils) + dart_add_test("regression" test_Issue1193) + target_link_libraries(test_Issue1193 dart-utils) +endif() diff --git a/unittests/regression/test_Issue1193.cpp b/unittests/regression/test_Issue1193.cpp new file mode 100644 index 000000000000..42e7f33d5d4c --- /dev/null +++ b/unittests/regression/test_Issue1193.cpp @@ -0,0 +1,321 @@ +/* + * Copyright (c) 2011-2021, 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 "dart/dart.hpp" +#include "dart/utils/sdf/SdfParser.hpp" + +#include "TestHelpers.hpp" + +using namespace dart::math; +using namespace dart::collision; +using namespace dart::dynamics; +using namespace dart::simulation; +using namespace dart::utils; + +//============================================================================== +TEST(Issue1193, AngularVelAdd) +{ + WorldPtr world = World::create(); + EXPECT_TRUE(world != nullptr); + world->setGravity(Eigen::Vector3d(0.0, -10.0, 0.0)); + const double dt = 0.001; + world->setTimeStep(dt); + + SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0)); + BodyNode* sphere = sphereSkel->getBodyNode(0); + Joint* sphereJoint = sphere->getParentJoint(); + sphereJoint->setVelocity(0, 10.0); // ang_x -> Affect lz and ly + sphereJoint->setVelocity(1, 10.0); // ang_y -> No effect + sphereJoint->setVelocity(2, 10.0); // ang_z -> Affect lx and ly + world->addSkeleton(sphereSkel); + + Eigen::Vector3d linearVelBefore = sphere->getLinearVelocity(); + EXPECT_EQ(Vector3d::Zero(), linearVelBefore); + + int maxSteps = 500; + for (int i = 0; i < maxSteps; i++) + { + world->step(); + } + + Vector3d linearVelAfter = sphere->getLinearVelocity(); + double lx = linearVelAfter[0]; + double ly = linearVelAfter[1]; + double lz = linearVelAfter[2]; + + EXPECT_NEAR(0.0, lx, 1e-8); + EXPECT_NEAR(maxSteps * world->getGravity().y() * dt, ly, 1e-8); + EXPECT_NEAR(0.0, lz, 1e-8); +} + +const double tol = 1e-5; +const int g_iters = 100000; + +Eigen::Vector3d computeWorldAngularMomentum(const SkeletonPtr skel) +{ + Eigen::Vector3d angMomentum = Eigen::Vector3d::Zero(); + for (auto* bn : skel->getBodyNodes()) + { + angMomentum += dart::math::dAdInvT( + bn->getWorldTransform(), + bn->getSpatialInertia() * bn->getSpatialVelocity()) + .head<3>(); + } + return angMomentum; +} + +//============================================================================== +TEST(Issue1193, SingleBody) +{ + WorldPtr world = World::create(); + const double dt = 0.001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + SkeletonPtr skel = createBox({1, 1, 1}); + world->addSkeleton(skel); + auto rootBn = skel->getRootBodyNode(); + + Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation(); + Vector6d vels = compose({0, 25, 0}, {0, 0, -100}); + + rootBn->getParentJoint()->setVelocities(vels); + + rootBn->addExtTorque({0, 50, 0}); + for (int i = 0; i < g_iters; ++i) + { + world->step(); + } + Eigen::Vector3d positionDiff + = rootBn->getWorldTransform().translation() - initPosition; + EXPECT_NEAR(0.0, positionDiff.x(), tol); + EXPECT_NEAR(0.0, positionDiff.y(), tol); + EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol); +} + +//============================================================================== +TEST(Issue1193, SingleBodyWithOffDiagonalMoi) +{ + WorldPtr world = World::create(); + const double dt = 0.001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + SkeletonPtr skel = createBox({1, 4, 9}); + world->addSkeleton(skel); + auto rootBn = skel->getRootBodyNode(); + rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0); + Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation(); + + // Setting this to compose({0, 25, 0}, {0, 0, -100}) causes the test to fail. + Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10.0}); + + rootBn->getParentJoint()->setVelocities(vels); + + rootBn->addExtTorque({0, 50, 0}); + // + for (int i = 0; i < g_iters; ++i) + { + world->step(); + } + Eigen::Vector3d positionDiff + = rootBn->getWorldTransform().translation() - initPosition; + EXPECT_NEAR(0.0, positionDiff.x(), tol); + EXPECT_NEAR(0.0, positionDiff.y(), tol); + EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol); +} + +//============================================================================== +TEST(Issue1193, SingleBodyWithJointOffset) +{ + WorldPtr world = World::create(); + const double dt = 0.001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + SkeletonPtr skel = createBox({1, 1, 1}); + world->addSkeleton(skel); + auto rootBn = skel->getRootBodyNode(); + rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0); + Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation(); + + Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10}); + + auto* freeJoint = dynamic_cast(rootBn->getParentJoint()); + freeJoint->setVelocities(vels); + + Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity(); + jointPoseInParent.translate(Eigen::Vector3d(0.0, 4.0, 0)); + freeJoint->setTransformFromParentBodyNode(jointPoseInParent); + + rootBn->addExtTorque({0, 50, 0}); + for (int i = 0; i < g_iters; ++i) + { + world->step(); + } + + Eigen::Vector3d positionDiff + = rootBn->getWorldTransform().translation() - initPosition; + EXPECT_NEAR(0.0, positionDiff.x(), tol); + EXPECT_NEAR(4.0, positionDiff.y(), tol); + EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol); +} + +//============================================================================== +TEST(Issue1193, SingleBodyWithCOMOffset) +{ + WorldPtr world = World::create(); + const double dt = 0.001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + SkeletonPtr skel = createBox({1, 1, 1}); + world->addSkeleton(skel); + auto rootBn = skel->getRootBodyNode(); + rootBn->setMomentOfInertia(1.0 / 6, 1.0 / 6.0, 1.0 / 6.0, 0, 0, 0); + rootBn->setLocalCOM({1, 5, 8}); + const Eigen::Vector3d extForce{10, 0, 0}; + rootBn->addExtForce(extForce); + world->step(); + auto comLinearVel = rootBn->getCOMLinearVelocity(); + // TODO (azeey) Improve FreeJoint integration so that a higher tolerance is + // not necessary here. + EXPECT_NEAR(extForce.x() * dt, comLinearVel.x(), tol * 1e2); +} + +//============================================================================== +TEST(Issue1193, WithFixedJoint) +{ + WorldPtr world = World::create(); + const double dt = 0.001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + SkeletonPtr skel = createBox({1, 1, 1}, {0, 0, 2}); + world->addSkeleton(skel); + auto rootBn = skel->getRootBodyNode(); + + Eigen::Isometry3d comRelPose; + comRelPose = Eigen::Translation3d(0, 0, -2); + auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose); + Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation(); + + GenericJoint::Properties joint2Prop(std::string("joint2")); + BodyNode::Properties link2Prop( + BodyNode::AspectProperties(std::string("link2"))); + link2Prop.mInertia.setMass(1.0); + + auto pair = rootBn->createChildJointAndBodyNodePair( + WeldJoint::Properties(joint2Prop), link2Prop); + auto* joint = pair.first; + + Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity(); + jointPoseInParent.translate(Eigen::Vector3d(0.0, 0.0, -4)); + joint->setTransformFromParentBodyNode(jointPoseInParent); + + // TODO (azeey) Improve FreeJoint integration so we can test with larger + // forces. Currently, increasing these forces much more causes the test to + // fail. + rootBn->setExtTorque({0, 2500, 0}); + rootBn->setExtForce({0, 0, -1000}); + + for (int i = 0; i < g_iters; ++i) + { + world->step(); + } + Eigen::Vector3d positionDiff + = comFrame->getWorldTransform().translation() - initComPosition; + EXPECT_NEAR(0.0, positionDiff.x(), tol); + EXPECT_NEAR(0.0, positionDiff.y(), tol); +} + +//============================================================================== +TEST(Issue1193, WithRevoluteJoint) +{ + auto world = SdfParser::readWorld( + "dart://sample/sdf/test/issue1193_revolute_test.sdf"); + ASSERT_TRUE(world != nullptr); + const double dt = 0.001; + world->setTimeStep(dt); + + SkeletonPtr skel = world->getSkeleton(0); + auto rootBn = skel->getRootBodyNode(); + + Eigen::Isometry3d comRelPose; + comRelPose = Eigen::Translation3d(0, 0, -2); + auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose); + Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation(); + + auto* joint = skel->getJoint("revJoint"); + ASSERT_NE(nullptr, joint); + + for (int i = 0; i < g_iters; ++i) + { + joint->setCommand(0, 0.1); + world->step(); + } + + Eigen::Vector3d positionDiff + = comFrame->getWorldTransform().translation() - initComPosition; + EXPECT_NEAR(0.0, positionDiff.x(), tol * 5e2); + EXPECT_NEAR(0.0, positionDiff.y(), tol * 5e2); +} + +//============================================================================== +TEST(Issue1193, ConservationOfMomentumWithRevoluteJointWithOffset) +{ + auto world = SdfParser::readWorld( + "dart://sample/sdf/test/issue1193_revolute_with_offset_test.sdf"); + ASSERT_TRUE(world != nullptr); + const double dt = 0.0001; + world->setTimeStep(dt); + world->setGravity(Vector3d::Zero()); + + SkeletonPtr skel = world->getSkeleton(0); + auto link1 = skel->getBodyNode("link1"); + + auto* joint = skel->getJoint("revJoint"); + ASSERT_NE(nullptr, joint); + + link1->getParentJoint()->setVelocities(compose({0, 0.25, 0}, {0, 0, -0.1})); + world->step(); + Eigen::Vector3d maxAngMomentumChange = Eigen::Vector3d::Zero(); + Eigen::Vector3d h0 = computeWorldAngularMomentum(skel); + for (int i = 1; i < g_iters; ++i) + { + joint->setCommand(0, 0.1); + world->step(); + + Eigen::Vector3d hNext = computeWorldAngularMomentum(skel); + maxAngMomentumChange + = (hNext - h0).cwiseAbs().cwiseMax(maxAngMomentumChange); + } + + EXPECT_NEAR(0.0, maxAngMomentumChange.norm(), tol * 10); +}