From 96a4a559a0bb016de5e95585e65c53441e2350b3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 9 Jun 2021 12:35:29 -0500 Subject: [PATCH] Fix FreeJoint velocity and position integration for articulated bodies (#28) #10 attempted to fix an integration issue when the COM offset from the link origin. However, this seems to work only for single body systems. With an articulated body, the fix in #10 actually caused errors that lead to fictitious forced being applied. This in essence reverts #10 while keeping the necessary changes for updateConstrainedTerms and adding more tests. Signed-off-by: Addisu Z. Taddese --- dart/dynamics/FreeJoint.cpp | 87 +++---- data/sdf/test/issue1193_revolute_test.sdf | 62 +++++ .../issue1193_revolute_with_offset_test.sdf | 62 +++++ unittests/comprehensive/test_Dynamics.cpp | 20 +- unittests/regression/CMakeLists.txt | 1 + unittests/regression/test_Issue1193.cpp | 242 ++++++++++++++++++ 6 files changed, 411 insertions(+), 63 deletions(-) create mode 100644 data/sdf/test/issue1193_revolute_test.sdf create mode 100644 data/sdf/test/issue1193_revolute_with_offset_test.sdf diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index d581ccedf8404..90d0e3178dfd1 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -589,77 +589,54 @@ bool FreeJoint::isCyclic(std::size_t _index) const //============================================================================== void FreeJoint::integratePositions(double _dt) { - const Eigen::Vector6d& vel = getVelocitiesStatic(); - const Eigen::Vector6d& accel = getAccelerationsStatic(); - auto* bn = getChildBodyNode(); - Eigen::Isometry3d Tcom; - // Transform from joint to center of mass of child body - Tcom = Eigen::Translation3d( - Joint::mAspectProperties.mT_ChildBodyToJoint.inverse() - * bn->getLocalCOM()); - auto velAtCom = math::AdT(Tcom.inverse(), vel); - auto accelAtCom = math::AdT(Tcom.inverse(), accel); - - const Eigen::Isometry3d Qcom = getQ() * Tcom; - const Eigen::Isometry3d Qdiff = convertToTransform(velAtCom * _dt); - - const Eigen::Vector6d newVelAtCom = math::AdR(Qdiff.inverse(), velAtCom); - const Eigen::Vector6d newAccelAtCom = math::AdR(Qdiff.inverse(), accelAtCom); - - const Eigen::Isometry3d QnextAtCom = Qcom * Qdiff; - const Eigen::Isometry3d Qnext = QnextAtCom * Tcom.inverse(); - - setVelocitiesStatic(math::AdT(Tcom, newVelAtCom)); - setAccelerationsStatic(math::AdT(Tcom, newAccelAtCom)); + const Eigen::Isometry3d Qdiff + = convertToTransform(getVelocitiesStatic() * _dt); + const Eigen::Isometry3d Qnext = getQ() * Qdiff; + + setVelocitiesStatic(math::AdR(Qdiff.inverse(), getVelocitiesStatic())); + setAccelerationsStatic(math::AdR(Qdiff.inverse(), getAccelerationsStatic())); setPositionsStatic(convertToPositions(Qnext)); } //============================================================================== void FreeJoint::integrateVelocities(double _dt) { - const Eigen::Vector6d& vel = getVelocitiesStatic(); - // Acceleration with additional term to take into account changing linear - // velocity in the inertial frame. - Eigen::Vector6d accelWithInertialTerm = getAccelerationsStatic(); - - // Not sure if we need a different inertia than the one from the child body - // node. - const Eigen::Matrix6d& mI = getChildBodyNode()->getSpatialInertia(); - const Eigen::Matrix6d& artInvProjI = getInvProjArtInertia(); - // Remove Coriolis term because the velocity will be updated when integrating - // position and that will account for the rotation of the frame. - accelWithInertialTerm.tail<3>() - -= (artInvProjI * math::dad(vel, mI * vel)).tail<3>(); - + // 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(), accelWithInertialTerm, _dt)); - // vel now points to the updated velocity - accelWithInertialTerm.tail<3>() - += (artInvProjI * math::dad(vel, mI * vel)).tail<3>(); - setAccelerationsStatic(accelWithInertialTerm); + 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& vel = getVelocitiesStatic(); - Eigen::Vector6d accelWithInertialTerm = getAccelerationsStatic(); - const Eigen::Matrix6d& mI = getChildBodyNode()->getSpatialInertia(); - const Eigen::Matrix6d& artInvI = getInvProjArtInertia(); - // Remove Coriolis term because the velocity will be updated when integrating - // position and that will account for the rotation of the frame. - accelWithInertialTerm.tail<3>() - -= (artInvI * math::dad(vel, mI * vel)).tail<3>(); + const Eigen::Vector6d &velBefore = getVelocitiesStatic(); + Eigen::Vector6d accel = getAccelerationsStatic(); + accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges); - // vel now points to the updated velocity - accelWithInertialTerm.tail<3>() - += (artInvI * math::dad(vel, mI * vel)).tail<3>(); - setAccelerationsStatic( - accelWithInertialTerm + mVelocityChanges * invTimeStep); - + 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 } diff --git a/data/sdf/test/issue1193_revolute_test.sdf b/data/sdf/test/issue1193_revolute_test.sdf new file mode 100644 index 0000000000000..a6cc24d012fdb --- /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 0000000000000..13f813dc32094 --- /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 b87d0ff0b6878..f458130dc0edf 100644 --- a/unittests/comprehensive/test_Dynamics.cpp +++ b/unittests/comprehensive/test_Dynamics.cpp @@ -2531,7 +2531,9 @@ TEST_F(DynamicsTest, OffsetCOM) box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass())); inertia.setLocalCOM({0, 500, 0}); box->setInertia(inertia); - box->addExtTorque({0, 0, 100}, false); + Eigen::Isometry3d boxInitialPose = box->getWorldTransform(); + Eigen::Vector3d torque(0, 0, 100); + box->addExtTorque(torque, false); { Vector3d comVel = box->getCOMLinearVelocity(); @@ -2548,28 +2550,30 @@ TEST_F(DynamicsTest, OffsetCOM) // Velocity at COM should be zero since the object is just rotating. { Vector3d comVel = box->getCOMLinearVelocity(); - EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel)) + EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4)) << "comVel: " << comVel.transpose(); } { Vector3d angVel = box->getAngularVelocity(); - EXPECT_TRUE(equals(Vector3d(0, 0, 0.01), angVel)) + // 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(box->getWorldTransform().linear() * -box->getLocalCOM()); + = angVel.cross(boxInitialPose.linear() * -box->getLocalCOM()); EXPECT_TRUE(equals(expLinVel, linVel)) << "Expected: " << expLinVel.transpose() << "\nActual: " << linVel.transpose(); Vector3d angAccel = box->getAngularAcceleration(); - EXPECT_TRUE(equals(Vector3d(0, 0, 10), angAccel)) + EXPECT_TRUE(equals(expectedAngAccel, angAccel)) << "angAccel: " << angAccel.transpose(); Vector3d linAccel = box->getLinearAcceleration(); Vector3d expLinAccel - = angVel.cross(linVel) - + angAccel.cross( - box->getWorldTransform().linear() * -box->getLocalCOM()); + = 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 e58521f7742e4..5c639510e7c94 100644 --- a/unittests/regression/CMakeLists.txt +++ b/unittests/regression/CMakeLists.txt @@ -35,3 +35,4 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode) endif() dart_add_test("regression" test_Issue1193) +target_link_libraries(test_Issue1193 dart-utils) diff --git a/unittests/regression/test_Issue1193.cpp b/unittests/regression/test_Issue1193.cpp index 45f550850c5b5..979f5766506bb 100644 --- a/unittests/regression/test_Issue1193.cpp +++ b/unittests/regression/test_Issue1193.cpp @@ -35,10 +35,13 @@ #include "TestHelpers.hpp" +#include "dart/utils/sdf/SdfParser.hpp" + using namespace dart::math; using namespace dart::collision; using namespace dart::dynamics; using namespace dart::simulation; +using namespace dart::utils; //============================================================================== TEST(Issue1193, AngularVelAdd) @@ -75,3 +78,242 @@ TEST(Issue1193, AngularVelAdd) 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}); + skel->disableSelfCollisionCheck(); + 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}); + skel->disableSelfCollisionCheck(); + 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}); + skel->disableSelfCollisionCheck(); + 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}); + rootBn->addExtForce({10, 0, 0}); + world->step(); + auto comLinearVel = rootBn->getCOMLinearVelocity(); + // TODO (addisu) Improve FreeJoint integration so that a higher tolerance is + // not necessary here. + EXPECT_NEAR(0.01, 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}); + skel->disableSelfCollisionCheck(); + 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 (addisu) 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); +}