dartsim: support non-tree kinematics in AttachFixedJoint

[scpeters]

🦟 Bug fix

Alternative to #345 and #347

Summary

With inspiration from #345 and #347, this implements a variation on the approach from gazebo-classic for handling non-tree kinematics (including kinematic loops) with the reduced coordinate Featherstone formulation by splitting links and welding them back together with a constraint. This adds data to the LinkInfo struct and a SplitAndWeldLink helper function to Base.hh. The test made by @arjo129 is included as well.

This only implements support for kinematic loops in the AttachFixedJoint method, but it should be applicable in other Attach*Joint methods and the ConstructSdfJoint method to support non-tree kinematics in more cases.

This implementation differs slightly from the approach in gazebo-classic. In gazebo11, all joints that are created dynamically using gazebo::physics::Model::CreateJoint use the approach of splitting and welding a link, even if the joint could simply be added as part of the existing Skeleton (see the call to DARTModelPrivate::CreateLoopJointAndNodePair in DARTModel::CreateJointHelper in DARTModel.cc). Within DARTModlPrivate::CreateLoopJointAndNodePair, a new joint and node pair are created for the desired joint type by calling DARTModelPrivate::CreateJointAndNodePair and then welded to the desired child link with blended inertia by calling DARTLink::AddSlaveBodyNode.

In this implementation, the split-and-weld approach is only taken if the target child link already has a parent joint that is not FreeJoint. To minimize changes to the existing code, when an existing parent joint is detected, the split-and-weld is done first, by creating a new FreeJoint and node pair and passing it to the SplitAndWeldLink helper function, which blends the inertia between the nodes and creates a WeldJointConstraint. The newly welded body node is then moved to a WeldJoint using the moveTo API. This move requires the transform for the WeldJointConstraint to be updated to ensure consistency.

UPDATE: with thanks to @azeey for the changes in #367, the welded links are now properly removed when joints are detached.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

❗ No coverage uploaded for pull request base (ign-physics5@e91c58a). Click here to learn what that means.
The diff coverage is n/a.

@@               Coverage Diff               @@
##             ign-physics5     #352   +/-   ##
===============================================
  Coverage                ?   76.35%           
===============================================
  Files                   ?      129           
  Lines                   ?     5810           
  Branches                ?        0           
===============================================
  Hits                    ?     4436           
  Misses                  ?     1374           
  Partials                ?        0           

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update e91c58a...80bafb3. Read the comment docs.

[arjo129]

I've added the unit tests in but they do not pass. Neither does the down stream pr in osrf/mbzirc#108 work. I'm not too sure whats wrong with it. Either ways I'm not on mbzirc right now. I believe @aaronchongth is taking over this aspect of it.

[aaronchongth]

I might be wrong, but could the unit test that was ported over not be related to kinetic loops since both the parents of the added fixed joints are free floating and not attached to each other?

[scpeters]

I might be wrong, but could the unit test that was ported over not be related to kinetic loops since both the parents of the added fixed joints are free floating and not attached to each other?

I think the test is ok. It may not precisely be a "loop", but it doesn't follow the tree structure because the middle box will have two parents after attaching the fixed joints.

I think I have a fix in 21a608d

[scpeters]

I've updated the description and marked this ready for review

The test has been fixed

[aaronchongth]

Thanks for explaining @scpeters !

Attaching works like a charm! I am having trouble detaching the models though, it looks like one of the models will refuse to detach.

detach_and_stuck-2022-06-10_11.55.40.mp4

It looks like the first parent that attaches, is unable to detach afterwards, but the second parent is free to attach and detach. After detaching, both the still-attached parent and the model will be stuck in space somehow, until the other previously detached parent bumps into it.

I tried dropping shapes onto the stuck platform, but it doesn't budge.

[aaronchongth]

After testing it more downstream, I faced some interesting behaviors when detaching the models one after another. Consistently, detaching one of the joints does not seem to do anything, while detaching the other one causes both to be detached.

detach_1.mp4

