Skip to content

Commit

Permalink
Replace BodyIndex with LinkIndex alias. (RobotLocomotion#21811)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sherm1
sherm1 authored Aug 14, 2024
1 parent 75fc21f commit 1443dd1
Show file tree
Hide file tree
Showing 13 changed files with 277 additions and 264 deletions.
34 changes: 17 additions & 17 deletions multibody/topology/link_joint_graph.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,8 @@ void LinkJointGraph::Clear() {
rpy_floating_joint_traits_index());

// Define the World Link.
const BodyIndex world_index = AddLink("world", world_model_instance());
DRAKE_DEMAND(world_index == BodyIndex(0));
const LinkIndex world_index = AddLink("world", world_model_instance());
DRAKE_DEMAND(world_index == LinkIndex(0));
}

void LinkJointGraph::SetGlobalForestBuildingOptions(
Expand Down Expand Up @@ -163,7 +163,7 @@ const LinkJointGraph::Link& LinkJointGraph::world_link() const {
return links(LinkOrdinal(0));
}

BodyIndex LinkJointGraph::AddLink(const std::string& link_name,
LinkIndex LinkJointGraph::AddLink(const std::string& link_name,
ModelInstanceIndex model_instance,
LinkFlags flags) {
DRAKE_DEMAND(model_instance.is_valid());
Expand Down Expand Up @@ -196,7 +196,7 @@ BodyIndex LinkJointGraph::AddLink(const std::string& link_name,
// If we have a SpanningForest, it's no good now.
InvalidateForest();

const BodyIndex link_index(num_link_indexes());
const LinkIndex link_index(num_link_indexes());
const LinkOrdinal link_ordinal(ssize(links()));
data_.link_index_to_ordinal.push_back(link_ordinal);

Expand All @@ -219,7 +219,7 @@ BodyIndex LinkJointGraph::AddLink(const std::string& link_name,
data_.non_static_must_be_base_body_link_indexes.push_back(link_index);
}

std::vector<BodyIndex>& links_in_instance =
std::vector<LinkIndex>& links_in_instance =
data_.model_instance_to_link_indexes[model_instance];
links_in_instance.push_back(link_index);

Expand Down Expand Up @@ -274,7 +274,7 @@ bool LinkJointGraph::HasJointNamed(
}

std::optional<JointIndex> LinkJointGraph::MaybeGetJointBetween(
BodyIndex link1_index, BodyIndex link2_index) const {
LinkIndex link1_index, LinkIndex link2_index) const {
// Work with the Link that has the fewest joints. (If one of these is World
// it is probably the other one!)
const Link& link1 = link_by_index(link1_index);
Expand All @@ -296,8 +296,8 @@ std::optional<JointIndex> LinkJointGraph::MaybeGetJointBetween(
JointIndex LinkJointGraph::AddJoint(const std::string& name,
ModelInstanceIndex model_instance_index,
const std::string& type,
BodyIndex parent_link_index,
BodyIndex child_link_index,
LinkIndex parent_link_index,
LinkIndex child_link_index,
JointFlags flags) {
DRAKE_DEMAND(model_instance_index.is_valid());
DRAKE_DEMAND(parent_link_index.is_valid());
Expand Down Expand Up @@ -515,7 +515,7 @@ std::optional<JointTraitsIndex> LinkJointGraph::GetJointTraitsIndex(
return it->second;
}

void LinkJointGraph::ChangeLinkFlags(BodyIndex link_index, LinkFlags flags) {
void LinkJointGraph::ChangeLinkFlags(LinkIndex link_index, LinkFlags flags) {
InvalidateForest();
mutable_link(index_to_ordinal(link_index)).set_flags(flags);
}
Expand Down Expand Up @@ -588,7 +588,7 @@ JointIndex LinkJointGraph::AddEphemeralJointToWorld(
data_.joint_index_to_ordinal.push_back(new_joint_ordinal);
data_.joints.emplace_back(
Joint(new_joint_index, new_joint_ordinal, joint_name, model_instance,
traits_index, BodyIndex(0), child.index(), JointFlags::kDefault));
traits_index, LinkIndex(0), child.index(), JointFlags::kDefault));
// Links need to know their joints.
mutable_link(LinkOrdinal(0)).add_joint_as_parent(new_joint_index);
mutable_link(child_link_ordinal).add_joint_as_child(new_joint_index);
Expand All @@ -612,7 +612,7 @@ LinkCompositeIndex LinkJointGraph::AddToLinkComposite(
existing_composite_index = maybe_composite_link.link_composite_index_ =
LinkCompositeIndex(ssize(data_.link_composites));
data_.link_composites.emplace_back(LinkComposite{
.links = std::vector<BodyIndex>{maybe_composite_link.index()},
.links = std::vector<LinkIndex>{maybe_composite_link.index()},
.is_massless = maybe_composite_link.is_massless()});
}

Expand All @@ -639,7 +639,7 @@ LinkOrdinal LinkJointGraph::AddShadowLink(LinkOrdinal primary_link_ordinal,
bool shadow_is_parent) {
/* Caution: this Link reference will be invalid after the emplace. */
const Link& primary_link = links(primary_link_ordinal);
const BodyIndex primary_link_index = primary_link.index();
const LinkIndex primary_link_index = primary_link.index();
const int shadow_num = primary_link.num_shadows() + 1;
/* Name should be <primary_name>$<shadow_num> (unique within primary's model
instance). In the unlikely event that a user has names like this, we'll keep
Expand All @@ -649,7 +649,7 @@ LinkOrdinal LinkJointGraph::AddShadowLink(LinkOrdinal primary_link_ordinal,
fmt::format("{}${}", primary_link.name(), shadow_num);
while (HasLinkNamed(shadow_link_name, primary_link.model_instance()))
shadow_link_name = "_" + shadow_link_name;
const BodyIndex shadow_link_index(num_link_indexes());
const LinkIndex shadow_link_index(num_link_indexes());
const LinkOrdinal shadow_link_ordinal(ssize(links()));
DRAKE_DEMAND(shadow_link_ordinal >= num_user_links()); // A sanity check.
data_.link_index_to_ordinal.push_back(shadow_link_ordinal);
Expand Down Expand Up @@ -705,7 +705,7 @@ bool LinkJointGraph::link_is_static(const Link& link) const {
}

void LinkJointGraph::ThrowLinkWasRemoved(const char* func,
BodyIndex link_index) const {
LinkIndex link_index) const {
throw std::logic_error(fmt::format(
"{}(): An attempt was made to access a link with index {} but that "
"link was removed.",
Expand All @@ -727,7 +727,7 @@ LinkJointGraph::Data::~Data() = default;
auto LinkJointGraph::Data::operator=(const Data&) -> Data& = default;
auto LinkJointGraph::Data::operator=(Data&&) -> Data& = default;

LinkJointGraph::Link::Link(BodyIndex index, LinkOrdinal ordinal,
LinkJointGraph::Link::Link(LinkIndex index, LinkOrdinal ordinal,
std::string name, ModelInstanceIndex model_instance,
LinkFlags flags)
: index_(index),
Expand Down Expand Up @@ -768,8 +768,8 @@ LinkJointGraph::Joint::Joint(JointIndex index, JointOrdinal ordinal,
std::string name,
ModelInstanceIndex model_instance,
JointTraitsIndex joint_traits_index,
BodyIndex parent_link_index,
BodyIndex child_link_index, JointFlags flags)
LinkIndex parent_link_index,
LinkIndex child_link_index, JointFlags flags)
: index_(index),
ordinal_(ordinal),
name_(std::move(name)),
Expand Down
88 changes: 49 additions & 39 deletions multibody/topology/link_joint_graph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,16 +35,26 @@ namespace internal {
/** Represents a graph consisting of Links (user-defined rigid bodies)
interconnected by Joints.
Terminology note: for clarity we use "Link" here to mean what MultibodyPlant
calls a "RigidBody" (or just "Body"), that is, what a user inputs as an sdf or
urdf "link", as a MuJoCo "body", or using the AddRigidBody() call in
MultibodyPlant's API. (It would have been preferable to use Link in
MultibodyPlant as well, but that ship has sailed and there is less chance of
confusion there.) The spanning forest we generate uses "mobilized bodies"
(Mobods). A single Mobod may represent multiple Links (e.g., when multiple
links are welded together). Conversely, a single Link may be split to create
multiple Mobods (to break cycles in the graph). As there is not necessarily a
one-to-one mapping, we're careful not to mix the two terms.
Terminology notes:
- For clarity we use "Link" here to mean what MultibodyPlant calls a
"RigidBody" (or just "Body"), that is, what a user inputs as an SDFormat or
URDF "link", as a MuJoCo "body", or using the AddRigidBody() call in
MultibodyPlant's API. (It would have been preferable to use Link exclusively
in MultibodyPlant as well, but that ship has sailed and there is less chance
of confusion there.)
- The LinkIndex we use here is just an alias for MultibodyPlant's BodyIndex;
they may be used interchangeably.
- The spanning forest we generate uses "mobilized bodies" (Mobods). A single
Mobod may represent multiple Links when multiple links are welded together.
Conversely, a single Link may be split to create multiple Mobods (to break
cycles in the graph). As there is not necessarily a one-to-one mapping, we're
careful not to mix the two terms.
- Because Links and Joints may be removed from an existing graph, there may be
gaps in the sequence of LinkIndex and JointIndex values. However, any
remaining Links and Joints are stored consecutively, indexed by LinkOrdinal
and JointOrdinal. The sequence of ordinals does not have gaps. Indices are
persistent once assigned; ordinals may change as elements are added or
removed.
%LinkJointGraph is a directed, possibly cyclic, graph whose nodes are Links and
whose edges are Joints, defined by a sequence of calls to AddLink() and
Expand Down Expand Up @@ -86,7 +96,7 @@ In general during SpanningForest building:
- We never delete any of the user's Links or Joints; we may add new ones
during forest building but those are distinct from the user's.
@note Links are indexed using MultibodyPlant's BodyIndex type; there is no
@note Links are indexed using MultibodyPlant's LinkIndex type; there is no
separate LinkIndex type since these are necessarily the same. */
class LinkJointGraph {
public:
Expand All @@ -107,7 +117,7 @@ class LinkJointGraph {
/** A LinkComposite is a set of Links that are mutually connected by weld
joints. It is massless only if _all_ its constituent Links are massless. */
struct LinkComposite {
std::vector<BodyIndex> links;
std::vector<LinkIndex> links;
bool is_massless{false};
};

Expand Down Expand Up @@ -221,28 +231,28 @@ class LinkJointGraph {
any flags marked "internal use only" (currently just `Shadow`).
@note The World Link is always predefined with name "world" (case sensitive),
model instance world_model_instance(), and index 0.
@returns The unique BodyIndex for the added Link in the graph.
@returns The unique LinkIndex for the added Link in the graph.
@throws std::exception if `name` is duplicated within `model_instance`.
@throws std::exception if an attempt is made to add a link into the World's
model instance
@throws std::exception if the `flags` parameter has any internal-only flags
set (currently just `Shadow`). */
BodyIndex AddLink(const std::string& name, ModelInstanceIndex model_instance,
LinkIndex AddLink(const std::string& name, ModelInstanceIndex model_instance,
LinkFlags flags = LinkFlags::kDefault);

// TODO(sherm1) Add this method;
// void RemoveLink(BodyIndex doomed_link_index);
// void RemoveLink(LinkIndex doomed_link_index);

/** Returns true if the given `link_index` is in range and the corresponding
Link hasn't been removed. Can be a user or ephemeral Link. */
[[nodiscard]] bool has_link(BodyIndex link_index) const {
[[nodiscard]] bool has_link(LinkIndex link_index) const {
return link_index < num_link_indexes() &&
data_.link_index_to_ordinal[link_index].has_value();
}

/** Returns true if the given Link exists and is ephemeral. Ephemeral links
are present only if the SpanningForest is valid. */
[[nodiscard]] bool link_is_ephemeral(BodyIndex link_index) const {
[[nodiscard]] bool link_is_ephemeral(LinkIndex link_index) const {
if (has_link(link_index) && link_index > data_.max_user_link_index) {
DRAKE_ASSERT(index_to_ordinal(link_index) >= num_user_links());
return true;
Expand All @@ -264,10 +274,10 @@ class LinkJointGraph {
registered with calls to RegisterJointType().
@param[in] parent_link_index
The index of a Link previously obtained with a call to AddLink(), or
BodyIndex(0) for the World Link.
LinkIndex(0) for the World Link.
@param[in] child_link_index
The index of a Link previously obtained with a call to AddLink(), or
BodyIndex(0) for the World Link.
LinkIndex(0) for the World Link.
@param[in] flags
Optional JointFlags requesting special handling for this Joint.
@returns The unique JointIndex for the added Joint in the graph.
Expand All @@ -281,8 +291,8 @@ class LinkJointGraph {
other than a "weld" Joint. */
JointIndex AddJoint(const std::string& name,
ModelInstanceIndex model_instance_index,
const std::string& type, BodyIndex parent_link_index,
BodyIndex child_link_index,
const std::string& type, LinkIndex parent_link_index,
LinkIndex child_link_index,
JointFlags flags = JointFlags::kDefault);

/** Removes a previously-added Joint and any references to it in the graph.
Expand Down Expand Up @@ -358,7 +368,7 @@ class LinkJointGraph {
}

/** Returns a reference to the vector of Link objects, contiguous and ordered
by ordinal (not by BodyIndex). World is always the first entry and is always
by ordinal (not by LinkIndex). World is always the first entry and is always
present. `ssize(links())` is the number of Links currently in this graph. */
[[nodiscard]] const std::vector<Link>& links() const { return data_.links; }

Expand All @@ -370,11 +380,11 @@ class LinkJointGraph {
[[nodiscard]] inline const Link& links(LinkOrdinal link_ordinal) const;

/** Returns a reference to a particular Link using the index returned by
AddLink(). The World Link is BodyIndex(0). Ephemeral links added by
BuildForest() are indexed last. Requires a BodyIndex, not a plain integer.
AddLink(). The World Link is LinkIndex(0). Ephemeral links added by
BuildForest() are indexed last. Requires a LinkIndex, not a plain integer.
@throws std::exception if the index is out of range or if the selected Link
was removed. */
[[nodiscard]] inline const Link& link_by_index(BodyIndex link_index) const;
[[nodiscard]] inline const Link& link_by_index(LinkIndex link_index) const;

/** Returns a reference to the vector of Joint objects, contiguous and ordered
by ordinal (not by JointIndex). `ssize(joints())` is the number of Joints
Expand Down Expand Up @@ -420,7 +430,7 @@ class LinkJointGraph {
will be the mobilized body for the whole composite. If the Link was split into
a primary and shadows, this is the mobilized body followed by the primary. If
there is no valid Forest, the returned index will be invalid. */
[[nodiscard]] MobodIndex link_to_mobod(BodyIndex index) const;
[[nodiscard]] MobodIndex link_to_mobod(LinkIndex index) const;

/** After the SpanningForest has been built, returns groups of Links that are
welded together, which we call "LinkComposites". Each composite may be modeled
Expand Down Expand Up @@ -473,7 +483,7 @@ class LinkJointGraph {
building, so you may get a different answer before and after modeling. Cost is
O(j) where j=min(j₁,j₂) with jᵢ the number of Joints attached to linkᵢ. */
[[nodiscard]] std::optional<JointIndex> MaybeGetJointBetween(
BodyIndex link1_index, BodyIndex link2_index) const;
LinkIndex link1_index, LinkIndex link2_index) const;

/** Returns true if the given Link should be treated as massless. That
requires that the Link was flagged kMassless and is not connected by
Expand All @@ -483,7 +493,7 @@ class LinkJointGraph {
LinkOrdinal link_ordinal) const;

/** (Internal use only) For testing -- invalidates the Forest. */
void ChangeLinkFlags(BodyIndex link_index, LinkFlags flags);
void ChangeLinkFlags(LinkIndex link_index, LinkFlags flags);

/** (Internal use only) For testing -- invalidates the Forest. */
void ChangeJointFlags(JointIndex joint_index, JointFlags flags);
Expand Down Expand Up @@ -572,7 +582,7 @@ class LinkJointGraph {

// Given a link index returns that link's ordinal.
// @pre the index refers to a link that exists and hasn't been removed.
[[nodiscard]] LinkOrdinal index_to_ordinal(BodyIndex link_index) const {
[[nodiscard]] LinkOrdinal index_to_ordinal(LinkIndex link_index) const {
DRAKE_ASSERT(link_index.is_valid() &&
link_index < ssize(data_.link_index_to_ordinal));
const std::optional<LinkOrdinal>& ordinal =
Expand Down Expand Up @@ -659,14 +669,14 @@ class LinkJointGraph {
const std::string& joint_type_name) const;

// Links that were explicitly flagged LinkFlags::kStatic in AddLink().
const std::vector<BodyIndex>& static_link_indexes() const {
const std::vector<LinkIndex>& static_link_indexes() const {
return data_.static_link_indexes;
}

// Links that were flagged LinkFlags::kMustBeBaseBody in AddLink() but were
// not also flagged kStatic (those are inherently base bodies since by
// definition they are welded to World).
const std::vector<BodyIndex>& non_static_must_be_base_body_link_indexes()
const std::vector<LinkIndex>& non_static_must_be_base_body_link_indexes()
const {
return data_.non_static_must_be_base_body_link_indexes;
}
Expand All @@ -691,7 +701,7 @@ class LinkJointGraph {
LinkCompositeIndex which);

[[noreturn]] void ThrowLinkWasRemoved(const char* func,
BodyIndex link_index) const;
LinkIndex link_index) const;

[[noreturn]] void ThrowJointWasRemoved(const char* func,
JointIndex joint_index) const;
Expand All @@ -710,7 +720,7 @@ class LinkJointGraph {

std::vector<JointTraits> joint_traits;

// The first entry in links is the World Link with BodyIndex(0) and name
// The first entry in links is the World Link with LinkIndex(0) and name
// world_link().name(). Ephemeral "as-built" links and joints are placed
// at the end of these lists, following the user-supplied ones.
// Access these lists by _ordinal_ rather than _index_.
Expand All @@ -732,19 +742,19 @@ class LinkJointGraph {
// is an index temporarily assigned to ephemeral elements. Note that the
// highest-ordinal user link or joint does not necessarily have the
// highest-ever user index since higher ones might have been removed.
BodyIndex max_user_link_index;
LinkIndex max_user_link_index;
JointIndex max_user_joint_index;

// Loop Weld Constraints are only added during forest building; we don't
// record any user constraints in the LinkJointGraph because they don't
// affect how we build the forest.
std::vector<LoopConstraint> loop_constraints;

std::vector<BodyIndex> static_link_indexes;
std::vector<BodyIndex> non_static_must_be_base_body_link_indexes;
std::vector<LinkIndex> static_link_indexes;
std::vector<LinkIndex> non_static_must_be_base_body_link_indexes;

// Every user Link, organized by Model Instance.
std::map<ModelInstanceIndex, std::vector<BodyIndex>>
std::map<ModelInstanceIndex, std::vector<LinkIndex>>
model_instance_to_link_indexes;

// This must always have the same number of entries as joint_traits_.
Expand All @@ -754,7 +764,7 @@ class LinkJointGraph {
// links/joints/actuators/etc may appear with the same name in different
// model instances. The index values are still unique across the graph.
// These include only user-supplied links and joints.
string_unordered_multimap<BodyIndex> link_name_to_index;
string_unordered_multimap<LinkIndex> link_name_to_index;
string_unordered_multimap<JointIndex> joint_name_to_index;

// The 0th composite always exists and contains World (listed first)
Expand All @@ -768,7 +778,7 @@ class LinkJointGraph {
// These parallel {link,joint}_name_to_index except they hold mappings
// for ephemeral links and joints added during forest-building. We keep
// them separately so they are easy to get rid of.
string_unordered_multimap<BodyIndex> ephemeral_link_name_to_index;
string_unordered_multimap<LinkIndex> ephemeral_link_name_to_index;
string_unordered_multimap<JointIndex> ephemeral_joint_name_to_index;

ForestBuildingOptions global_forest_building_options{
Expand Down
Loading

0 comments on commit 1443dd1

Please sign in to comment.