Skip to content

Commit

Permalink
lint
Browse files Browse the repository at this point in the history
  • Loading branch information
azeey committed Apr 7, 2022
1 parent 0776e27 commit 7009139
Show file tree
Hide file tree
Showing 5 changed files with 17 additions and 29 deletions.
25 changes: 7 additions & 18 deletions multibody/parsing/detail_multibody_plant_subgraph.cc
Original file line number Diff line number Diff line change
Expand Up @@ -333,62 +333,51 @@ void CheckPlantAggregate(GetFunction get_func, const T* item) {
}

template <typename Item>
bool ContainsItem(const std::set<Item>& container, const Item& item){
bool ContainsItem(const std::set<Item>& container, const Item& item) {
return std::find(container.begin(), container.end(), item) != container.end();
}

void CheckSubgraphInvariants(const MultibodyPlantElements& elem){
void CheckSubgraphInvariants(const MultibodyPlantElements& elem) {
auto plant_model_instances = GetModelInstances(*elem.plant());
DRAKE_DEMAND(std::includes(
plant_model_instances.begin(), plant_model_instances.end(),
elem.model_instances().begin(), elem.model_instances().end()));

const MultibodyPlant<double> *plant = elem.plant();
// Check bodies.
for (const auto *body : elem.bodies()){
for (const auto* body : elem.bodies()) {
CheckPlantAggregate([&](auto i) { return &plant->get_body(i); }, body);
DRAKE_DEMAND(ContainsItem(elem.model_instances(), body->model_instance()));
}

// Check frames.
for (const auto *frame : elem.frames()){
for (const auto* frame : elem.frames()) {
CheckPlantAggregate([&](auto i) { return &plant->get_frame(i); }, frame);
DRAKE_DEMAND(ContainsItem(elem.bodies(), &frame->body()));
DRAKE_DEMAND(ContainsItem(elem.model_instances(), frame->model_instance()));
}

// Check joints.
for (const auto *joint : elem.joints()){
for (const auto* joint : elem.joints()) {
CheckPlantAggregate([&](auto i) { return &plant->get_joint(i); }, joint);
IsJointSolelyConnectedTo(joint, elem.bodies());
DRAKE_DEMAND(ContainsItem(elem.model_instances(), joint->model_instance()));
}

// Check actuators.
for (const auto *joint_actuator : elem.joint_actuators()){
for (const auto* joint_actuator : elem.joint_actuators()) {
CheckPlantAggregate([&](auto i) { return &plant->get_joint_actuator(i); },
joint_actuator);
DRAKE_DEMAND(ContainsItem(elem.joints(), &joint_actuator->joint()));
DRAKE_DEMAND(
ContainsItem(elem.model_instances(), joint_actuator->model_instance()));
}

// # Check geometries.
// if scene_graph is not None:
// assert plant.geometry_source_is_registered()
// inspector = scene_graph.model_inspector()
// for geometry_id in elem.geometry_ids:
// assert isinstance(geometry_id, GeometryId)
// frame_id = inspector.GetFrameId(geometry_id)
// body = plant.GetBodyFromFrameId(frame_id)
// assert body in elem.bodies
// else:
// assert elem.geometry_ids == set(), elem.geometry_ids
// Check geometries.
if (elem.scene_graph() != nullptr) {
DRAKE_DEMAND(plant->geometry_source_is_registered());
const auto &inspector = elem.scene_graph()->model_inspector();
for (const auto geometry_id: elem.geometry_ids()){
for (const auto geometry_id : elem.geometry_ids()) {
auto frame_id = inspector.GetFrameId(geometry_id);
auto body = plant->GetBodyFromFrameId(frame_id);
DRAKE_DEMAND(ContainsItem(elem.bodies(), body));
Expand Down
1 change: 0 additions & 1 deletion multibody/parsing/detail_multibody_plant_subgraph.h
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,6 @@ class MultibodyPlantElements {
std::set<const Joint<double>*> joints_;
std::set<const JointActuator<double>*> joint_actuators_;
std::set<geometry::GeometryId> geometry_ids_;

};

using FrameNameRemapFunction = std::function<std::string(
Expand Down
7 changes: 4 additions & 3 deletions multibody/parsing/detail_sdf_parser.cc
Original file line number Diff line number Diff line change
Expand Up @@ -1557,9 +1557,10 @@ std::optional<ModelInstanceIndex> AddModelFromSdf(
model_name_in.empty() ? model.Name() : model_name_in;

std::vector<ModelInstanceIndex> added_model_instances =
AddModelsFromSpecification(
workspace.diagnostic, model, model_name, {}, workspace.plant,
workspace.package_map, data_source.GetRootDir(), reusable_model_instances);
AddModelsFromSpecification(workspace.diagnostic, model, model_name, {},
workspace.plant, workspace.package_map,
data_source.GetRootDir(),
reusable_model_instances);

DRAKE_DEMAND(!added_model_instances.empty());
return added_model_instances.front();
Expand Down
10 changes: 5 additions & 5 deletions multibody/parsing/test/detail_sdf_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -2492,12 +2492,12 @@ void TestMergeIncludeWithInterfaceAPI(const MultibodyPlant<double>& plant,
// Check that geometries have been created properly.
const auto frame_id = plant.GetBodyFrameIdOrThrow(
plant.GetBodyByName("L1", arm_urdf_model_instance).index());
EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception, scene_graph,
geometry::Box{0.1, 0.1, 0.1},
sdf::JoinName(model_prefix, "arm_urdf")));
EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kPerception,
scene_graph, geometry::Box{0.1, 0.1, 0.1},
sdf::JoinName(model_prefix, "arm_urdf")));
EXPECT_TRUE(FrameHasShape(frame_id, geometry::Role::kProximity, scene_graph,
geometry::Sphere{0.2},
sdf::JoinName(model_prefix, "arm_urdf")));
geometry::Sphere{0.2},
sdf::JoinName(model_prefix, "arm_urdf")));
}
{
// Frame F represents the model frame of model top::arm_sdf::flange
Expand Down
3 changes: 1 addition & 2 deletions multibody/plant/multibody_plant.h
Original file line number Diff line number Diff line change
Expand Up @@ -4483,12 +4483,11 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
// 2. RegisterAsSourceForSceneGraph() was called on `this` plant.
// 3. `scene_graph` points to the same SceneGraph instance previously
// passed to RegisterAsSourceForSceneGraph().
public: geometry::GeometryId RegisterGeometry(
geometry::GeometryId RegisterGeometry(
const Body<T>& body, const math::RigidTransform<double>& X_BG,
const geometry::Shape& shape,
const std::string& name);

private:
// Registers a geometry frame for every body. If the body already has a
// geometry frame, it is unchanged. This registration is part of finalization.
// This requires RegisterAsSourceForSceneGraph() was called on `this` plant.
Expand Down

0 comments on commit 7009139

Please sign in to comment.