diff --git a/README.md b/README.md index 9cd9775896a6..d66937f3a6e7 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ -# Drake +# HACK Drake -Model-Based Design and Verification for Robotics. +Hacking to test out an API. -Please see the [Drake Documentation](https://drake.mit.edu) for more -information. +~Model-Based Design and Verification for Robotics.~ + +~Please see the [Drake Documentation](https://drake.mit.edu) for more +information.~ diff --git a/WORKSPACE b/WORKSPACE index 2dfc87d14b86..2c9f5143406f 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -1,5 +1,7 @@ # -*- python -*- +BREAK_CI_SO_WE_DONT_WASTE_RESOURCES + # This file marks a workspace root for the Bazel build system. see # https://bazel.build/ . diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index e3a776441b05..664fe38058ca 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -416,15 +416,202 @@ void AddJointFromSpecification( joint_types->insert(joint_spec.Type()); } +std::set GetModelInstanceSet( + const MultibodyPlant& plant) { + std::set out; + for (int i = 0; i < plant.num_model_instances(): ++i) { + out.add(ModelInstanceIndex(i)); + } + return out; +} + +std::range GetModelLinks( + const MultibodyPlant& plant, ModelInstanceIndex model) { + ... +} + +std::range GetModelFrames( + const MultibodyPlant& plant, ModelInstanceIndex model) { + ... +} + +std::pair SplitName(std::string full) { + const std::string delim = "::"; // get from libsdformat? + auto pos = full.rfind(delim); + std::string first = full.substr(0, pos); + std::string second = full.substr(pos + delim.size()); + return {first, second}; +} + +RigidTransformd GetDefaultFramePose( + const MultibodyPlant& plant, + const Frame& frame) { + const RigidTransformd X_WB = + plant->GetDefaultFreeBodyPose(frame.body()); + const RigidTransformd X_WF = + X_WB * frame.GetFixedPoseInBodyFrame(); + return X_WF; +} + +// For the libsdformat API, see: +// http://sdformat.org/tutorials?tut=composition_proposal&cat=pose_semantics_docs&branch=feature-composition-interface-api&repo=https%3A%2F%2Fgithub.com%2FEricCousineau-TRI%2Fsdf_tutorials-1#1-5-minimal-libsdformat-interface-types-for-non-sdformat-models + +constexpr char kExtUrdf[] = ".urdf"; +// To test re-parsing an SDFormat document, but in complete isolation. Tests +// out separate model formats. +constexpr char kExtForcedNesting[] = ".forced_nesting_sdf"; + +// This assumes that parent models will have their parsing start before child +// models! +sdf::InterfaceModelPtr ParseNestedInterfaceModel( + MultibodyPlant* plant, + sdf::NestedInclude include, + sdf::Error& errors) { + // Do not attempt to parse anything other than URDF or forced nesting files. + const bool is_urdf = EndsWith(include.resolved_file_name, kExtUrdf); + const bool is_forced_nesting = + EndsWith(include.resolved_file_name, kExtForcedNesting); + if (!is_urdf && !is_forced_nesting) { + return nullptr; + } + + if (include.is_static) { + throw std::runtime_error( + "Drake does not yet support //include/static for custom nesting."); + } + + // WARNING: Most of these hacks (remembering model instances, getting silly + // name hierarchy) come about since MultibodyPlant does not have any + // mechanism for easy composition by itself (e.g. copying subgraphs, or + // getting a subgraph, etc.). + // If this ever happens (see #12703), this logic will be *greatly* + // simplified. + + std::vector new_model_instances; + if (is_urdf) { + // TOOD(eric.cousineau): Handle URDF and SetDefaultFreeBodyPose() (#13692) + // N.B. Errors will just happen via thrown exceptions. + new_model_instances = { + AddModelFromUrdfFile( + plant, include.file_path, include.absolute_model_name)}; + // Add explicit model frame to first link. + auto& canonical_link = GetModelLinks(plant, new_model_instances[0]).at(0); + plant.AddFrame(FixedOffsetFrame("__model__", canonical_link)); + } else { + // N.B. Errors will just happen via thrown exceptions. + DRAKE_DEMAND(is_forced_nesting); + new_model_instances = AddModelsFromSdfFile( + plant, include.file_path, include.absolute_model_name); + } + DRAKE_DEMAND(new_model_instances.size() > 0); + const ModelInstanceIndex main_model_instance = new_model_instances[0]; + + // And because we don't have great composition, I need to go through and do + // hacks to override default initial poses. + // Process: + // * Remember parsed pose from top-level model's pose. + // * Measure delta between that and the overriding pose. + // * Apply delta to each individual body's default pose :( + RigidTransformd X_WMdesired = ToRigidTransform(include.pose_WM); + const Frame& main_model_frame = + plant->GetFrameByName("__model__", main_model_instance); + + // This will be populated for the first model instance. + sdf::InterfaceModelPtr main_interface_model; + // Record by name to remember local hierarchy. + // Related this comment: + // https://github.com/RobotLocomotion/drake/issues/12270#issuecomment-606757766 + std::map interface_model_hierarchy; + + // Converts an MBP Body to sdf::InterfaceLink. + auto body_to_interface_link = [&](const Body& link) { + const RigidTransformd X_WM = GetDefaultFramePose(*plant, main_model_frame); + RigidTransformd X_ML = + X_WM.inverse() * plant->GetDefaultFreeBodyPose(link); + return make_shared(link->name(), ToPose3d(X_ML)); + }; + + // Converts an MBP Frame to sdf::InterfaceFrame. + auto frame_to_interface_frame = [&](const Frame& frame) { + const std::string body_name = frame->body().name(); + const std::string attached_to = body_name; + sdf::InterfacePose pose(frame->GetFixedPoseInBodyFrame(), body_name); + if (name == "" || name == body_name) { + // Do not register un-named frames or body frames. + return nullptr; + } + return make_shared(name, body_name, ToPose3d(X_BF)); + }; + + // N.B. For hierarchy, this assumes that "parent" models are defined before + // their "child" models. + for (auto model_instance : new_model_instances) { + sdf::RepostureFunction reposture_model = [plant, model_instance]( + sdf::InterfaceModelPtr interface_model, + sdf::NestedModelFrameGraphPtr graph) { + // N.B. This should also posture the model appropriately. + for (auto interface_link : interface_model.GetLinks()) { + const auto& body = + plant->GetBodyByName(interface_link.GetName(), model_instance); + RigidTransformd X_WL = + graph.ResolveNestedFramePose(interface_link.GetName()); + plant->SetFreeBodyPose(); + } + }; + + const std::string absolute_name = + plant->GetModelInstanceName(model_instance); + const auto [absolute_parent_name, local_name] = SplitName(absolute_name); + + sdf::InterfaceModelPtr interface_model = + std::make_shared( + local_name, + reposture_model, + model_frame.body().name(), + model_frame.GetFixedPoseInBodyFrame()); + + // Record all frames. + for (auto* link : GetModelLinks(plant, model_instance)) { + interface_model->AddLink(body_to_interface_link(*body)); + } + for (auto* frame : GetModelFrames(plant, model_instance)) { + if (frame.name() == "__model__") { + // Already added. + continue; + } + interface_model->AddFrame(frame_to_interface_frame(*frame)); + } + + if (!main_interface_model) { + main_interface_model = interface_model; + interface_model_hierarchy[absolute_name] = main_interface_model; + } else { + // Register with its parent model. + sdf::InterfaceModelPtr parent_interface_model = + interface_model_hierarchy.at(absolute_parent_name); + parent_interface_model->AddNestedModel(interface_model); + } + } + + return main_interface_model; +} + // Helper method to load an SDF file and read the contents into an sdf::Root // object. std::string LoadSdf( sdf::Root* root, - const std::string& file_name) { + const std::string& file_name, + MultibodyPlant* plant) { const std::string full_path = GetFullPath(file_name); - // Load the SDF file. + // WARNING: This will "invalidate" a plant if there is attempted recovery + // after an exception is thrown. However, this is probably the case for + // normal URDF and SDFormat parsing. + root->registerCustomModelParser( + [plant](sdf::NestedInclude include, sdf::Errors& errors) { + return ParseNestedInterfaceModel(plant, include, errors); + }); ThrowAnyErrors(root->Load(full_path)); // Uses the directory holding the SDF to be the root directory @@ -486,6 +673,8 @@ std::vector AddLinksFromSpecification( plant->SetDefaultFreeBodyPose(body, X_WL); if (plant->geometry_source_is_registered()) { + // TODO(eric.cousineau): Make this the primary thing to pass around. Do + // not pass around (package_map, root_dir). ResolveFilename resolve_filename = [&package_map, &root_dir](std::string uri) { const std::string resolved_name = @@ -632,6 +821,15 @@ bool AreWelded( return false; } +void AddNestedModelsFromSpecification( + auto model, model_name, auto plant, X_WM, package_map, root_dir) { + Parser parser(plant); + for (sdf::Model nested_model : model.GetParsedNestedModels()) { + // libsdformat parsed the model fully. Add the new elements directly. + AddModelFromSpecification(nested_model.AsSdfModel(), ...); + } +} + // Helper method to add a model to a MultibodyPlant given an sdf::Model // specification object. ModelInstanceIndex AddModelFromSpecification( @@ -649,6 +847,11 @@ ModelInstanceIndex AddModelFromSpecification( ThrowIfPoseFrameSpecified(model.Element()); const RigidTransformd X_WM = ToRigidTransform(model.RawPose()); + // First, add nested models, since they are encapsulated, so that the MBP + // frames can be referenced explicitly. + AddNestedModelsFromSpecification( + model, model_name, plant, X_WM, package_map, root_dir); + drake::log()->trace("sdf_parser: Add links"); std::vector added_link_infos = AddLinksFromSpecification( model_instance, model, X_WM, plant, package_map, root_dir); @@ -705,6 +908,9 @@ ModelInstanceIndex AddModelFromSpecification( } if (model.Static()) { + // NOTE: This should work for nested models, as SDFormat should ensure that + // any nested model's are also defined as static. + // Only weld / fixed joints are permissible. // TODO(eric.cousineau): Consider "freezing" non-weld joints, as is // permissible in Bullet and DART via Gazebo (#12227). @@ -742,7 +948,11 @@ ModelInstanceIndex AddModelFromSdfFile( sdf::Root root; - std::string root_dir = LoadSdf(&root, file_name); + if (scene_graph != nullptr && !plant->geometry_source_is_registered()) { + plant->RegisterAsSourceForSceneGraph(scene_graph); + } + + std::string root_dir = LoadSdf(&root, file_name, plant); if (root.ModelCount() != 1) { throw std::runtime_error("File must have a single element."); @@ -751,10 +961,6 @@ ModelInstanceIndex AddModelFromSdfFile( // Get the only model in the file. const sdf::Model& model = *root.ModelByIndex(0); - if (scene_graph != nullptr && !plant->geometry_source_is_registered()) { - plant->RegisterAsSourceForSceneGraph(scene_graph); - } - const std::string model_name = model_name_in.empty() ? model.Name() : model_name_in; @@ -772,7 +978,7 @@ std::vector AddModelsFromSdfFile( sdf::Root root; - std::string root_dir = LoadSdf(&root, file_name); + std::string root_dir = LoadSdf(&root, file_name, plant); // Throw an error if there are no models or worlds. if (root.ModelCount() == 0 && root.WorldCount() == 0) { diff --git a/multibody/parsing/test/nested_models/psuedo_file.sdf b/multibody/parsing/test/nested_models/psuedo_file.sdf new file mode 100644 index 000000000000..dcad5a994db8 --- /dev/null +++ b/multibody/parsing/test/nested_models/psuedo_file.sdf @@ -0,0 +1,73 @@ + + + + + + + + ... + + + + X_TArm + file://.../arm.forced_nesting_sdf + + + + torso + arm::mount + + + + X_ArmExtra + file://.../extra_arm.urdf + + + + arm::L1 + extra_arm::L2 + + + + + + + + + + + + + + + + + + + + + + {X_L1J1} + L1 + L2 + + + + {X_L2Gm} + + + + file://.../gripper.sdf + + + + + + + + + + {X_GGrasp} + + +