detach_2.mp4

I duplicated the test in this PR to detach one at a time, but could not seem to replicate this behvaior. I was wondering if you knew what might be the cause?

[iche033]

After testing it more downstream, I faced some interesting behaviors when detaching the models one after another. Consistently, detaching one of the joints does not seem to do anything, while detaching the other one causes both to be detached.

I looked into this a bit and turns out that it's a downstream issue. We are generating the same detach topic name for both UAVs (one overriding another). So something to be fixed on our end.

[scpeters]

After testing it more downstream, I faced some interesting behaviors when detaching the models one after another. Consistently, detaching one of the joints does not seem to do anything, while detaching the other one causes both to be detached.

I looked into this a bit and turns out that it's a downstream issue. We are generating the same detach topic name for both UAVs (one overriding another). So something to be fixed on our end.

ok, I've also done some testing and if I only detach one joint, I would expect both boxes to fall, but only one is doing so. There may still be issues in the Detaching functionality here

[iche033]

ok, I've also done some testing and if I only detach one joint, I would expect both boxes to fall, but only one is doing so. There may still be issues in the Detaching functionality here

@aaronchongth was also able to reproduce the issue. Once the joint between a drone and an object is detached, we noticed that the drone is able to continue flying but the object remains static. Let us know if there's anything we can help here.

[scpeters]

ok, I've also done some testing and if I only detach one joint, I would expect both boxes to fall, but only one is doing so. There may still be issues in the Detaching functionality here

@aaronchongth was also able to reproduce the issue. Once the joint between a drone and an object is detached, we noticed that the drone is able to continue flying but the object remains static. Let us know if there's anything we can help here.

I've tried debugging it, but I may need some advice from @mxgrey and @azeey about how to figure out what is going on

[arjo129]

Just a random thought: I don't see us removing the WeldConstraint in the detach function. Does this have something to do with it.

[mxgrey]

I haven't looked at the code yet, but based on the conversation I might suspect that if the WeldConstraint is being kept alive inside the World where one BodyNode was removed from the constraint while the other was kept inside the constraint, then DART will believe that the BodyNode that remains inside the constraint is meant to be rigidly attached to "the world" because a whenever a BodyNode* has a nullptr value it's treated as "the world".

I'll take a look to check my hypothesis later today.

[mxgrey]

Like @arjo129 suggested, I don't see ConstraintSolver::removeConstraint ever get called to remove the weld constraint that gets added here. I believe this is almost certainly the cause of the problem.

What I would suggest is to maintain a hash map of BodyNode* -> ConstraintBasePtr where each BodyNode* in the map is a shard that was created for the sake of this WeldConstraint technique. When DetachJoint is called, we should check that hash map to see if the child BodyNode of the joint is one of the keys. If it is, then we should delete the shard, redistribute the inertia across all the remaining shards for that link, and then finally remove the ConstraintBasePtr weld constraint from the world.

All that being said, it's worth noting that this link splitting technique is not actually needed for this PR. The ordinary way of using the weld constraint would completely suffice for AttachFixedJoint. The motive for splitting the link back in Gazebo classic was to allow other joint types (e.g. Revolute, Prismatic), to form non-tree structures. If a link ever gets assigned more than one parent where one of the parents is a non-fixed joint, then we split the link into BodyNode shards, glue the shards together with weld constraints, and give each shard a different parent joint. Doing that allows each parent joint to be any joint type. But if we are only implementing this for fixed joint then there's no benefit to the splitting.

I might suggest reworking this PR so that the link splitting logic gets applied to all the joint types instead of just the fixed joint. Otherwise it would be much cleaner to just remove the link splitting entirely.

[scpeters]

I've merged #367 into this branch, so I think this is ready for re-review

[iche033]

tested and confirmed that this PR works for us. I'll leave it to @azeey to give the final blessing.

[azeey]

It would be great to add test expectations on the resulting masses after a WeldJointConstraint is created and destroyed, but feel free to punt for now.

[scpeters]

I'll do this in a follow-up