Is there a way to detect if two bodies/frames are fixed in Drake?

Question

Does anybody know of an easy way to determine if two bodies (or frames) are fixed with respect to one another in Pydrake? Thank you!

score 3 · Accepted Answer · answered May 10 '22 at 18:53

3

There is the GetBodiesWeldedTo() method on MultibodyPlant.

Python doc is here. Given some reference body, it will return a list of all the bodies connected to it by one or more weld joints.

If you are looking at welds for collision filtering purposes, note that Drake release 1.2.0 and later will ignore collisions within all welded groups of bodies by default.

answered May 10 '22 at 18:53

rpoyner-tri

46
2

Does a fixed joint in a URDF automatically become a welded joint in Drake? – JoshuaF May 10 '22 at 18:57
1

Yes, when [parsing a type=fixed joint in URDF](https://github.com/RobotLocomotion/drake/blob/55b8b27af263fd8d7911f7049f60b75693dd857a/multibody/parsing/detail_urdf_parser.cc#L523-L527) it creates a WeldJoint. – jwnimmer-tri May 11 '22 at 05:35

score 3 · Answer 2 · answered May 10 '22 at 21:15

Along Rico's post, this is what we use in Anzu (TRI codebase) which uses GetBodiesWeldedTo - nothing crazy, and Python looks about the same.

bool AreFramesWelded(
    const MultibodyPlant<double>& plant,
    const Frame<double>& A, const Frame<double>& B) {
  if (&A.body() == &B.body())
    return true;
  for (const auto* body : plant.GetBodiesWeldedTo(A.body())) {
    if (body == &B.body())
      return true;
  }
  return false;
}

Is there a way to detect if two bodies/frames are fixed in Drake?

2 Answers2