From 31ff69b8990c4846972144f5b578d8ce5d6a1ca1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 4 Jun 2022 22:52:22 -0700 Subject: [PATCH 1/8] dartsim: add data to LinkInfo needed for loops In gazebo classic, kinematic loops are closed by identifying a link with multiple parents and creating a mirror body node that is welded to the original. The inertia will be split evenly between the body nodes. This change adds data to store the welded mirror nodes, the weld constraints, and the original inertial data. Signed-off-by: Steve Peters --- dartsim/src/Base.hh | 18 +++++++++++++++++- dartsim/src/SDFFeatures.cc | 2 +- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 4d9248f91..1368d0e94 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include +#include #include #include @@ -57,6 +59,16 @@ struct LinkInfo /// moving the BodyNode to a new skeleton), so we store the Gazebo-specified /// name of the Link here. std::string name; + /// \brief To close kinematic loops, a body node may be divided into separate + /// nodes that are welded together using a WeldJointConstraint. The inertia + /// is divided evenly between these body nodes. This matches the approach + /// used in gazebo-classic. + std::vector< std::pair< + dart::dynamics::BodyNode *, + dart::constraint::WeldJointConstraintPtr> > weldedNodes; + /// \brief The total link inertia, which may be split between the `link` and + /// `weldedNodes` body nodes. + std::optional inertial; }; struct ModelInfo @@ -319,7 +331,8 @@ class Base : public Implements3d> } public: inline std::size_t AddLink(DartBodyNode *_bn, - const std::string &_fullName, std::size_t _modelID) + const std::string &_fullName, std::size_t _modelID, + std::optional _inertial = std::nullopt) { const std::size_t id = this->GetNextEntity(); auto linkInfo = std::make_shared(); @@ -328,6 +341,9 @@ class Base : public Implements3d> // The name of the BodyNode during creation is assumed to be the // Gazebo-specified name. linkInfo->name = _bn->getName(); + // Inertial properties (if available) used when splitting nodes to close + // kinematic loops. + linkInfo->inertial = _inertial; this->links.objectToID[_bn] = id; this->frames[id] = _bn; diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 0d5206187..a6ca00e30 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -640,7 +640,7 @@ Identity SDFFeatures::ConstructSdfLink( const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); - const std::size_t linkID = this->AddLink(bn, fullName, _modelID); + const std::size_t linkID = this->AddLink(bn, fullName, _modelID, sdfInertia); this->AddJoint(joint); auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID)); From 28773d7e1c31ce5adc908ad43703283604339bd5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Jun 2022 01:15:33 -0700 Subject: [PATCH 2/8] Split links and weld the pieces together To support closing kinematic loops. Signed-off-by: Steve Peters --- dartsim/src/Base.hh | 56 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 1368d0e94..a82817ec6 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -18,7 +18,10 @@ #ifndef IGNITION_PHYSICS_DARTSIM_BASE_HH_ #define IGNITION_PHYSICS_DARTSIM_BASE_HH_ +#include +#include #include +#include #include #include #include @@ -32,6 +35,7 @@ #include #include +#include #include #include @@ -363,6 +367,58 @@ class Base : public Implements3d> return id; } + public: inline DartBodyNode* SplitAndWeldLink(LinkInfo *_link) + { + // First create a new body node with FreeJoint and a unique name based + // on the number of welded miror nodes. + dart::dynamics::BodyNode::Properties weldedBodyProperties; + { + std::size_t weldedBodyCount = _link->weldedNodes.size(); + weldedBodyProperties.mName = + _link->name + "_welded_mirror_" + std::to_string(weldedBodyCount); + } + dart::dynamics::FreeJoint::Properties jointProperties; + jointProperties.mName = weldedBodyProperties.mName + "_FreeJoint"; + DartSkeletonPtr skeleton = _link->link->getSkeleton(); + auto pairJointBodyNode = + skeleton->createJointAndBodyNodePair( + nullptr, jointProperties, weldedBodyProperties); + + // Weld the new body node to the original + auto weld = std::make_shared( + _link->link, pairJointBodyNode.second); + _link->weldedNodes.emplace_back(pairJointBodyNode.second, weld); + auto worldId = this->GetWorldOfModelImpl(models.objectToID[skeleton]); + auto dartWorld = this->worlds.at(worldId); + dartWorld->getConstraintSolver()->addConstraint(weld); + + // Rebalance the link inertia between the original body node and its + // welded mirror nodes if inertial data is available. + if (_link->inertial) + { + std::size_t nodeCount = 1 + _link->weldedNodes.size(); + const double mass = _link->inertial->MassMatrix().Mass() / nodeCount; + const math::Matrix3d moi = _link->inertial->Moi() * (1 / nodeCount); + const math::Vector3d cog = _link->inertial->Pose().Pos(); + + _link->link->setMass(mass); + _link->link->setMomentOfInertia( + moi(0, 0), moi(1, 1), moi(2, 2), + moi(0, 1), moi(0, 2), moi(1, 2)); + _link->link->setLocalCOM(math::eigen3::convert(cog)); + for (auto weldedNodePair : _link->weldedNodes) + { + weldedNodePair.first->setMass(mass); + weldedNodePair.first->setMomentOfInertia( + moi(0, 0), moi(1, 1), moi(2, 2), + moi(0, 1), moi(0, 2), moi(1, 2)); + weldedNodePair.first->setLocalCOM(math::eigen3::convert(cog)); + } + } + + return pairJointBodyNode.second; + } + public: inline std::size_t AddJoint(DartJoint *_joint) { const std::size_t id = this->GetNextEntity(); From fdf7d5263f7f6ae55932a190d1a8e2a0eb656b4d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Jun 2022 01:40:45 -0700 Subject: [PATCH 3/8] AttachFixedJoint: handle kinematic loops If the child link already has a parent, split the link and weld the new node to the original node. Signed-off-by: Steve Peters --- dartsim/src/JointFeatures.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 8f5ec221d..34f7a4e64 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -455,7 +455,7 @@ Identity JointFeatures::AttachFixedJoint( const std::string &_name) { auto linkInfo = this->ReferenceInterface(_childID); - DartBodyNode *const bn = linkInfo->link.get(); + DartBodyNode *bn = linkInfo->link.get(); dart::dynamics::WeldJoint::Properties properties; properties.mName = _name; @@ -465,8 +465,8 @@ Identity JointFeatures::AttachFixedJoint( if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint - // TODO(scpeters): use a WeldJointConstraint between the two bodies - return this->GenerateInvalidId(); + // split and weld the child body node, and attach to the new welded node + bn = SplitAndWeldLink(linkInfo); } { From 393d02986d498231440a55e346f73f2229a56f8f Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 6 Jun 2022 10:00:09 +0800 Subject: [PATCH 4/8] Add unit tests Signed-off-by: Arjo Chakravarty --- dartsim/src/JointFeatures_TEST.cc | 136 ++++++++++++++++++++++++++++ dartsim/worlds/joint_constraint.sdf | 110 ++++++++++++++++++++++ 2 files changed, 246 insertions(+) create mode 100644 dartsim/worlds/joint_constraint.sdf diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 4894b4ef9..e44e5a16d 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -883,6 +883,142 @@ TEST_F(JointFeaturesFixture, JointAttachDetach) EXPECT_NEAR(0.0, dartBody2->getLinearVelocity().z(), 1e-3); } +//////////////////////////////////////////////// +// Essentially what happens is there are two floating boxes and a box in the +// middle that's resting. We start the system out by creating the two +// fixed joints between the boxes resting on the big box. The middle box will +// now have two parents. However there should be no movement as the middle box +// will be holding the other two boxes that are floating in mid air. We run +// this for 100 steps to make sure that there is no movement. This is because +// the middle box is holding on to the two side boxes. Then we release the +// joints the two boxes should fall away. +TEST_F(JointFeaturesFixture, JointAttachMultiple) +{ + sdf::Root root; + const sdf::Errors errors = + root.Load(TEST_WORLD_DIR "joint_constraint.sdf"); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = this->engine->ConstructWorld(*root.WorldByIndex(0)); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); + ASSERT_NE(nullptr, dartWorld); + + // M1 and M3 are floating boxes + const std::string modelName1{"M1"}; + const std::string modelName2{"M2"}; + const std::string modelName3{"M3"}; + const std::string bodyName{"link"}; + + auto model1 = world->GetModel(modelName1); + auto model2 = world->GetModel(modelName2); + auto model3 = world->GetModel(modelName3); + + auto model1Body = model1->GetLink(bodyName); + auto model2Body = model2->GetLink(bodyName); + auto model3Body = model3->GetLink(bodyName); + + const dart::dynamics::SkeletonPtr skeleton1 = + dartWorld->getSkeleton(modelName1); + const dart::dynamics::SkeletonPtr skeleton2 = + dartWorld->getSkeleton(modelName2); + const dart::dynamics::SkeletonPtr skeleton3 = + dartWorld->getSkeleton(modelName3); + ASSERT_NE(nullptr, skeleton1); + ASSERT_NE(nullptr, skeleton2); + ASSERT_NE(nullptr, skeleton3); + + auto *dartBody1 = skeleton1->getBodyNode(bodyName); + auto *dartBody2 = skeleton2->getBodyNode(bodyName); + auto *dartBody3 = skeleton3->getBodyNode(bodyName); + + ASSERT_NE(nullptr, dartBody1); + ASSERT_NE(nullptr, dartBody2); + ASSERT_NE(nullptr, dartBody3); + + const math::Pose3d initialModel1Pose(0, -0.2, 0.45, 0, 0, 0); + const math::Pose3d initialModel2Pose(0, 0.2, 0.45, 0, 0, 0); + const math::Pose3d initialModel3Pose(0, 0.6, 0.45, 0, 0, 0); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + physics::ForwardStep::Output output; + physics::ForwardStep::State state; + physics::ForwardStep::Input input; + + // Create the first joint. This should be a normal fixed joint. + const auto poseParent1 = dartBody1->getTransform(); + const auto poseChild1 = dartBody2->getTransform(); + auto poseParentChild1 = poseParent1.inverse() * poseChild1; + auto fixedJoint1 = model2Body->AttachFixedJoint(model1Body); + fixedJoint1->SetTransformFromParent(poseParentChild1); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + // Create the second joint. This should be a WeldJoint constraint + const auto poseParent2 = dartBody3->getTransform(); + const auto poseChild2 = dartBody2->getTransform(); + auto poseParentChild2 = poseParent2.inverse() * poseChild2; + auto fixedJoint2 = model2Body->AttachFixedJoint(model3Body); + fixedJoint2->SetTransformFromParent(poseParentChild2); + + EXPECT_EQ(initialModel1Pose, + math::eigen3::convert(dartBody1->getWorldTransform())); + EXPECT_EQ(initialModel2Pose, + math::eigen3::convert(dartBody2->getWorldTransform())); + EXPECT_EQ(initialModel3Pose, + math::eigen3::convert(dartBody3->getWorldTransform())); + + const std::size_t numSteps = 100; + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + // Expect all the bodies to be at rest. + // (since they're held in place by the joints) + math::Vector3d body1LinearVelocity = + math::eigen3::convert(dartBody1->getLinearVelocity()); + math::Vector3d body2LinearVelocity = + math::eigen3::convert(dartBody2->getLinearVelocity()); + math::Vector3d body3LinearVelocity = + math::eigen3::convert(dartBody3->getLinearVelocity()); + EXPECT_NEAR(0.0, body1LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + EXPECT_NEAR(0.0, body3LinearVelocity.Z(), 1e-7); + } + + // Detach the joints. M1 and M3 should fall as there is now nothing stopping + // them from falling. + fixedJoint1->Detach(); + fixedJoint2->Detach(); + + for (std::size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + + // Expect the middle box to be still as it is already at rest. + // Expect the two side boxes to fall away. + math::Vector3d body1LinearVelocity = + math::eigen3::convert(dartBody1->getLinearVelocity()); + math::Vector3d body2LinearVelocity = + math::eigen3::convert(dartBody2->getLinearVelocity()); + math::Vector3d body3LinearVelocity = + math::eigen3::convert(dartBody3->getLinearVelocity()); + EXPECT_GT(0, body1LinearVelocity.Z()); + EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); + EXPECT_GT(0, body3LinearVelocity.Z()); + } +} + ///////////////////////////////////////////////// // Expectations on number of links before/after attach/detach TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) diff --git a/dartsim/worlds/joint_constraint.sdf b/dartsim/worlds/joint_constraint.sdf new file mode 100644 index 000000000..dc7459ccb --- /dev/null +++ b/dartsim/worlds/joint_constraint.sdf @@ -0,0 +1,110 @@ + + + + + + 0 -0.2 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.2 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.6 0.45 0 0 0 + false + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0.2 0.2 0 0 0 + true + + + + + 0.4 0.4 0.4 + + + + + + + 0.4 0.4 0.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + \ No newline at end of file From 21a608d08188eaef02b4b9433c9c088d57be5f84 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 9 Jun 2022 15:17:41 -0700 Subject: [PATCH 5/8] Fix test by resetting constraint transform Signed-off-by: Steve Peters --- dartsim/src/Base.hh | 1 + dartsim/src/JointFeatures.cc | 10 +++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index a82817ec6..b882164a6 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -372,6 +372,7 @@ class Base : public Implements3d> // First create a new body node with FreeJoint and a unique name based // on the number of welded miror nodes. dart::dynamics::BodyNode::Properties weldedBodyProperties; + weldedBodyProperties.mIsCollidable = false; { std::size_t weldedBodyCount = _link->weldedNodes.size(); weldedBodyProperties.mName = diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 34f7a4e64..3bc806018 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -462,22 +462,30 @@ Identity JointFeatures::AttachFixedJoint( auto *const parentBn = _parent ? this->ReferenceInterface( _parent->FullIdentity())->link.get() : nullptr; + std::string childLinkName = linkInfo->name; if (bn->getParentJoint()->getType() != "FreeJoint") { // child already has a parent joint // split and weld the child body node, and attach to the new welded node bn = SplitAndWeldLink(linkInfo); + childLinkName = bn->getName(); } { auto skeleton = bn->getSkeleton(); if (skeleton) { - bn->setName(skeleton->getName() + '/' + linkInfo->name); + bn->setName(skeleton->getName() + '/' + childLinkName); } } const std::size_t jointID = this->AddJoint( bn->moveTo(parentBn, properties)); + if (linkInfo->weldedNodes.size() > 0) + { + // weld constraint needs to be updated after moving to new skeleton + auto constraint = linkInfo->weldedNodes.back().second; + constraint->setRelativeTransform(Eigen::Isometry3d::Identity()); + } // TODO(addisu) Remove incrementVersion once DART has been updated to // internally increment the BodyNode's version after moveTo. bn->incrementVersion(); From fb11680f89dad22d10405d2db88908072e6907c5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 10 Jun 2022 00:16:20 -0700 Subject: [PATCH 6/8] Expect falling velocity to be linear in time This exposes a current test failures Signed-off-by: Steve Peters --- dartsim/src/JointFeatures_TEST.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index e44e5a16d..3dfbd8a3b 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -21,6 +21,7 @@ #include +#include #include #include @@ -949,6 +950,11 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple) physics::ForwardStep::Output output; physics::ForwardStep::State state; physics::ForwardStep::Input input; + // 1 ms time step + const double dt = 0.001; + auto dur = std::chrono::duration(dt); + input.Get() = + std::chrono::duration_cast(dur); // Create the first joint. This should be a normal fixed joint. const auto poseParent1 = dartBody1->getTransform(); @@ -1013,9 +1019,9 @@ TEST_F(JointFeaturesFixture, JointAttachMultiple) math::eigen3::convert(dartBody2->getLinearVelocity()); math::Vector3d body3LinearVelocity = math::eigen3::convert(dartBody3->getLinearVelocity()); - EXPECT_GT(0, body1LinearVelocity.Z()); + EXPECT_NEAR(dt * (i + 1) * -9.81, body1LinearVelocity.Z(), 1e-3); EXPECT_NEAR(0.0, body2LinearVelocity.Z(), 1e-7); - EXPECT_GT(0, body3LinearVelocity.Z()); + EXPECT_NEAR(dt * (i + 1) * -9.81, body3LinearVelocity.Z(), 1e-3); } } From fe92bd5f1ac0b9895be2acd5adea0e3d6703cc9e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 12 Jun 2022 22:44:56 -0700 Subject: [PATCH 7/8] Fix detaching welded nodes Signed-off-by: Steve Peters --- dartsim/src/Base.hh | 5 +++++ dartsim/src/JointFeatures.cc | 18 +++++++++++++++--- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index b882164a6..ff2da1167 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -417,6 +417,7 @@ class Base : public Implements3d> } } + this->linkByWeldedNode[pairJointBodyNode.second] = _link; return pairJointBodyNode.second; } @@ -523,6 +524,10 @@ class Base : public Implements3d> /// to the BodyNode object. This is useful for keeping track of BodyNodes even /// as they move to other skeletons. public: std::unordered_map linksByName; + + /// \brief Map from welded body nodes to the LinkInfo for the original link + /// they are welded to. This is useful when detaching joints. + public: std::unordered_map linkByWeldedNode; }; } diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 3bc806018..4f48e77f7 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -377,13 +377,25 @@ void JointFeatures::DetachJoint(const Identity &_jointId) dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); - if (!this->links.HasEntity(child)) + LinkInfo *childLinkInfo; + if (this->links.HasEntity(child)) { + childLinkInfo = this->links.at(child).get(); + } + else if (this->linkByWeldedNode.find(child) != + this->linkByWeldedNode.end()) + { + childLinkInfo = this->linkByWeldedNode.at(child); + } + else + { + ignerr << "Could not find LinkInfo for child link [" << child->getName() + << "] when detaching joint " + << "[" << joint->getName() << "]. Joint detaching failed." + << std::endl; return; } - auto childLinkInfo = this->links.at(child); - dart::dynamics::SkeletonPtr skeleton; { // Find the original skeleton the child BodyNode belonged to From 80bafb35cd2604bf249970d76e7392b91eb6cf80 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 28 Jun 2022 11:14:41 -0500 Subject: [PATCH 8/8] Suggestions to #352 (#367) * Remove welded dummy bodies when detaching a joint. Also fixes a bug with integer divisions. Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 90 ++++++++++++++++++++++++++++++------ dartsim/src/JointFeatures.cc | 4 ++ 2 files changed, 79 insertions(+), 15 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index ff2da1167..fe9f21380 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -367,6 +367,35 @@ class Base : public Implements3d> return id; } + private: static math::Inertiald DivideInertial( + const math::Inertiald &_wholeInertial, std::size_t _count) + { + if (_count == 1) + { + return _wholeInertial; + } + math::Inertiald dividedInertial; + math::MassMatrix3d dividedMassMatrix; + dividedMassMatrix.SetMass(_wholeInertial.MassMatrix().Mass() / + static_cast(_count)); + dividedMassMatrix.SetMoi(_wholeInertial.MassMatrix().Moi() * + (1. / static_cast(_count))); + dividedInertial.SetMassMatrix(dividedMassMatrix); + dividedInertial.SetPose(_wholeInertial.Pose()); + return dividedInertial; + } + + private: static void AssignInertialToBody( + const math::Inertiald &_inertial, DartBodyNode * _body) + { + const math::Matrix3d &moi = _inertial.Moi(); + const math::Vector3d &com = _inertial.Pose().Pos(); + _body->setMass(_inertial.MassMatrix().Mass()); + _body->setMomentOfInertia(moi(0, 0), moi(1, 1), moi(2, 2), moi(0, 1), + moi(0, 2), moi(1, 2)); + _body->setLocalCOM(math::eigen3::convert(com)); + } + public: inline DartBodyNode* SplitAndWeldLink(LinkInfo *_link) { // First create a new body node with FreeJoint and a unique name based @@ -398,22 +427,11 @@ class Base : public Implements3d> if (_link->inertial) { std::size_t nodeCount = 1 + _link->weldedNodes.size(); - const double mass = _link->inertial->MassMatrix().Mass() / nodeCount; - const math::Matrix3d moi = _link->inertial->Moi() * (1 / nodeCount); - const math::Vector3d cog = _link->inertial->Pose().Pos(); - - _link->link->setMass(mass); - _link->link->setMomentOfInertia( - moi(0, 0), moi(1, 1), moi(2, 2), - moi(0, 1), moi(0, 2), moi(1, 2)); - _link->link->setLocalCOM(math::eigen3::convert(cog)); - for (auto weldedNodePair : _link->weldedNodes) + const auto dividedInertial = DivideInertial(*_link->inertial, nodeCount); + AssignInertialToBody(dividedInertial, _link->link); + for (const auto &weldedNodePair : _link->weldedNodes) { - weldedNodePair.first->setMass(mass); - weldedNodePair.first->setMomentOfInertia( - moi(0, 0), moi(1, 1), moi(2, 2), - moi(0, 1), moi(0, 2), moi(1, 2)); - weldedNodePair.first->setLocalCOM(math::eigen3::convert(cog)); + AssignInertialToBody(dividedInertial, weldedNodePair.first); } } @@ -421,6 +439,48 @@ class Base : public Implements3d> return pairJointBodyNode.second; } + public: void MergeLinkAndWeldedBody(LinkInfo *_link, DartBodyNode *child) + { + // Break the existing joint first. + child->moveTo(nullptr); + auto it = _link->weldedNodes.begin(); + bool foundWeld = false; + for (; it != _link->weldedNodes.end(); ++it) + { + if (it->first == child) + { + auto worldId = this->GetWorldOfModelImpl( + this->models.objectToID[child->getSkeleton()]); + auto dartWorld = this->worlds.at(worldId); + dartWorld->getConstraintSolver()->removeConstraint(it->second); + // Okay to erase since we break afterward. + _link->weldedNodes.erase(it); + foundWeld = true; + break; + } + } + + if (!foundWeld) + { + // We have not found a welded node associated with _link. This shouldn't + // happen. + ignerr << "Could not find welded body node for link " << _link->name + << ". Merging of link and welded body failed."; + return; + } + + if (_link->inertial) + { + std::size_t nodeCount = 1 + _link->weldedNodes.size(); + const auto dividedInertial = DivideInertial(*_link->inertial, nodeCount); + AssignInertialToBody(dividedInertial, _link->link); + for (const auto &weldedNodePair : _link->weldedNodes) + { + AssignInertialToBody(dividedInertial, weldedNodePair.first); + } + } + } + public: inline std::size_t AddJoint(DartJoint *_joint) { const std::size_t id = this->GetNextEntity(); diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc index 4f48e77f7..99f83a9fc 100644 --- a/dartsim/src/JointFeatures.cc +++ b/dartsim/src/JointFeatures.cc @@ -386,6 +386,10 @@ void JointFeatures::DetachJoint(const Identity &_jointId) this->linkByWeldedNode.end()) { childLinkInfo = this->linkByWeldedNode.at(child); + this->MergeLinkAndWeldedBody(childLinkInfo, child); + this->linkByWeldedNode.erase(child); + child->remove(); + return; } else {