Add `WeldJointConstraint` for fixed joints if the parent is not a FreeJoint #345

arjo129 · 2022-04-28T08:57:50Z

🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸🔸

🎉 New feature

Allows for Fixed Joints to be created when the parent is not a FreeJoint. This is required when we try to simulate multiple suction grippers gripping onto the same object.

⚠️ ⚠️ ⚠️ ⚠️

Summary

Currently, if we try to attach two models to the same item using fixed joints, we are unable to do so. This is because Dart's WeldJoint requires that the rigid body skeletons to form a tree. We can solve this by using WeldJointConstraints instead and that is what I'm trying to use.

Test it

I've introduced a unit test here.

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 box resting on the big box and the floating 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.

TODO

Since this affects the physics in a lot of places it will be important to do the following before merging

Check performance - Seems to be able to do realtime speeds in the example shown below
Figure out how to cast joints to weld contraints - Skip casting if is a constraint
Do an upstream integration test.
Get a ✔️ in the CI. - Good ol' 🪟 not passing
Check other affected functions.
Find out how to deal with ObjectToId as weld constraints are not added to this list. - Pending reviewer feedback.

Real world gripping demo

Manual integration test for this behaviour (skip to 1min mark):
https://user-images.githubusercontent.com/542272/167532762-380d3fa8-8f51-4b51-91ce-4a5fdc439e55.mp4

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
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.

…ched to the same opbject. Signed-off-by: Arjo Chakravarty <[email protected]>

Signed-off-by: Arjo Chakravarty <[email protected]>

codecov · 2022-05-09T02:36:59Z

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     #345   +/-   ##
===============================================
  Coverage                ?   80.13%           
===============================================
  Files                   ?      199           
  Lines                   ?     7154           
  Branches                ?        0           
===============================================
  Hits                    ?     5733           
  Misses                  ?     1421           
  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...3325673. Read the comment docs.

…lock. > Note: Not sure if we really should be keeping this stuff in `mbzir_ign` or we should have a `mbzirc_demos` package. This demo demonstrates using multiple robots to lift the same overweight block. Requires gazebosim/gz-physics#345 ## To Test Make sure you have the right branch of gz-physics set up. Launch the test world file: ```bash ign gazebo /your/path/to/mbzirc_ws/src/mbzirc/mbzirc_ign/worlds/test/multi_drone_lift.sdf ``` In another terminal run: ```bash ros2 launch mbzirc_ign multi_uav_lift_demo.launch.py ``` You should see the drones spawn. After that run the simulators and let the robots fall onto the block. When the robots are resting on the block run: ```bash ros2 run mbzirc_ign multi_uav_demo.py ``` Signed-off-by: Arjo Chakravarty <[email protected]>

iche033

the changes look good to me and it's now possible to create more than one detachable joints with the same link in our application as demonstrated in osrf/mbzirc#108.

I think it would be good to have one of the physics experts take a look at these changes as well

iche033 · 2022-05-10T20:28:11Z

dartsim/src/Base.hh

+    //         _joint->getTransformFromChildBodyNode());
+
+    // this->joints.idToObject[id]->frame = jointFrame;
+    // this->frames[id] = this->joints.idToObject[id]->frame.get();


remove this code or is this relevant to the todo comment?

Yeah honestly I'm not sure where this->frames and this->jointsidToObject is used. Would be great if someone who knew the physics subsystem well could advice on whether we need it.

The two containers are used for implementing frame semantics (see

gz-physics/dartsim/src/KinematicsFeatures.cc

Line 64 in e91c58a

const dart::dynamics::Frame *KinematicsFeatures::SelectFrame(

) i.e, it allows us to query the position, velocity, and acceleration of any frame bearing entities, such models, links, and joints. In the dartsim plugin, frame semantics is currently implemented for links, shapes, joints, and freegroups.

So yes, we need to implement this since the weld constraint represents an actual joint whose frame semantics could be queried by the user (although I don't think we do that currently in gz-sim).

BTW, a dynamics::SimpleFrame is created because DART doesn't automatically associate a frame with a joint, unlike BodyNodes.

dartsim/src/JointFeatures.cc

dartsim/src/Base.hh

azeey · 2022-05-10T20:55:25Z

I think this can be a way to solve #25. In Gazebo classic, I believe kinematic loops were supported by breaking one of the links in the loop in half and attaching a constraint, which is similar to what's being done here except that the addition of the WeldConstraint only happens here when handling fixed joints. @scpeters, do you think we can make this the recommended way of dealing with kinematic loops where the user creates the fixed joint that creates the loop which triggers ign-physics to create a WeldConstraint? It's more of a manual approach compared to what's done in Gazebo classic.

dartsim/src/JointFeatures.cc

Signed-off-by: Arjo Chakravarty <[email protected]>

arjo129 · 2022-05-12T04:27:46Z

I think this can be a way to solve #25. In Gazebo classic, I believe kinematic loops were supported by breaking one of the links in the loop in half and attaching a constraint, which is similar to what's being done here except that the addition of the WeldConstraint only happens here when handling fixed joints. @scpeters, do you think we can make this the recommended way of dealing with kinematic loops where the user creates the fixed joint that creates the loop which triggers ign-physics to create a WeldConstraint? It's more of a manual approach compared to what's done in Gazebo classic.

I'm open to reworking this to follow an approach closer to that of gazebo classic so that we solve #25 as well. To keep things automated we could potentially add a joint, a bodyNode and a weld constraint instead of just the weld constraint.

This PR supercedes #345 with a more general solution that would also help address #25 and solve a lot of issues related to casting joints. Open to feedback.. # TODO - [ ] clean up all the excess joints and links created during the process of loop joint creation. - [ ] fix SetJointTransformChild to use only the joint. Signed-off-by: Arjo Chakravarty <[email protected]>

scpeters · 2022-06-05T09:04:50Z

see #352

…lock (#108) * Adds a simple demo with multiple robots lifting the same overweight block. > Note: Not sure if we really should be keeping this stuff in `mbzir_ign` or we should have a `mbzirc_demos` package. This demo demonstrates using multiple robots to lift the same overweight block. Requires gazebosim/gz-physics#345 ## To Test Make sure you have the right branch of gz-physics set up. Launch the test world file: ```bash ign gazebo /your/path/to/mbzirc_ws/src/mbzirc/mbzirc_ign/worlds/test/multi_drone_lift.sdf ``` In another terminal run: ```bash ros2 launch mbzirc_ign multi_uav_lift_demo.launch.py ``` You should see the drones spawn. After that run the simulators and let the robots fall onto the block. When the robots are resting on the block run: ```bash ros2 run mbzirc_ign multi_uav_demo.py ``` Signed-off-by: Arjo Chakravarty <[email protected]> * Address PR Feedback Signed-off-by: Arjo Chakravarty <[email protected]> * fix spawn multi uav Signed-off-by: Ian Chen <[email protected]> * fix params Signed-off-by: Arjo Chakravarty <[email protected]> * Add the test worlds Signed-off-by: Arjo Chakravarty <[email protected]> * more test stuff Signed-off-by: Arjo Chakravarty <[email protected]> * Added new large models for dragging into testing world, launch scripts for dragging Signed-off-by: Aaron Chong <[email protected]> * Cleaning up additional flat scenario on the roof Signed-off-by: Aaron Chong <[email protected]> * fix style (#141) Signed-off-by: Ian Chen <[email protected]> * Create unique gripper model per parent vehicle model (#143) * create unique gripper model files per uav Signed-off-by: Ian Chen <[email protected]> * style Signed-off-by: Ian Chen <[email protected]> * Checking if gipper model name is a substring instead of direct compare (#145) Signed-off-by: Aaron Chong <[email protected]> Co-authored-by: Aaron Chong <[email protected]> * fix style Signed-off-by: Ian Chen <[email protected]> * fix spawning uav without gripper Signed-off-by: Ian Chen <[email protected]> * fixing tests Signed-off-by: Ian Chen <[email protected]> * Updating doc in example lift and drag demos, adding detach flag in demo script Signed-off-by: Aaron Chong <[email protected]> * Clean up ununsed demo launch file Signed-off-by: Aaron Chong <[email protected]> Co-authored-by: Ian Chen <[email protected]> Co-authored-by: Aaron Chong <[email protected]>

azeey

I think there are some thorny problems with this approach. The main issue being the fact that we don't have a dart::dynamics::Joint in the JointInfo object we associate with the Joint entity. There are many features (mostly in JointFeatures.cc) that depend on that joint pointer being valid. Using these features would cause a segfault in the current state of things. For example, EntityManagementFeatures::GetJointName calls dart::dynamics::Joint::getName, but that wouldn't work if this->ReferenceInterface<JointInfo>(_jointID)->joint is nullptr. I don't think gz-sim calls GetName on a joint entity, so that might be why it's working fine right now. However, the same applies to JointFeatures::GetJointPosition JointFeatures::GetJointVelocity, etc. and if we added the JointStatePublisher system to the model that contains a kinematic loop, I'm not sure if it would work. Since we know it's a fixed joint, we can add conditionals in those functions that return 0 if the joint is a WeldJointConstraint, but we'd need to something similar in all the places this->ReferenceInterface<JointInfo>(_jointID)->joint is used and we would need some extra bookkeeping.

We also used fixed joints to model force/torque sensors. JointFeatures::GetJointTransmittedWrenchInJointFrame would have to be reworked to support WeldJointConstraints.

I haven't reviewed @scpeters' alternative PR, but that may not suffer from the same issues as this since the WeldJointConstraint would be an internal object that is not associated with a Joint entity.

azeey · 2022-06-23T20:11:20Z

dartsim/src/Base.hh

@@ -73,6 +74,15 @@ struct JointInfo
 {
  dart::dynamics::JointPtr joint;
  dart::dynamics::SimpleFramePtr frame;
+
+  enum JointType


nit: Use enum class.

azeey · 2022-06-23T20:25:03Z

dartsim/src/Base.hh

+    //         _joint->getTransformFromChildBodyNode());
+
+    // this->joints.idToObject[id]->frame = jointFrame;
+    // this->frames[id] = this->joints.idToObject[id]->frame.get();


The two containers are used for implementing frame semantics (see

gz-physics/dartsim/src/KinematicsFeatures.cc

Line 64 in e91c58a

const dart::dynamics::Frame *KinematicsFeatures::SelectFrame(

) i.e, it allows us to query the position, velocity, and acceleration of any frame bearing entities, such models, links, and joints. In the dartsim plugin, frame semantics is currently implemented for links, shapes, joints, and freegroups.

So yes, we need to implement this since the weld constraint represents an actual joint whose frame semantics could be queried by the user (although I don't think we do that currently in gz-sim).

BTW, a dynamics::SimpleFrame is created because DART doesn't automatically associate a frame with a joint, unlike BodyNodes.

scpeters · 2022-06-29T18:35:29Z

alternative fix merged in #352

arjo129 added 4 commits April 20, 2022 16:32

Use WeldJointConstraint to allow more than one fixed joint to be atta…

80d780b

…ched to the same opbject. Signed-off-by: Arjo Chakravarty <[email protected]>

Remove item

59e4428

Signed-off-by: Arjo Chakravarty <[email protected]>

Added unit test hope it works.

a261ed7

Signed-off-by: Arjo Chakravarty <[email protected]>

Docs

54c29f1

Signed-off-by: Arjo Chakravarty <[email protected]>

github-actions bot added the 🏯 fortress Ignition Fortress label Apr 28, 2022

style

ffb1f6a

Signed-off-by: Arjo Chakravarty <[email protected]>

arjo129 added the DART DART engine label Apr 29, 2022

arjo129 changed the title ~~Switch to use of WeldJointConstraint for fixed joints~~ ~~Switch to use of WeldJointConstraint for fixed joints~~ Trying to figure loop joints out. Apr 29, 2022

arjo129 changed the title ~~~~Switch to use of WeldJointConstraint for fixed joints~~ Trying to figure loop joints out.~~ ~Switch to use of WeldJointConstraint for fixed joints~ Trying to figure loop joints out. Apr 29, 2022

arjo129 changed the title ~~~Switch to use of WeldJointConstraint for fixed joints~ Trying to figure loop joints out.~~ Trying to figure loop joints out. Apr 29, 2022

arjo129 added 2 commits May 9, 2022 10:03

Only use WeldJointConstraint in the event of non-tree topology

0690374

Signed-off-by: Arjo Chakravarty <[email protected]>

Handle casts

a43f308

Signed-off-by: Arjo Chakravarty <[email protected]>

arjo129 marked this pull request as ready for review May 9, 2022 02:13

arjo129 requested review from azeey, mxgrey and scpeters as code owners May 9, 2022 02:13

arjo129 changed the title ~~Trying to figure loop joints out.~~ Add WeldJointConstraint for fixed joints if the parent is not a FreeJoint May 9, 2022

arjo129 mentioned this pull request May 10, 2022

Adds a simple demo with multiple robots lifting the same overweight block osrf/mbzirc#108

Merged

iche033 reviewed May 10, 2022

View reviewed changes

arjo129 commented May 11, 2022

View reviewed changes

dartsim/src/JointFeatures.cc Show resolved Hide resolved

arjo129 added 2 commits May 11, 2022 09:59

PR feedback

f921d76

Signed-off-by: Arjo Chakravarty <[email protected]>

PR feedback

3325673

Signed-off-by: Arjo Chakravarty <[email protected]>

chapulina added the mbzirc Sponsored by MBZIRC: https://github.com/osrf/mbzirc/ label May 14, 2022

arjo129 mentioned this pull request May 17, 2022

Try creating a loop joint #347

Closed

11 tasks

scpeters mentioned this pull request Jun 5, 2022

dartsim: support non-tree kinematics in AttachFixedJoint #352

Merged

8 tasks

arjo129 closed this Jun 17, 2022

iche033 mentioned this pull request Jun 23, 2022

Add WeldJointConstraint for fixed joints if the parent is not a FreeJoint #366

Closed

15 tasks

iche033 reopened this Jun 23, 2022

azeey requested changes Jun 23, 2022

View reviewed changes

scpeters closed this Jun 29, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add `WeldJointConstraint` for fixed joints if the parent is not a FreeJoint #345

Add `WeldJointConstraint` for fixed joints if the parent is not a FreeJoint #345

arjo129 commented Apr 28, 2022 •

edited

Loading

codecov bot commented May 9, 2022 •

edited

Loading

iche033 left a comment

iche033 May 10, 2022

arjo129 May 11, 2022

azeey Jun 23, 2022

azeey commented May 10, 2022

arjo129 commented May 12, 2022 •

edited

Loading

scpeters commented Jun 5, 2022

azeey left a comment •

edited

Loading

azeey Jun 23, 2022

azeey Jun 23, 2022

scpeters commented Jun 29, 2022

Add WeldJointConstraint for fixed joints if the parent is not a FreeJoint #345

Add WeldJointConstraint for fixed joints if the parent is not a FreeJoint #345

Conversation

arjo129 commented Apr 28, 2022 • edited Loading

🎉 New feature

Summary

Test it

TODO

Real world gripping demo

Checklist

codecov bot commented May 9, 2022 • edited Loading

Codecov Report

iche033 left a comment

Choose a reason for hiding this comment

iche033 May 10, 2022

Choose a reason for hiding this comment

arjo129 May 11, 2022

Choose a reason for hiding this comment

azeey Jun 23, 2022

Choose a reason for hiding this comment

azeey commented May 10, 2022

arjo129 commented May 12, 2022 • edited Loading

scpeters commented Jun 5, 2022

azeey left a comment • edited Loading

Choose a reason for hiding this comment

azeey Jun 23, 2022

Choose a reason for hiding this comment

azeey Jun 23, 2022

Choose a reason for hiding this comment

scpeters commented Jun 29, 2022

Add `WeldJointConstraint` for fixed joints if the parent is not a FreeJoint #345

Add `WeldJointConstraint` for fixed joints if the parent is not a FreeJoint #345

arjo129 commented Apr 28, 2022 •

edited

Loading

codecov bot commented May 9, 2022 •

edited

Loading

arjo129 commented May 12, 2022 •

edited

Loading

azeey left a comment •

edited

Loading